From bbec96810fb125760ed40436e05008c8fd6c91eb Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 00:56:49 +0100 Subject: [PATCH] Set up basic rover flow, individual functions to be completed --- Control/src/main.cpp | 309 ++++++++++++++++++++++++++++++++++--------- 1 file changed, 247 insertions(+), 62 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 8ea0695..637e9d8 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -1,7 +1,7 @@ #pragma region Includes #include #include -// #include Software Serial not currently needed +#include #include #include #include @@ -12,6 +12,7 @@ #include #include "status.h" #include "instruction.h" +#include #pragma endregion #pragma region Enable extra debugging info for ESP32 @@ -21,27 +22,44 @@ #pragma endregion #pragma region Definitions eg pins -#define RX1pin 14 // Pin 10 on expansion board, UART1 -#define TX1pin 4 // Pin 11 on expansion board, UART1 +#define RX1pin 18 // Pin 6 on expansion board, UART1 +#define TX1pin 5 // Pin 7 on expansion board, UART1 +#define RX2pin 17 // Pin 8 on expansion board, UART2 +#define TX2pin 16 // Pin 9 on expansion board, UART2 +#define RX3pin 14 // Pin 10 on expansion board, UART3 +#define TX3pin 4 // Pin 11 on expansion board, UART3 #pragma endregion #pragma region Function Declarations -void printFPGAoutput(); -void returnSensorData(); void notFound(AsyncWebServerRequest *request); void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length); +void queueInstruction(RoverInstruction instruction); +void sendToCommand(); +void sendToDrive(RoverInstruction instruction); +void recvFromDrive(); +void sendToEnergy(RoverInstruction instruction); +void recvFromEnergy(); +void sendToVision(); +void recvFromVision(); #pragma endregion #pragma region Global objects AsyncWebServer webserver(80); WebSocketsServer websocketserver(81); Ticker ticker; +SoftwareSerial Serial3; +std::queue InstrQueue; #pragma endregion #pragma region Global variables ControlStatus_t Status; -float battery_voltage = 4.0f; -int distance_travelled = 0; +float batteryVoltage; +int batteryLevel; +int odometer; +int heading; +int xpos, ypos; +int signalStrength; +int lastCompletedCommand; #pragma endregion void setup() @@ -50,10 +68,21 @@ void setup() esp_log_level_set("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client + Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) + Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive) + Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy) + Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision) + + // Set global variable startup values Status = CS_IDLE; - Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) - Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 - // Set up remaining communication ports here (Energy, Drive, Vision) + batteryVoltage = 0; + batteryLevel = 0; + odometer = 0; + heading = 0; + xpos = 0; + ypos = 0; + signalStrength = 0; + lastCompletedCommand = 0; if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -63,11 +92,11 @@ void setup() Serial.println("SPIFFS mounted"); WiFi.begin(WIFI_SSID, WIFI_PW); - while (WiFi.status() != WL_CONNECTED) + while (WiFi.status() != WL_CONNECTED) // Wait for ESP32 to connect to AP in "credentials.h" { delay(500); } - while (!MDNS.begin("rover")) + while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/" { Serial.println("Error setting up mDNS, retrying in 5s"); delay(5000); @@ -75,56 +104,95 @@ void setup() Serial.println("mDNS set up, access Control Panel at 'rover.local/'"); webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request) - { request->send(SPIFFS, "/index.html", "text/html"); }); + { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request) - { request->send(SPIFFS, "/favicon.ico", "image/png"); }); - webserver.onNotFound(notFound); - webserver.begin(); + { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon + webserver.onNotFound(notFound); // Set up basic 404NotFound page + webserver.begin(); // Start Asynchronous Web Server - websocketserver.begin(); - websocketserver.onEvent(webSocketEvent); - ticker.attach(0.5, returnSensorData); + websocketserver.begin(); // Start Websocket Server + websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command + ticker.attach(0.5, sendToCommand); // Set up recurring function to forward rover status to Command } -void loop() +void loop() // TO DO { - printFPGAoutput(); - - String FPGAinput; // Forward serial monitor input to FPGA - if (Serial.available()) - { - FPGAinput = String(Serial.readStringUntil('\n')); - Serial1.println(FPGAinput); - } - websocketserver.loop(); // Handle incoming client connections -} - -void printFPGAoutput() -{ // Print serial communication from FPGA to serial monitor - String FPGAoutput; - if (Serial1.available()) + switch (Status) { - FPGAoutput = String(Serial1.readStringUntil('\n')); - Serial.println(FPGAoutput); - } -} - -void returnSensorData() -{ - // Collect sensor data here? - distance_travelled++; - if (battery_voltage < 6) + case CS_ERROR: { - battery_voltage += 0.2; + Serial.println("Rover in error state, rebooting..."); + exit(1); } - else + break; + case CS_IDLE: { - battery_voltage = 4; + if (InstrQueue.empty()) // If Rover idle and InstrQueue empty: + { + // TO DO: Collect all data (recvFrom) and + sendToCommand(); // Update command panel + // Maybe wait 1s? Possibly prevent from looping too fast + } + else + { + // Do the next command in the queue + RoverInstruction *instr = &InstrQueue.front(); + switch (instr->instr) + { + case INSTR_RESET: + { + odometer = 0; + xpos = 0; + ypos = 0; + } + break; + case INSTR_STOP: + { + while (1) + { + Serial.println("Emergency Stop should not get queued, hold and print"); + delay(1000); + } + } + break; + case INSTR_MOVE: + { + Status = CS_MOVING; + sendToDrive(*instr); + } + break; + case INSTR_CHARGE: + { + Status = CS_CHARGING; + sendToEnergy(*instr); + } + break; + default: + { + Serial.println("Unknown instruction type in queue, skipping..."); + } + break; + } + } + } + break; + case CS_MOVING: + { + // TO DO + } + break; + case CS_CHARGING: + { + // TO DO + } + break; + default: + { + Serial.println("Unknown rover state, exiting..."); + exit(1); + } } - String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}"; - Serial.println(JSON_Data); - websocketserver.broadcastTXT(JSON_Data); } void notFound(AsyncWebServerRequest *request) @@ -132,7 +200,7 @@ void notFound(AsyncWebServerRequest *request) request->send(404, "text/plain", "Page Not found. Check URI/IP address."); } -void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) // TO DO { switch (type) { @@ -147,13 +215,13 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) Serial.printf("Client[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload); } break; - case WStype_TEXT: + case WStype_TEXT: // MSG received from command panel { - Serial.printf("Client[%u] sent Text: %s\n", num, payload); - String command = String((char *)(payload)); + Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal + String command = String((char *)(payload)); // Convert received command to string type - DynamicJsonDocument doc(200); //creating an instance of a DynamicJsonDocument allocating 200bytes on the heap. - DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive. + DynamicJsonDocument rdoc(200); // Create instance of DynamicJsonDocument on heap, 200 Bytes + DeserializationError error = deserializeJson(rdoc, command); // Convert command string to JSONDocument and capture any errors if (error) { Serial.print("deserializeJson() failed: "); @@ -161,12 +229,62 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) return; } - int MVM_F_status = doc["MVM_F"]; - int MVM_L_status = doc["MVM_L"]; - int MVM_R_status = doc["MVM_R"]; - int MVM_B_status = doc["MVM_B"]; + RoverInstruction instr; + int mode = rdoc["mode"]; + switch (mode) + { + case -1: // Add to queue, reset x/y/odometer (telemetry data) + { + Serial.println("Reset telemetry command received"); + instr.id = rdoc["Cid"]; + instr.instr = INSTR_RESET; + // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] - Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>'); + /* Put reset command in commandFIFO */ + } + break; + case 0: // Stop immediately, clear command cache + { + Serial.println("Emergency stop command received"); + // instr.instr = INSTR_STOP; // Not needed as Emergency Stop is not queued + // Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] + + /* Clear commandFIFO */ + } + break; + case 1: // Normal movement command, added to end of command cache + { + Serial.println("Normal movement command received"); + instr.id = rdoc["Cid"]; + instr.instr = INSTR_MOVE; + instr.heading = rdoc["rH"]; + instr.distance = rdoc["rD"]; + instr.speed = rdoc["rS"]; + // Ignore rdoc["rC"] + + /* Put movement command in commandFIFO */ + } + break; + case 2: // Normal charge command, results in no motion, added to end of command cache + { + Serial.println("Normal charge command received"); + instr.id = rdoc["Cid"]; + instr.instr = INSTR_CHARGE; + instr.charge = rdoc["rC"]; + // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"] + + /* Put charge command in commandFIFO */ + } + break; + default: + { + Serial.println("Unknown Command type received, ignoring"); // Default case, print and continue + // Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] + } + break; + } + + queueInstruction(instr); } break; case WStype_PONG: @@ -179,5 +297,72 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) Serial.println(String("Websocket received invalid event type: ") + type + String(", exiting")); exit(1); } + break; } } + +void queueInstruction(RoverInstruction instruction) +{ + InstrQueue.push(instruction); +} + +void sendToCommand() +{ + DynamicJsonDocument tdoc(1024); + tdoc["st"] = Status; + tdoc["bV"] = batteryVoltage; + tdoc["bL"] = batteryLevel; + tdoc["tD"] = odometer; + tdoc["cH"] = heading; + tdoc["pos"][0] = xpos; + tdoc["pos"][1] = ypos; + tdoc["rssi"] = signalStrength; + tdoc["LCCid"] = lastCompletedCommand; + String JSON_Data; + serializeJson(tdoc, JSON_Data); + websocketserver.broadcastTXT(JSON_Data); +} + +void sendToDrive(RoverInstruction instruction) +{ + DynamicJsonDocument tdoc(1024); + tdoc["rH"] = instruction.heading; + tdoc["dist"] = instruction.distance; + tdoc["sp"] = instruction.speed; + tdoc["cH"] = heading; + serializeJson(tdoc, Serial1); +} + +void recvFromDrive() // TO DO +{ +} + +void sendToEnergy(RoverInstruction instruction) +{ + DynamicJsonDocument tdoc(1024); + tdoc["ch"] = instruction.charge; + serializeJson(tdoc, Serial2); +} + +void recvFromEnergy() // TO DO +{ +} + +void sendToVision() +{ + Serial3.print("R"); // Request new data from Vision +} + +void recvFromVision() // TO DO +{ +} + +void emergencyStop() // TO DO +{ + // Send stop signals to drive, energy + while (InstrQueue.size()) + { + InstrQueue.pop(); // Clear Instruction Queue + } + Serial.println("Instruction Queue cleared"); +} \ No newline at end of file