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 574d542..dc110bb 100644 --- a/Control/include/instruction.h +++ b/Control/include/instruction.h @@ -5,7 +5,9 @@ typedef enum { INSTR_RESET = -1, INSTR_STOP, INSTR_MOVE, - INSTR_CHARGE + INSTR_CHARGE, + INSTR_WAIT, + INSTR_COLOUR } instr_t; typedef struct instruction @@ -16,6 +18,8 @@ typedef struct instruction int distance; float speed; int charge; + int time; + int colour; } 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/ref/command.cpp b/Control/ref/command.cpp index 3a014df..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; // Info Command ==> Control +int command_id, mode, reqHeading, reqDistance, reqCharge, reqTime, reqColour; // Info Command ==> Control float reqSpeed; // Info Command ==> Control void setup() {} @@ -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"]; @@ -36,9 +36,13 @@ 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 + // 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 ff2cb42..658cc1c 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 @@ -26,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 @@ -45,6 +46,7 @@ void recvFromEnergy(); void sendToVision(); void recvFromVision(); void recvFromCompass(); +void updateRSSI(); void emergencyStop(); #pragma endregion @@ -70,6 +72,8 @@ bool driveCommandComplete; 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() @@ -78,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 @@ -98,6 +102,8 @@ void setup() lastCompletedCommand = 0; driveCommandComplete = 1; chargeGoal = 0; + waitGoal = 0; + colour = C_RED; if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -139,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: @@ -188,6 +195,18 @@ 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; + case INSTR_COLOUR: + { + Status = CS_IDLE; + colour = (Colour_t)instr->colour; + } + break; default: { Serial.println("Unknown instruction type in queue, skipping..."); @@ -226,6 +245,16 @@ void loop() // 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: { Serial.println("Unknown rover state, exiting..."); @@ -279,7 +308,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 } @@ -288,7 +317,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(); } @@ -301,7 +330,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 } @@ -312,7 +341,29 @@ 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 + } + 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 } @@ -408,7 +459,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 @@ -437,6 +488,11 @@ void recvFromCompass() } } +void updateRSSI() +{ + signalStrength = WiFi.RSSI(); +} + void emergencyStop() { DynamicJsonDocument tdoc(1024);