From b6cbfc7619d9aef35fb6d8fab19b1c815c5fd441 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 21:24:38 +0100 Subject: [PATCH 1/7] Add ability to pause rover for a defined time to reference doc --- Control/ref/command.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index 3a014df..570c8aa 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -5,7 +5,7 @@ #define WebSocket 0 int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command float batteryVoltage, batteryLevel, batteryCycles; // Info Control ==> Command -int command_id, mode, reqHeading, reqDistance, reqCharge; // Info Command ==> Control +int command_id, mode, reqHeading, reqDistance, reqCharge, reqTime; // Info Command ==> Control float reqSpeed; // Info Command ==> Control void setup() {} @@ -36,9 +36,11 @@ void loop() // 0 = Stop immediately, clear command cache // 1 = Normal movement command, added to end of command cache // 2 = Normal charge command, results in no motion, added to end of command cache + // 3 = Pause command, wait for defined time in seconds, added to end of command cache tdoc["rH"] = reqHeading; tdoc["rD"] = reqDistance; tdoc["rS"] = reqSpeed; tdoc["rC"] = reqCharge; + tdoc["pSt"] = reqTime; serializeJson(tdoc, WebSocket, WebSocket); // Build JSON and send on UART1 } From 39a8d232a7fdb1bd75193596057b0c23e688a343 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 21:43:43 +0100 Subject: [PATCH 2/7] Add wait function to rover Instruction type and handling within switch statements --- Control/include/instruction.h | 4 +++- Control/include/status.h | 3 ++- Control/src/main.cpp | 40 +++++++++++++++++++++++++++++------ 3 files changed, 39 insertions(+), 8 deletions(-) diff --git a/Control/include/instruction.h b/Control/include/instruction.h index 574d542..1a2d32d 100644 --- a/Control/include/instruction.h +++ b/Control/include/instruction.h @@ -5,7 +5,8 @@ typedef enum { INSTR_RESET = -1, INSTR_STOP, INSTR_MOVE, - INSTR_CHARGE + INSTR_CHARGE, + INSTR_WAIT } instr_t; typedef struct instruction @@ -16,6 +17,7 @@ typedef struct instruction int distance; float speed; int charge; + int time; } RoverInstruction; #endif diff --git a/Control/include/status.h b/Control/include/status.h index 4b8b2d9..46830d7 100644 --- a/Control/include/status.h +++ b/Control/include/status.h @@ -5,7 +5,8 @@ typedef enum { CS_ERROR = -1, CS_IDLE, CS_MOVING, - CS_CHARGING + CS_CHARGING, + CS_WAITING } ControlStatus_t; #endif diff --git a/Control/src/main.cpp b/Control/src/main.cpp index ead562f..c86e7f7 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -66,6 +66,7 @@ bool driveCommandComplete; int bb_left, bb_right, bb_top, bb_bottom; int bb_centre_x, bb_centre_y; float chargeGoal; +int waitGoal; #pragma endregion void setup() @@ -93,6 +94,7 @@ void setup() lastCompletedCommand = 0; driveCommandComplete = 1; chargeGoal = 0; + waitGoal = 0; if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -180,6 +182,12 @@ void loop() sendToEnergy(1); // Forward to Energy handler } break; + case INSTR_WAIT: // Normal wait + { + Status = CS_WAITING; // Set waiting state + waitGoal = millis() + 1000*(instr->time); // Set wait time + } + break; default: { Serial.println("Unknown instruction type in queue, skipping..."); @@ -212,10 +220,19 @@ void loop() { Status = CS_IDLE; lastCompletedCommand = lastExecutedCommand; // Update last completed command - sendToEnergy(0); // Stop charging if goal reached + sendToEnergy(0); // Stop charging if goal reached } // Otherwise continue charging, no change - + } + break; + case CS_WAITING: + { + if (millis() >= waitGoal) // Compare waitGoal to current time + { + Status = CS_IDLE; + lastCompletedCommand = lastExecutedCommand; // Update last completed command + } + // Otherwise continue waiting, no change } break; default: @@ -271,7 +288,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) Serial.println("Reset telemetry command received"); instr.id = rdoc["Cid"]; instr.instr = INSTR_RESET; - // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] + // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"], rdoc["pSt"] queueInstruction(instr); // Put reset command in InstrQueue } @@ -280,7 +297,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) { 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"] + // Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"], rdoc["pSt"] emergencyStop(); } @@ -293,7 +310,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) instr.heading = rdoc["rH"]; instr.distance = rdoc["rD"]; instr.speed = rdoc["rS"]; - // Ignore rdoc["rC"] + // Ignore rdoc["rC"], rdoc["pSt"] queueInstruction(instr); // Put movement command in InstrQueue } @@ -304,7 +321,18 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) instr.id = rdoc["Cid"]; instr.instr = INSTR_CHARGE; instr.charge = rdoc["rC"]; - // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"] + // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["pSt"] + + queueInstruction(instr); // Put charge command in InstrQueue + } + break; + case 3: // Normal wait command, results in no motion, added to end of command cache + { + Serial.println("Normal wait command received"); + instr.id = rdoc["Cid"]; + instr.instr = INSTR_WAIT; + instr.time = rdoc["pSt"]; + // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] queueInstruction(instr); // Put charge command in InstrQueue } From cea4237b75dd90b9bbad4f18b015b9a1c21ae585 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 23:33:49 +0100 Subject: [PATCH 3/7] Add waiting state to reference doc --- Control/ref/command.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index 570c8aa..9360b9a 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -14,7 +14,7 @@ void loop() { DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket - state = rdoc["st"]; // State: -1 = Error, 0 = Idle, 1 = Moving, 2 = Charging + state = rdoc["st"]; // State: -1 = Error, 0 = Idle, 1 = Moving, 2 = Charging, 3 = Waiting batteryVoltage = rdoc["bV"]; batteryLevel = rdoc["bL"]; batteryCycles = rdoc["bC"]; From c98bafd6f77365c45ed09ed2be80429e851b53f9 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Tue, 15 Jun 2021 03:14:04 +0100 Subject: [PATCH 4/7] Add ability to select Vision colour tracking --- Control/include/colour.h | 12 ++++++++++++ Control/include/instruction.h | 4 +++- Control/src/main.cpp | 22 +++++++++++++++++++++- 3 files changed, 36 insertions(+), 2 deletions(-) create mode 100644 Control/include/colour.h diff --git a/Control/include/colour.h b/Control/include/colour.h new file mode 100644 index 0000000..6544242 --- /dev/null +++ b/Control/include/colour.h @@ -0,0 +1,12 @@ +#ifndef COLOUR_H +#define COLOUR_H + +typedef enum { + C_RED, + C_LBLUE, + C_GREEN, + C_PINK, + C_ORANGE +} Colour_t; + +#endif diff --git a/Control/include/instruction.h b/Control/include/instruction.h index 1a2d32d..eed1b92 100644 --- a/Control/include/instruction.h +++ b/Control/include/instruction.h @@ -6,7 +6,8 @@ typedef enum { INSTR_STOP, INSTR_MOVE, INSTR_CHARGE, - INSTR_WAIT + INSTR_WAIT, + INSTR_COLOUR } instr_t; typedef struct instruction @@ -18,6 +19,7 @@ typedef struct instruction float speed; int charge; int time; + Colour_t colour; } RoverInstruction; #endif diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 1ad0ac4..06a9a49 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -13,6 +13,7 @@ #include #include "status.h" #include "instruction.h" +#include "colour.h" #include #pragma endregion @@ -71,6 +72,7 @@ int bb_left, bb_right, bb_top, bb_bottom; int bb_centre_x, bb_centre_y; float chargeGoal; int waitGoal; +Colour_t colour; #pragma endregion void setup() @@ -100,6 +102,7 @@ void setup() driveCommandComplete = 1; chargeGoal = 0; waitGoal = 0; + colour = C_RED; if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -196,6 +199,12 @@ void loop() waitGoal = millis() + 1000*(instr->time); // Set wait time } break; + case INSTR_COLOUR: + { + Status = CS_IDLE; + colour = instr->colour; + } + break; default: { Serial.println("Unknown instruction type in queue, skipping..."); @@ -346,6 +355,17 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) queueInstruction(instr); // Put charge command in InstrQueue } break; + case 4: // Normal wait command, results in no motion, added to end of command cache + { + Serial.println("Change colour tracking command received"); + instr.id = rdoc["Cid"]; + instr.instr = INSTR_COLOUR; + instr.colour = rdoc["col"]; + // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] + + queueInstruction(instr); // Put charge command in InstrQueue + } + break; default: { // Default case, print and continue @@ -437,7 +457,7 @@ void recvFromEnergy() // Update telemetry data and state info from Energy packet void sendToVision() { - Serial3.print("R"); // Request new data from Vision + Serial3.print(colour); // Select coloured ball to track } void recvFromVision() // Update bounding box and obstacle detection data from Vision packet From 531a86c6d0aed610632ab1b75ac7923558f0a940 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Tue, 15 Jun 2021 08:52:33 +0100 Subject: [PATCH 5/7] Fix instruction struct Type declaration of colour variable --- Control/include/instruction.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Control/include/instruction.h b/Control/include/instruction.h index eed1b92..dc110bb 100644 --- a/Control/include/instruction.h +++ b/Control/include/instruction.h @@ -19,7 +19,7 @@ typedef struct instruction float speed; int charge; int time; - Colour_t colour; + int colour; } RoverInstruction; #endif From 8353103e1ff6183630f6722d9f5dfd2a2755e0a1 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Tue, 15 Jun 2021 08:55:42 +0100 Subject: [PATCH 6/7] Add colour selection to Command<->Control reference doc --- Control/ref/command.cpp | 4 +++- Control/src/main.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index 9360b9a..8556faf 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -5,7 +5,7 @@ #define WebSocket 0 int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command float batteryVoltage, batteryLevel, batteryCycles; // Info Control ==> Command -int command_id, mode, reqHeading, reqDistance, reqCharge, reqTime; // Info Command ==> Control +int command_id, mode, reqHeading, reqDistance, reqCharge, reqTime, reqColour; // Info Command ==> Control float reqSpeed; // Info Command ==> Control void setup() {} @@ -37,10 +37,12 @@ void loop() // 1 = Normal movement command, added to end of command cache // 2 = Normal charge command, results in no motion, added to end of command cache // 3 = Pause command, wait for defined time in seconds, added to end of command cache + // 4 = Change colour tracking using Vision tdoc["rH"] = reqHeading; tdoc["rD"] = reqDistance; tdoc["rS"] = reqSpeed; tdoc["rC"] = reqCharge; tdoc["pSt"] = reqTime; + tdoc["col"] = reqColour; serializeJson(tdoc, WebSocket, WebSocket); // Build JSON and send on UART1 } diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 06a9a49..9ab6ba8 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -202,7 +202,7 @@ void loop() case INSTR_COLOUR: { Status = CS_IDLE; - colour = instr->colour; + colour = (Colour_t)instr->colour; } break; default: From 6ca4be5e11f9788f6a5f2674881a7d9e2ff31ebb Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Tue, 15 Jun 2021 08:59:11 +0100 Subject: [PATCH 7/7] Add missing RSSI / Signal Strength tracking --- Control/src/main.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 9ab6ba8..658cc1c 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -27,7 +27,7 @@ #define RX1pin 17 // Pin 6 on expansion board, UART1 #define TX1pin 16 // Pin 7 on expansion board, UART1 #define RX2pin 18 // Pin 8 on expansion board, UART2 -#define TX2pin 5 // Pin 9 on expansion board, UART2 +#define TX2pin 5 // Pin 9 on expansion board, UART2 #define RX3pin 14 // Pin 10 on expansion board, UART3 #define TX3pin 4 // Pin 11 on expansion board, UART3 #define RX4pin 15 // Pin 12 on expansion board, UART4 @@ -46,6 +46,7 @@ void recvFromEnergy(); void sendToVision(); void recvFromVision(); void recvFromCompass(); +void updateRSSI(); void emergencyStop(); #pragma endregion @@ -81,10 +82,10 @@ 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) + 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) Serial4.begin(9600, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass) // Set global variable startup values @@ -144,6 +145,7 @@ void loop() recvFromEnergy(); // Update stats from Energy // recvFromVision(); // Update stats from Vision recvFromCompass(); // Update stats from Compass + updateRSSI(); switch (Status) { case CS_ERROR: @@ -195,8 +197,8 @@ void loop() break; case INSTR_WAIT: // Normal wait { - Status = CS_WAITING; // Set waiting state - waitGoal = millis() + 1000*(instr->time); // Set wait time + Status = CS_WAITING; // Set waiting state + waitGoal = millis() + 1000 * (instr->time); // Set wait time } break; case INSTR_COLOUR: @@ -486,6 +488,11 @@ void recvFromCompass() } } +void updateRSSI() +{ + signalStrength = WiFi.RSSI(); +} + void emergencyStop() { DynamicJsonDocument tdoc(1024);