supleed2/ELEC50003-P1-CW
1
0
Fork 0
mirror of https://github.com/supleed2/ELEC50003-P1-CW.git synced 2024-12-22 21:45:49 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add wait function to rover

Browse source 
Instruction type and handling within switch statements
This commit is contained in:
Aadi Desai 2021-06-13 21:43:43 +01:00
parent b6cbfc7619
commit 39a8d232a7
3 changed files with 39 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
Control/include/instruction.h
View file

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

3
Control/include/status.h
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

38
Control/src/main.cpp
View file

@ -66,6 +66,7 @@ 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;
#pragma endregion #pragma endregion
void setup() void setup()
@ -93,6 +94,7 @@ void setup()
lastCompletedCommand = 0; lastCompletedCommand = 0;
driveCommandComplete = 1; driveCommandComplete = 1;
chargeGoal = 0; chargeGoal = 0;
waitGoal = 0;
if (!SPIFFS.begin(true)) // Mount SPIFFS if (!SPIFFS.begin(true)) // Mount SPIFFS
{ {
@ -180,6 +182,12 @@ 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;
default: default:
{ {
Serial.println("Unknown instruction type in queue, skipping..."); Serial.println("Unknown instruction type in queue, skipping...");
@ -215,7 +223,16 @@ void loop()
sendToEnergy(0); // Stop charging if goal reached sendToEnergy(0); // Stop charging if goal reached
} }
// Otherwise continue charging, no change // 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; break;
default: 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"); 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
} }
@ -280,7 +297,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();
} }
@ -293,7 +310,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
} }
@ -304,7 +321,18 @@ 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 queueInstruction(instr); // Put charge command in InstrQueue
} }