ELEC50003-P1-CW/Control/ref/drive.cpp
2021-06-10 14:40:00 +01:00

40 lines
1.3 KiB
C++

#include <Arduino.h>
#include <ArduinoJson.h>
#include <string>
#define RXpin 0 // Define your RX pin here
#define TXpin 0 // Define your TX pin here
void setup()
{
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
Serial1.begin(9600, SERIAL_8N1, RXpin, TXpin); // Set up hardware UART1
// Other Drive setup stuff
}
void loop()
{
DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
deserializeJson(rdoc, Serial1); // Take JSON input from UART1
int requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored
int distance = rdoc["dist"];
float spd = rdoc["sp"];
int currentHeading = rdoc["cH"];
bool commandComplete = 0;
float powerUsage_mWh = 0.0;
int distTravelled_mm = 0;
int current_x = 0;
int current_y = 0;
// Do Drive stuff, set the 5 values above
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
tdoc["comp"] = commandComplete; // If 0: command in progress, current heading requested
tdoc["mWh"] = powerUsage_mWh;
tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y;
serializeJson(tdoc, Serial1); // Build JSON and send on UART1
}