Set up basic rover flow, individual functions to be completed

This commit is contained in:
Aadi Desai 2021-06-13 00:56:49 +01:00
parent 5e6254029a
commit bbec96810f

View file

@ -1,7 +1,7 @@
#pragma region Includes #pragma region Includes
#include <Arduino.h> #include <Arduino.h>
#include <string> #include <string>
// #include <SoftwareSerial.h> Software Serial not currently needed #include <SoftwareSerial.h>
#include <AsyncTCP.h> #include <AsyncTCP.h>
#include <ESPAsyncWebServer.h> #include <ESPAsyncWebServer.h>
#include <ESPmDNS.h> #include <ESPmDNS.h>
@ -12,6 +12,7 @@
#include <SPIFFS.h> #include <SPIFFS.h>
#include "status.h" #include "status.h"
#include "instruction.h" #include "instruction.h"
#include <queue>
#pragma endregion #pragma endregion
#pragma region Enable extra debugging info for ESP32 #pragma region Enable extra debugging info for ESP32
@ -21,27 +22,44 @@
#pragma endregion #pragma endregion
#pragma region Definitions eg pins #pragma region Definitions eg pins
#define RX1pin 14 // Pin 10 on expansion board, UART1 #define RX1pin 18 // Pin 6 on expansion board, UART1
#define TX1pin 4 // Pin 11 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 endregion
#pragma region Function Declarations #pragma region Function Declarations
void printFPGAoutput();
void returnSensorData();
void notFound(AsyncWebServerRequest *request); void notFound(AsyncWebServerRequest *request);
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);
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 endregion
#pragma region Global objects #pragma region Global objects
AsyncWebServer webserver(80); AsyncWebServer webserver(80);
WebSocketsServer websocketserver(81); WebSocketsServer websocketserver(81);
Ticker ticker; Ticker ticker;
SoftwareSerial Serial3;
std::queue<RoverInstruction> InstrQueue;
#pragma endregion #pragma endregion
#pragma region Global variables #pragma region Global variables
ControlStatus_t Status; ControlStatus_t Status;
float battery_voltage = 4.0f; float batteryVoltage;
int distance_travelled = 0; int batteryLevel;
int odometer;
int heading;
int xpos, ypos;
int signalStrength;
int lastCompletedCommand;
#pragma endregion #pragma endregion
void setup() 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("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
Status = CS_IDLE;
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 Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive)
// Set up remaining communication ports here (Energy, Drive, Vision) 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;
batteryVoltage = 0;
batteryLevel = 0;
odometer = 0;
heading = 0;
xpos = 0;
ypos = 0;
signalStrength = 0;
lastCompletedCommand = 0;
if (!SPIFFS.begin(true)) // Mount SPIFFS if (!SPIFFS.begin(true)) // Mount SPIFFS
{ {
@ -63,11 +92,11 @@ void setup()
Serial.println("SPIFFS mounted"); Serial.println("SPIFFS mounted");
WiFi.begin(WIFI_SSID, WIFI_PW); 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); 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"); Serial.println("Error setting up mDNS, retrying in 5s");
delay(5000); delay(5000);
@ -75,56 +104,95 @@ void setup()
Serial.println("mDNS set up, access Control Panel at 'rover.local/'"); Serial.println("mDNS set up, access Control Panel at 'rover.local/'");
webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request) 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) webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
{ request->send(SPIFFS, "/favicon.ico", "image/png"); }); { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
webserver.onNotFound(notFound); webserver.onNotFound(notFound); // Set up basic 404NotFound page
webserver.begin(); webserver.begin(); // Start Asynchronous Web Server
websocketserver.begin(); websocketserver.begin(); // Start Websocket Server
websocketserver.onEvent(webSocketEvent); websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command
ticker.attach(0.5, returnSensorData); 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 websocketserver.loop(); // Handle incoming client connections
} switch (Status)
void printFPGAoutput()
{ // Print serial communication from FPGA to serial monitor
String FPGAoutput;
if (Serial1.available())
{ {
FPGAoutput = String(Serial1.readStringUntil('\n')); case CS_ERROR:
Serial.println(FPGAoutput);
}
}
void returnSensorData()
{ {
// Collect sensor data here? Serial.println("Rover in error state, rebooting...");
distance_travelled++; exit(1);
if (battery_voltage < 6) }
break;
case CS_IDLE:
{ {
battery_voltage += 0.2; 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 else
{ {
battery_voltage = 4; // 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) void notFound(AsyncWebServerRequest *request)
@ -132,7 +200,7 @@ void notFound(AsyncWebServerRequest *request)
request->send(404, "text/plain", "Page Not found. Check URI/IP address."); 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) 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); Serial.printf("Client[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
} }
break; break;
case WStype_TEXT: case WStype_TEXT: // MSG received from command panel
{ {
Serial.printf("Client[%u] sent Text: %s\n", num, payload); Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal
String command = String((char *)(payload)); 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. DynamicJsonDocument rdoc(200); // Create instance of DynamicJsonDocument on heap, 200 Bytes
DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive. DeserializationError error = deserializeJson(rdoc, command); // Convert command string to JSONDocument and capture any errors
if (error) if (error)
{ {
Serial.print("deserializeJson() failed: "); Serial.print("deserializeJson() failed: ");
@ -161,12 +229,62 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
return; return;
} }
int MVM_F_status = doc["MVM_F"]; RoverInstruction instr;
int MVM_L_status = doc["MVM_L"]; int mode = rdoc["mode"];
int MVM_R_status = doc["MVM_R"]; switch (mode)
int MVM_B_status = doc["MVM_B"]; {
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; break;
case WStype_PONG: 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")); Serial.println(String("Websocket received invalid event type: ") + type + String(", exiting"));
exit(1); 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");
}