Merge branch 'main' into Integration

This commit is contained in:
Aadi Desai 2021-06-15 11:25:46 +01:00
commit a02bb51d7e
5 changed files with 91 additions and 14 deletions

12
Control/include/colour.h Normal file
View file

@ -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

View file

@ -5,7 +5,9 @@ typedef enum {
INSTR_RESET = -1, INSTR_RESET = -1,
INSTR_STOP, INSTR_STOP,
INSTR_MOVE, INSTR_MOVE,
INSTR_CHARGE INSTR_CHARGE,
INSTR_WAIT,
INSTR_COLOUR
} instr_t; } instr_t;
typedef struct instruction typedef struct instruction
@ -16,6 +18,8 @@ typedef struct instruction
int distance; int distance;
float speed; float speed;
int charge; int charge;
int time;
int colour;
} RoverInstruction; } RoverInstruction;
#endif #endif

View file

@ -5,7 +5,8 @@ typedef enum {
CS_ERROR = -1, CS_ERROR = -1,
CS_IDLE, CS_IDLE,
CS_MOVING, CS_MOVING,
CS_CHARGING CS_CHARGING,
CS_WAITING
} ControlStatus_t; } ControlStatus_t;
#endif #endif

View file

@ -5,7 +5,7 @@
#define WebSocket 0 #define WebSocket 0
int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command
float batteryVoltage, batteryLevel, batteryCycles; // 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 float reqSpeed; // Info Command ==> Control
void setup() {} void setup() {}
@ -14,7 +14,7 @@ void loop()
{ {
DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket 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"]; batteryVoltage = rdoc["bV"];
batteryLevel = rdoc["bL"]; batteryLevel = rdoc["bL"];
batteryCycles = rdoc["bC"]; batteryCycles = rdoc["bC"];
@ -36,9 +36,13 @@ void loop()
// 0 = Stop immediately, clear command cache // 0 = Stop immediately, clear command cache
// 1 = Normal movement command, added to end of 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 // 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["rH"] = reqHeading;
tdoc["rD"] = reqDistance; tdoc["rD"] = reqDistance;
tdoc["rS"] = reqSpeed; tdoc["rS"] = reqSpeed;
tdoc["rC"] = reqCharge; tdoc["rC"] = reqCharge;
tdoc["pSt"] = reqTime;
tdoc["col"] = reqColour;
serializeJson(tdoc, WebSocket, WebSocket); // Build JSON and send on UART1 serializeJson(tdoc, WebSocket, WebSocket); // Build JSON and send on UART1
} }

View file

@ -13,6 +13,7 @@
#include <SPIFFS.h> #include <SPIFFS.h>
#include "status.h" #include "status.h"
#include "instruction.h" #include "instruction.h"
#include "colour.h"
#include <queue> #include <queue>
#pragma endregion #pragma endregion
@ -26,7 +27,7 @@
#define RX1pin 17 // Pin 6 on expansion board, UART1 #define RX1pin 17 // Pin 6 on expansion board, UART1
#define TX1pin 16 // Pin 7 on expansion board, UART1 #define TX1pin 16 // Pin 7 on expansion board, UART1
#define RX2pin 18 // Pin 8 on expansion board, UART2 #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 RX3pin 14 // Pin 10 on expansion board, UART3
#define TX3pin 4 // Pin 11 on expansion board, UART3 #define TX3pin 4 // Pin 11 on expansion board, UART3
#define RX4pin 15 // Pin 12 on expansion board, UART4 #define RX4pin 15 // Pin 12 on expansion board, UART4
@ -45,6 +46,7 @@ void recvFromEnergy();
void sendToVision(); void sendToVision();
void recvFromVision(); void recvFromVision();
void recvFromCompass(); void recvFromCompass();
void updateRSSI();
void emergencyStop(); void emergencyStop();
#pragma endregion #pragma endregion
@ -70,6 +72,8 @@ bool driveCommandComplete;
int bb_left, bb_right, bb_top, bb_bottom; int bb_left, bb_right, bb_top, bb_bottom;
int bb_centre_x, bb_centre_y; int bb_centre_x, bb_centre_y;
float chargeGoal; float chargeGoal;
int waitGoal;
Colour_t colour;
#pragma endregion #pragma endregion
void setup() 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("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack
esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client 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) 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) 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) 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) 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) Serial4.begin(9600, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass)
// Set global variable startup values // Set global variable startup values
@ -98,6 +102,8 @@ void setup()
lastCompletedCommand = 0; lastCompletedCommand = 0;
driveCommandComplete = 1; driveCommandComplete = 1;
chargeGoal = 0; chargeGoal = 0;
waitGoal = 0;
colour = C_RED;
if (!SPIFFS.begin(true)) // Mount SPIFFS if (!SPIFFS.begin(true)) // Mount SPIFFS
{ {
@ -139,6 +145,7 @@ void loop()
recvFromEnergy(); // Update stats from Energy recvFromEnergy(); // Update stats from Energy
// recvFromVision(); // Update stats from Vision // recvFromVision(); // Update stats from Vision
recvFromCompass(); // Update stats from Compass recvFromCompass(); // Update stats from Compass
updateRSSI();
switch (Status) switch (Status)
{ {
case CS_ERROR: case CS_ERROR:
@ -188,6 +195,18 @@ void loop()
sendToEnergy(1); // Forward to Energy handler sendToEnergy(1); // Forward to Energy handler
} }
break; 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: default:
{ {
Serial.println("Unknown instruction type in queue, skipping..."); Serial.println("Unknown instruction type in queue, skipping...");
@ -226,6 +245,16 @@ void loop()
// Otherwise continue charging, no change // Otherwise continue charging, no change
} }
break; 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: default:
{ {
Serial.println("Unknown rover state, exiting..."); 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"); Serial.println("Reset telemetry command received");
instr.id = rdoc["Cid"]; instr.id = rdoc["Cid"];
instr.instr = INSTR_RESET; 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 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"); Serial.println("Emergency stop command received");
// instr.instr = INSTR_STOP; // Not needed as Emergency Stop is not queued // 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(); emergencyStop();
} }
@ -301,7 +330,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
instr.heading = rdoc["rH"]; instr.heading = rdoc["rH"];
instr.distance = rdoc["rD"]; instr.distance = rdoc["rD"];
instr.speed = rdoc["rS"]; instr.speed = rdoc["rS"];
// Ignore rdoc["rC"] // Ignore rdoc["rC"], rdoc["pSt"]
queueInstruction(instr); // Put movement command in InstrQueue 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.id = rdoc["Cid"];
instr.instr = INSTR_CHARGE; instr.instr = INSTR_CHARGE;
instr.charge = rdoc["rC"]; 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 queueInstruction(instr); // Put charge command in InstrQueue
} }
@ -408,7 +459,7 @@ void recvFromEnergy() // Update telemetry data and state info from Energy packet
void sendToVision() 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 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() void emergencyStop()
{ {
DynamicJsonDocument tdoc(1024); DynamicJsonDocument tdoc(1024);