Add ability to select Vision colour tracking

This commit is contained in:
Aadi Desai 2021-06-15 03:14:04 +01:00
parent bbe36fb9dd
commit c98bafd6f7
3 changed files with 36 additions and 2 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

@ -6,7 +6,8 @@ typedef enum {
INSTR_STOP, INSTR_STOP,
INSTR_MOVE, INSTR_MOVE,
INSTR_CHARGE, INSTR_CHARGE,
INSTR_WAIT INSTR_WAIT,
INSTR_COLOUR
} instr_t; } instr_t;
typedef struct instruction typedef struct instruction
@ -18,6 +19,7 @@ typedef struct instruction
float speed; float speed;
int charge; int charge;
int time; int time;
Colour_t colour;
} RoverInstruction; } RoverInstruction;
#endif #endif

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
@ -71,6 +72,7 @@ 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; int waitGoal;
Colour_t colour;
#pragma endregion #pragma endregion
void setup() void setup()
@ -100,6 +102,7 @@ void setup()
driveCommandComplete = 1; driveCommandComplete = 1;
chargeGoal = 0; chargeGoal = 0;
waitGoal = 0; waitGoal = 0;
colour = C_RED;
if (!SPIFFS.begin(true)) // Mount SPIFFS if (!SPIFFS.begin(true)) // Mount SPIFFS
{ {
@ -196,6 +199,12 @@ void loop()
waitGoal = millis() + 1000*(instr->time); // Set wait time waitGoal = millis() + 1000*(instr->time); // Set wait time
} }
break; break;
case INSTR_COLOUR:
{
Status = CS_IDLE;
colour = instr->colour;
}
break;
default: default:
{ {
Serial.println("Unknown instruction type in queue, skipping..."); Serial.println("Unknown instruction type in queue, skipping...");
@ -346,6 +355,17 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
queueInstruction(instr); // Put charge command in InstrQueue queueInstruction(instr); // Put charge command in InstrQueue
} }
break; 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
}
break;
default: default:
{ {
// Default case, print and continue // Default case, print and continue
@ -437,7 +457,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