supleed2/ELEC50003-P1-CW
1
0
Fork 0
mirror of https://github.com/supleed2/ELEC50003-P1-CW.git synced 2024-09-19 20:16:16 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into Integration

Browse source
This commit is contained in:
Aadi Desai 2021-06-15 11:25:46 +01:00
parent ca662990b7 6ca4be5e11
commit a02bb51d7e
5 changed files with 91 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

6
Control/include/instruction.h
View file

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

3
Control/include/status.h
View file

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

8
Control/ref/command.cpp
View file

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

76
Control/src/main.cpp
View file

@ -13,6 +13,7 @@
#include <SPIFFS.h>
#include "status.h"
#include "instruction.h"
#include "colour.h"
#include <queue>
#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);