From 7ab9de2cc76c5e058f92da235696ed4e36545e17 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Wed, 9 Jun 2021 18:33:37 +0100 Subject: [PATCH 01/15] Tidy up main.cpp formatting Regions allow for Code Folding --- Control/src/main.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 245331e..87707e7 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -1,3 +1,4 @@ +#pragma region Includes #include #include // #include Software Serial not currently needed @@ -9,29 +10,36 @@ #include #include #include +#pragma endregion -// Enable extra debugging info +#pragma region Enable extra debugging info for ESP32 #undef LOG_LOCAL_LEVEL #define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE #include "esp_log.h" +#pragma endregion +#pragma region Definitions eg pins #define RX1pin 14 // Pin 10 on expansion board, UART1 #define TX1pin 4 // Pin 11 on expansion board, UART1 +#pragma endregion -// Function Declarations +#pragma region Function Declarations void printFPGAoutput(); void returnSensorData(); void notFound(AsyncWebServerRequest *request); void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length); +#pragma endregion -// Global objects +#pragma region Global objects AsyncWebServer webserver(80); WebSocketsServer websocketserver(81); Ticker ticker; +#pragma endregion -// Global variables +#pragma region Global variables float battery_voltage = 4.0f; int distance_travelled = 0; +#pragma endregion void setup() { From 108f4d149593b492e0a63eb0790730734293d904 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Thu, 10 Jun 2021 00:19:35 +0100 Subject: [PATCH 02/15] Add nonblock flag to UART FILE* --- .../software/D8M_Camera_Test/main.c | 448 ++++++++---------- 1 file changed, 201 insertions(+), 247 deletions(-) diff --git a/Vision/DE10_LITE_D8M_VIP_16/software/D8M_Camera_Test/main.c b/Vision/DE10_LITE_D8M_VIP_16/software/D8M_Camera_Test/main.c index c45c7f2..f5fe500 100644 --- a/Vision/DE10_LITE_D8M_VIP_16/software/D8M_Camera_Test/main.c +++ b/Vision/DE10_LITE_D8M_VIP_16/software/D8M_Camera_Test/main.c @@ -1,5 +1,3 @@ - - #include #include "I2C_core.h" #include "terasic_includes.h" @@ -12,7 +10,7 @@ #include //EEE_IMGPROC defines -#define EEE_IMGPROC_MSG_START ('R'<<16 | 'B'<<8 | 'B') +#define EEE_IMGPROC_MSG_START ('R' << 16 | 'B' << 8 | 'B') //offsets #define EEE_IMGPROC_STATUS 0 @@ -26,286 +24,242 @@ #define GAIN_STEP 0x040 #define DEFAULT_LEVEL 3 -#define MIPI_REG_PHYClkCtl 0x0056 -#define MIPI_REG_PHYData0Ctl 0x0058 -#define MIPI_REG_PHYData1Ctl 0x005A -#define MIPI_REG_PHYData2Ctl 0x005C -#define MIPI_REG_PHYData3Ctl 0x005E -#define MIPI_REG_PHYTimDly 0x0060 -#define MIPI_REG_PHYSta 0x0062 -#define MIPI_REG_CSIStatus 0x0064 -#define MIPI_REG_CSIErrEn 0x0066 -#define MIPI_REG_MDLSynErr 0x0068 -#define MIPI_REG_FrmErrCnt 0x0080 -#define MIPI_REG_MDLErrCnt 0x0090 +#define MIPI_REG_PHYClkCtl 0x0056 +#define MIPI_REG_PHYData0Ctl 0x0058 +#define MIPI_REG_PHYData1Ctl 0x005A +#define MIPI_REG_PHYData2Ctl 0x005C +#define MIPI_REG_PHYData3Ctl 0x005E +#define MIPI_REG_PHYTimDly 0x0060 +#define MIPI_REG_PHYSta 0x0062 +#define MIPI_REG_CSIStatus 0x0064 +#define MIPI_REG_CSIErrEn 0x0066 +#define MIPI_REG_MDLSynErr 0x0068 +#define MIPI_REG_FrmErrCnt 0x0080 +#define MIPI_REG_MDLErrCnt 0x0090 -void mipi_clear_error(void){ - MipiBridgeRegWrite(MIPI_REG_CSIStatus,0x01FF); // clear error - MipiBridgeRegWrite(MIPI_REG_MDLSynErr,0x0000); // clear error - MipiBridgeRegWrite(MIPI_REG_FrmErrCnt,0x0000); // clear error - MipiBridgeRegWrite(MIPI_REG_MDLErrCnt, 0x0000); // clear error +void mipi_clear_error(void) +{ + MipiBridgeRegWrite(MIPI_REG_CSIStatus, 0x01FF); // clear error + MipiBridgeRegWrite(MIPI_REG_MDLSynErr, 0x0000); // clear error + MipiBridgeRegWrite(MIPI_REG_FrmErrCnt, 0x0000); // clear error + MipiBridgeRegWrite(MIPI_REG_MDLErrCnt, 0x0000); // clear error - MipiBridgeRegWrite(0x0082,0x00); - MipiBridgeRegWrite(0x0084,0x00); - MipiBridgeRegWrite(0x0086,0x00); - MipiBridgeRegWrite(0x0088,0x00); - MipiBridgeRegWrite(0x008A,0x00); - MipiBridgeRegWrite(0x008C,0x00); - MipiBridgeRegWrite(0x008E,0x00); - MipiBridgeRegWrite(0x0090,0x00); + MipiBridgeRegWrite(0x0082, 0x00); + MipiBridgeRegWrite(0x0084, 0x00); + MipiBridgeRegWrite(0x0086, 0x00); + MipiBridgeRegWrite(0x0088, 0x00); + MipiBridgeRegWrite(0x008A, 0x00); + MipiBridgeRegWrite(0x008C, 0x00); + MipiBridgeRegWrite(0x008E, 0x00); + MipiBridgeRegWrite(0x0090, 0x00); } -void mipi_show_error_info(void){ +void mipi_show_error_info(void) +{ - alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt; + alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt; - PHY_status = MipiBridgeRegRead(MIPI_REG_PHYSta); - SCI_status = MipiBridgeRegRead(MIPI_REG_CSIStatus); - MDLSynErr = MipiBridgeRegRead(MIPI_REG_MDLSynErr); - FrmErrCnt = MipiBridgeRegRead(MIPI_REG_FrmErrCnt); - MDLErrCnt = MipiBridgeRegRead(MIPI_REG_MDLErrCnt); - printf("PHY_status=%xh, CSI_status=%xh, MDLSynErr=%xh, FrmErrCnt=%xh, MDLErrCnt=%xh\r\n", PHY_status, SCI_status, MDLSynErr,FrmErrCnt, MDLErrCnt); + PHY_status = MipiBridgeRegRead(MIPI_REG_PHYSta); + SCI_status = MipiBridgeRegRead(MIPI_REG_CSIStatus); + MDLSynErr = MipiBridgeRegRead(MIPI_REG_MDLSynErr); + FrmErrCnt = MipiBridgeRegRead(MIPI_REG_FrmErrCnt); + MDLErrCnt = MipiBridgeRegRead(MIPI_REG_MDLErrCnt); + printf("PHY_status=%xh, CSI_status=%xh, MDLSynErr=%xh, FrmErrCnt=%xh, MDLErrCnt=%xh\r\n", PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt); } -void mipi_show_error_info_more(void){ - printf("FrmErrCnt = %d\n",MipiBridgeRegRead(0x0080)); - printf("CRCErrCnt = %d\n",MipiBridgeRegRead(0x0082)); - printf("CorErrCnt = %d\n",MipiBridgeRegRead(0x0084)); - printf("HdrErrCnt = %d\n",MipiBridgeRegRead(0x0086)); - printf("EIDErrCnt = %d\n",MipiBridgeRegRead(0x0088)); - printf("CtlErrCnt = %d\n",MipiBridgeRegRead(0x008A)); - printf("SoTErrCnt = %d\n",MipiBridgeRegRead(0x008C)); - printf("SynErrCnt = %d\n",MipiBridgeRegRead(0x008E)); - printf("MDLErrCnt = %d\n",MipiBridgeRegRead(0x0090)); - printf("FIFOSTATUS = %d\n",MipiBridgeRegRead(0x00F8)); - printf("DataType = 0x%04x\n",MipiBridgeRegRead(0x006A)); - printf("CSIPktLen = %d\n",MipiBridgeRegRead(0x006E)); +void mipi_show_error_info_more(void) +{ + printf("FrmErrCnt = %d\n", MipiBridgeRegRead(0x0080)); + printf("CRCErrCnt = %d\n", MipiBridgeRegRead(0x0082)); + printf("CorErrCnt = %d\n", MipiBridgeRegRead(0x0084)); + printf("HdrErrCnt = %d\n", MipiBridgeRegRead(0x0086)); + printf("EIDErrCnt = %d\n", MipiBridgeRegRead(0x0088)); + printf("CtlErrCnt = %d\n", MipiBridgeRegRead(0x008A)); + printf("SoTErrCnt = %d\n", MipiBridgeRegRead(0x008C)); + printf("SynErrCnt = %d\n", MipiBridgeRegRead(0x008E)); + printf("MDLErrCnt = %d\n", MipiBridgeRegRead(0x0090)); + printf("FIFOSTATUS = %d\n", MipiBridgeRegRead(0x00F8)); + printf("DataType = 0x%04x\n", MipiBridgeRegRead(0x006A)); + printf("CSIPktLen = %d\n", MipiBridgeRegRead(0x006E)); } +bool MIPI_Init(void) +{ + bool bSuccess; + bSuccess = oc_i2c_init_ex(I2C_OPENCORES_MIPI_BASE, 50 * 1000 * 1000, 400 * 1000); //I2C: 400K + if (!bSuccess) + { + printf("failed to init MIPI- Bridge i2c\r\n"); + } -bool MIPI_Init(void){ - bool bSuccess; - - - bSuccess = oc_i2c_init_ex(I2C_OPENCORES_MIPI_BASE, 50*1000*1000,400*1000); //I2C: 400K - if (!bSuccess) - printf("failed to init MIPI- Bridge i2c\r\n"); - - usleep(50*1000); + usleep(50 * 1000); MipiBridgeInit(); - usleep(500*1000); - -// bSuccess = oc_i2c_init_ex(I2C_OPENCORES_CAMERA_BASE, 50*1000*1000,400*1000); //I2C: 400K -// if (!bSuccess) -// printf("failed to init MIPI- Camera i2c\r\n"); + usleep(500 * 1000); MipiCameraInit(); MIPI_BIN_LEVEL(DEFAULT_LEVEL); -// OV8865_FOCUS_Move_to(340); -// oc_i2c_uninit(I2C_OPENCORES_CAMERA_BASE); // Release I2C bus , due to two I2C master shared! + usleep(1000); - - usleep(1000); - - -// oc_i2c_uninit(I2C_OPENCORES_MIPI_BASE); - - return bSuccess; + return bSuccess; } - - - int main() { + fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK); - fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK); - - printf("DE10-LITE D8M VGA Demo\n"); - printf("Imperial College EEE2 Project version\n"); - IOWR(MIPI_PWDN_N_BASE, 0x00, 0x00); - IOWR(MIPI_RESET_N_BASE, 0x00, 0x00); - - usleep(2000); - IOWR(MIPI_PWDN_N_BASE, 0x00, 0xFF); - usleep(2000); - IOWR(MIPI_RESET_N_BASE, 0x00, 0xFF); - - printf("Image Processor ID: %x\n",IORD(0x42000,EEE_IMGPROC_ID)); - //printf("Image Processor ID: %x\n",IORD(EEE_IMGPROC_0_BASE,EEE_IMGPROC_ID)); //Don't know why this doesn't work - definition is in system.h in BSP - - - usleep(2000); - - - // MIPI Init - if (!MIPI_Init()){ - printf("MIPI_Init Init failed!\r\n"); - }else{ - printf("MIPI_Init Init successfully!\r\n"); - } - -// while(1){ - mipi_clear_error(); - usleep(50*1000); - mipi_clear_error(); - usleep(1000*1000); - mipi_show_error_info(); -// mipi_show_error_info_more(); - printf("\n"); -// } - - -#if 0 // focus sweep - printf("\nFocus sweep\n"); - alt_u16 ii= 350; - alt_u8 dir = 0; - while(1){ - if(ii< 50) dir = 1; - else if (ii> 1000) dir =0; - - if(dir) ii += 20; - else ii -= 20; - - printf("%d\n",ii); - OV8865_FOCUS_Move_to(ii); - usleep(50*1000); - } -#endif + printf("DE10-LITE D8M VGA Demo\n"); + printf("Imperial College EEE2 Project version\n"); + IOWR(MIPI_PWDN_N_BASE, 0x00, 0x00); + IOWR(MIPI_RESET_N_BASE, 0x00, 0x00); + usleep(2000); + IOWR(MIPI_PWDN_N_BASE, 0x00, 0xFF); + usleep(2000); + IOWR(MIPI_RESET_N_BASE, 0x00, 0xFF); + printf("Image Processor ID: %x\n", IORD(0x42000, EEE_IMGPROC_ID)); + //printf("Image Processor ID: %x\n",IORD(EEE_IMGPROC_0_BASE,EEE_IMGPROC_ID)); //Don't know why this doesn't work - definition is in system.h in BSP + usleep(2000); + // MIPI Init + if (!MIPI_Init()) + { + printf("MIPI_Init Init failed!\r\n"); + } + else + { + printf("MIPI_Init Init successfully!\r\n"); + } + // while(1){ + mipi_clear_error(); + usleep(50 * 1000); + mipi_clear_error(); + usleep(1000 * 1000); + mipi_show_error_info(); + // mipi_show_error_info_more(); + printf("\n"); + // } ////////////////////////////////////////////////////////// - alt_u16 bin_level = DEFAULT_LEVEL; - alt_u8 manual_focus_step = 10; - alt_u16 current_focus = 300; - int boundingBoxColour = 0; - alt_u32 exposureTime = EXPOSURE_INIT; - alt_u16 gain = GAIN_INIT; + alt_u16 bin_level = DEFAULT_LEVEL; + alt_u8 manual_focus_step = 10; + alt_u16 current_focus = 300; + int boundingBoxColour = 0; + alt_u32 exposureTime = EXPOSURE_INIT; + alt_u16 gain = GAIN_INIT; - OV8865SetExposure(exposureTime); - OV8865SetGain(gain); - Focus_Init(); + OV8865SetExposure(exposureTime); + OV8865SetGain(gain); + Focus_Init(); - FILE* ser = fopen("/dev/uart_0", "rb+"); - if(ser){ - printf("Opened UART\n"); - } else { - printf("Failed to open UART\n"); - while (1); + FILE *ser = fopen("/dev/uart_0", "rb+"); + fcntl(ser, F_SETFL, O_NONBLOCK); + if (ser) + { + printf("Opened UART\n"); + } + else + { + printf("Failed to open UART\n"); + while (1) + { + } + } + + while (1) + { + + // touch KEY0 to trigger Auto focus + if ((IORD(KEY_BASE, 0) & 0x03) == 0x02) + { + + current_focus = Focus_Window(320, 240); + } + // touch KEY1 to ZOOM + if ((IORD(KEY_BASE, 0) & 0x03) == 0x01) + { + if (bin_level == 3) + bin_level = 1; + else + bin_level++; + printf("set bin level to %d\n", bin_level); + MIPI_BIN_LEVEL(bin_level); + usleep(500000); } - while(1){ + //Read messages from the image processor and print them on the terminal + while ((IORD(0x42000, EEE_IMGPROC_STATUS) >> 8) & 0xff) + { //Find out if there are words to read + int word = IORD(0x42000, EEE_IMGPROC_MSG); //Get next word from message buffer + if (fwrite(&word, 4, 1, ser) != 1) + printf("Error writing to UART"); + if (word == EEE_IMGPROC_MSG_START) //Newline on message identifier + printf("\n"); + printf("%08x ", word); + } - // touch KEY0 to trigger Auto focus - if((IORD(KEY_BASE,0)&0x03) == 0x02){ + //Update the bounding box colour + boundingBoxColour = ((boundingBoxColour + 1) & 0xff); + IOWR(0x42000, EEE_IMGPROC_BBCOL, (boundingBoxColour << 8) | (0xff - boundingBoxColour)); - current_focus = Focus_Window(320,240); - } - // touch KEY1 to ZOOM - if((IORD(KEY_BASE,0)&0x03) == 0x01){ - if(bin_level == 3 )bin_level = 1; - else bin_level ++; - printf("set bin level to %d\n",bin_level); - MIPI_BIN_LEVEL(bin_level); - usleep(500000); + //Process input commands + int in = getchar(); + switch (in) + { + case 'e': + { + exposureTime += EXPOSURE_STEP; + OV8865SetExposure(exposureTime); + printf("\nExposure = %x ", exposureTime); + break; + } + case 'd': + { + exposureTime -= EXPOSURE_STEP; + OV8865SetExposure(exposureTime); + printf("\nExposure = %x ", exposureTime); + break; + } + case 't': + { + gain += GAIN_STEP; + OV8865SetGain(gain); + printf("\nGain = %x ", gain); + break; + } + case 'g': + { + gain -= GAIN_STEP; + OV8865SetGain(gain); + printf("\nGain = %x ", gain); + break; + } + case 'r': + { + current_focus += manual_focus_step; + if (current_focus > 1023) + current_focus = 1023; + OV8865_FOCUS_Move_to(current_focus); + printf("\nFocus = %x ", current_focus); + break; + } + case 'f': + { + if (current_focus > manual_focus_step) + current_focus -= manual_focus_step; + OV8865_FOCUS_Move_to(current_focus); + printf("\nFocus = %x ", current_focus); + break; + } + } - } - - - #if 0 - if((IORD(KEY_BASE,0)&0x0F) == 0x0E){ - - current_focus = Focus_Window(320,240); - } - - // touch KEY1 to trigger Manual focus - step - if((IORD(KEY_BASE,0)&0x0F) == 0x0D){ - - if(current_focus > manual_focus_step) current_focus -= manual_focus_step; - else current_focus = 0; - OV8865_FOCUS_Move_to(current_focus); - - } - - // touch KEY2 to trigger Manual focus + step - if((IORD(KEY_BASE,0)&0x0F) == 0x0B){ - current_focus += manual_focus_step; - if(current_focus >1023) current_focus = 1023; - OV8865_FOCUS_Move_to(current_focus); - } - - // touch KEY3 to ZOOM - if((IORD(KEY_BASE,0)&0x0F) == 0x07){ - if(bin_level == 3 )bin_level = 1; - else bin_level ++; - printf("set bin level to %d\n",bin_level); - MIPI_BIN_LEVEL(bin_level); - usleep(500000); - - } - #endif - - //Read messages from the image processor and print them on the terminal - while ((IORD(0x42000,EEE_IMGPROC_STATUS)>>8) & 0xff) { //Find out if there are words to read - int word = IORD(0x42000,EEE_IMGPROC_MSG); //Get next word from message buffer - if (fwrite(&word, 4, 1, ser) != 1) - printf("Error writing to UART"); - if (word == EEE_IMGPROC_MSG_START) //Newline on message identifier - printf("\n"); - printf("%08x ",word); - } - - //Update the bounding box colour - boundingBoxColour = ((boundingBoxColour + 1) & 0xff); - IOWR(0x42000, EEE_IMGPROC_BBCOL, (boundingBoxColour << 8) | (0xff - boundingBoxColour)); - - //Process input commands - int in = getchar(); - switch (in) { - case 'e': { - exposureTime += EXPOSURE_STEP; - OV8865SetExposure(exposureTime); - printf("\nExposure = %x ", exposureTime); - break;} - case 'd': { - exposureTime -= EXPOSURE_STEP; - OV8865SetExposure(exposureTime); - printf("\nExposure = %x ", exposureTime); - break;} - case 't': { - gain += GAIN_STEP; - OV8865SetGain(gain); - printf("\nGain = %x ", gain); - break;} - case 'g': { - gain -= GAIN_STEP; - OV8865SetGain(gain); - printf("\nGain = %x ", gain); - break;} - case 'r': { - current_focus += manual_focus_step; - if(current_focus >1023) current_focus = 1023; - OV8865_FOCUS_Move_to(current_focus); - printf("\nFocus = %x ",current_focus); - break;} - case 'f': { - if(current_focus > manual_focus_step) current_focus -= manual_focus_step; - OV8865_FOCUS_Move_to(current_focus); - printf("\nFocus = %x ",current_focus); - break;} - } - - - //Main loop delay - usleep(10000); - - }; - return 0; + //Main loop delay + usleep(10000); + }; + return 0; } From 778be64c3d7b33848ddb15fd69f6f4ba91a96fdf Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Thu, 10 Jun 2021 18:00:13 +0100 Subject: [PATCH 03/15] Create Vision<->Control reference doc --- Control/ref/vision.cpp | 48 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/Control/ref/vision.cpp b/Control/ref/vision.cpp index e699eca..14e5ec0 100644 --- a/Control/ref/vision.cpp +++ b/Control/ref/vision.cpp @@ -1 +1,49 @@ +#include +#include +#include + const int ARDUINO_IO[16] = {-1/*RX*/, -1/*RX*/, 23, 22, 21, 19, 18, 5, 17, 16, 14, 4, 15, 2, 13, 12}; // Expansion board mapping +#define RXpin ARDUINO_IO[11] // Define your RX pin here +#define TXpin ARDUINO_IO[10] // Define your TX pin here +FILE* SerialUART; + +void setup() +{ + Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) +} + +void loop() +{ + int command = getc(SerialUART); + // command char, used for controlling exposure/focus/gain settings: + // e = increase exposure + // d = decrease exposure + // r = increase focus + // f = decrease focus + // t = increase gain + // g = decrease gain + + // Bounding Box edges + int bb_left = 0; + int bb_right = 0; + int bb_top = 0; + int bb_bottom = 0; + // Weighted average of detected pixels coordinates + int centre_x = 0; + int centre_y = 0; + // Heading from DE10-Lite magnetometer + float heading = 0.0; + + // Build hardcode JSON packet on DE10-Lite using fprintf() as space is minimal and library would be too large. + // fprintf(SerialUART, "{\"bb\":[%d,%d,%d,%d],\"cen\":[%d,%d],\"cH\":%d\"}", bb_left, bb_right, bb_top, bb_bottom, centre_x, centre_y, heading); + + DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be + deserializeJson(rdoc, SerialUART); + bb_left = rdoc["bb"][0]; + bb_right = rdoc["bb"][1]; + bb_top = rdoc["bb"][2]; + bb_bottom = rdoc["bb"][3]; + centre_x = rdoc["cen"][0]; + centre_y = rdoc["cen"][1]; + heading = rdoc["cH"]; +} \ No newline at end of file From 88adbdc5723072c4aa0872e3bf03f735536203f8 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sat, 12 Jun 2021 12:47:12 +0100 Subject: [PATCH 04/15] Correct location of credentials.h to include/ --- Control/.gitignore | 2 +- Control/{src => include}/credentials.h.dummy | 0 Control/src/main.cpp | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename Control/{src => include}/credentials.h.dummy (100%) diff --git a/Control/.gitignore b/Control/.gitignore index c7b130a..3074d78 100644 --- a/Control/.gitignore +++ b/Control/.gitignore @@ -3,4 +3,4 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch -src/credentials.h +include/credentials.h diff --git a/Control/src/credentials.h.dummy b/Control/include/credentials.h.dummy similarity index 100% rename from Control/src/credentials.h.dummy rename to Control/include/credentials.h.dummy diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 87707e7..710f1be 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -7,7 +7,7 @@ #include #include "TickerV2.h" #include -#include +#include "credentials.h" #include #include #pragma endregion From 5667ef67f21cb6b1bb5706c265b3eb305a807245 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sat, 12 Jun 2021 12:50:44 +0100 Subject: [PATCH 05/15] Add basic status flag for flow control --- Control/include/status.h | 11 +++++++++++ Control/src/main.cpp | 3 +++ 2 files changed, 14 insertions(+) create mode 100644 Control/include/status.h diff --git a/Control/include/status.h b/Control/include/status.h new file mode 100644 index 0000000..4b8b2d9 --- /dev/null +++ b/Control/include/status.h @@ -0,0 +1,11 @@ +#ifndef CONTROL_STATUS_H +#define CONTROL_STATUS_H + +typedef enum { + CS_ERROR = -1, + CS_IDLE, + CS_MOVING, + CS_CHARGING +} ControlStatus_t; + +#endif diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 710f1be..cda14c6 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -10,6 +10,7 @@ #include "credentials.h" #include #include +#include "status.h" #pragma endregion #pragma region Enable extra debugging info for ESP32 @@ -37,6 +38,7 @@ Ticker ticker; #pragma endregion #pragma region Global variables +ControlStatus_t Status; float battery_voltage = 4.0f; int distance_travelled = 0; #pragma endregion @@ -47,6 +49,7 @@ 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 + Status = CS_IDLE; Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 // Set up remaining communication ports here (Energy, Drive, Vision) From 1d297354295b2439d9105669839b691fea318774 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sat, 12 Jun 2021 15:15:37 +0100 Subject: [PATCH 06/15] Update Command<->Control reference doc --- Control/ref/command.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index 16761f0..6ece01e 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -3,7 +3,7 @@ #include #define WebSocket 0 -int batteryVoltage, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Command ==> Control +int batteryVoltage, batteryLevel, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Command ==> Control int command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Control ==> Command void setup() {} @@ -13,6 +13,7 @@ void loop() DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket batteryVoltage = rdoc["bV"]; + batteryLevel = rdoc["bL"]; totalTripDistance = rdoc["tD"]; currentHeading = rdoc["cH"]; current_x = rdoc["pos"][0]; From 52e14dfaa825e46d4d72a550edd216a3c6986182 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sat, 12 Jun 2021 21:00:55 +0100 Subject: [PATCH 07/15] Update Command<->Control reference doc Add Rover state and correct comments --- Control/ref/command.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index 6ece01e..ec029d9 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -3,8 +3,8 @@ #include #define WebSocket 0 -int batteryVoltage, batteryLevel, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Command ==> Control -int command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Control ==> Command +int state, batteryVoltage, batteryLevel, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command +int command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Command ==> Control void setup() {} @@ -12,6 +12,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 batteryVoltage = rdoc["bV"]; batteryLevel = rdoc["bL"]; totalTripDistance = rdoc["tD"]; From 1e87e9b0f423fd2d904fb94298a9959be2734912 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sat, 12 Jun 2021 23:04:01 +0100 Subject: [PATCH 08/15] Fix incorrect variable types reqSpeed is between 0 & 1, batteryVoltage has decimal places, both should be float not int --- Control/ref/command.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index ec029d9..0a65d23 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -3,8 +3,10 @@ #include #define WebSocket 0 -int state, batteryVoltage, batteryLevel, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command -int command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Command ==> Control +int state, batteryLevel, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command +float batteryVoltage; // Info Control ==> Command +int command_id, mode, reqHeading, reqDistance, reqCharge; // Info Command ==> Control +float reqSpeed; // Info Command ==> Control void setup() {} From 5e6254029ae80721f029085278b199978b064129 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 00:50:38 +0100 Subject: [PATCH 09/15] Add instruction.h Struct for holding instructions in FIFO Queue, enum for instruction types --- Control/include/instruction.h | 21 +++++++++++++++++++++ Control/src/main.cpp | 1 + 2 files changed, 22 insertions(+) create mode 100644 Control/include/instruction.h diff --git a/Control/include/instruction.h b/Control/include/instruction.h new file mode 100644 index 0000000..574d542 --- /dev/null +++ b/Control/include/instruction.h @@ -0,0 +1,21 @@ +#ifndef INSTRUCTION_H +#define INSTRUCTION_H + +typedef enum { + INSTR_RESET = -1, + INSTR_STOP, + INSTR_MOVE, + INSTR_CHARGE +} instr_t; + +typedef struct instruction +{ + int id; + int instr; + int heading; + int distance; + float speed; + int charge; +} RoverInstruction; + +#endif diff --git a/Control/src/main.cpp b/Control/src/main.cpp index cda14c6..8ea0695 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -11,6 +11,7 @@ #include #include #include "status.h" +#include "instruction.h" #pragma endregion #pragma region Enable extra debugging info for ESP32 From bbec96810fb125760ed40436e05008c8fd6c91eb Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 00:56:49 +0100 Subject: [PATCH 10/15] Set up basic rover flow, individual functions to be completed --- Control/src/main.cpp | 309 ++++++++++++++++++++++++++++++++++--------- 1 file changed, 247 insertions(+), 62 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 8ea0695..637e9d8 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -1,7 +1,7 @@ #pragma region Includes #include #include -// #include Software Serial not currently needed +#include #include #include #include @@ -12,6 +12,7 @@ #include #include "status.h" #include "instruction.h" +#include #pragma endregion #pragma region Enable extra debugging info for ESP32 @@ -21,27 +22,44 @@ #pragma endregion #pragma region Definitions eg pins -#define RX1pin 14 // Pin 10 on expansion board, UART1 -#define TX1pin 4 // Pin 11 on expansion board, UART1 +#define RX1pin 18 // Pin 6 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 region Function Declarations -void printFPGAoutput(); -void returnSensorData(); void notFound(AsyncWebServerRequest *request); 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 region Global objects AsyncWebServer webserver(80); WebSocketsServer websocketserver(81); Ticker ticker; +SoftwareSerial Serial3; +std::queue InstrQueue; #pragma endregion #pragma region Global variables ControlStatus_t Status; -float battery_voltage = 4.0f; -int distance_travelled = 0; +float batteryVoltage; +int batteryLevel; +int odometer; +int heading; +int xpos, ypos; +int signalStrength; +int lastCompletedCommand; #pragma endregion 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("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) + + // Set global variable startup values Status = CS_IDLE; - Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) - Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 - // Set up remaining communication ports here (Energy, Drive, Vision) + batteryVoltage = 0; + batteryLevel = 0; + odometer = 0; + heading = 0; + xpos = 0; + ypos = 0; + signalStrength = 0; + lastCompletedCommand = 0; if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -63,11 +92,11 @@ void setup() Serial.println("SPIFFS mounted"); 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); } - while (!MDNS.begin("rover")) + while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/" { Serial.println("Error setting up mDNS, retrying in 5s"); delay(5000); @@ -75,56 +104,95 @@ void setup() Serial.println("mDNS set up, access Control Panel at 'rover.local/'"); 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) - { request->send(SPIFFS, "/favicon.ico", "image/png"); }); - webserver.onNotFound(notFound); - webserver.begin(); + { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon + webserver.onNotFound(notFound); // Set up basic 404NotFound page + webserver.begin(); // Start Asynchronous Web Server - websocketserver.begin(); - websocketserver.onEvent(webSocketEvent); - ticker.attach(0.5, returnSensorData); + websocketserver.begin(); // Start Websocket Server + websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command + 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 -} - -void printFPGAoutput() -{ // Print serial communication from FPGA to serial monitor - String FPGAoutput; - if (Serial1.available()) + switch (Status) { - FPGAoutput = String(Serial1.readStringUntil('\n')); - Serial.println(FPGAoutput); - } -} - -void returnSensorData() -{ - // Collect sensor data here? - distance_travelled++; - if (battery_voltage < 6) + case CS_ERROR: { - battery_voltage += 0.2; + Serial.println("Rover in error state, rebooting..."); + exit(1); } - else + break; + case CS_IDLE: { - battery_voltage = 4; + 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 + { + // 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) @@ -132,7 +200,7 @@ void notFound(AsyncWebServerRequest *request) 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) { @@ -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); } break; - case WStype_TEXT: + case WStype_TEXT: // MSG received from command panel { - Serial.printf("Client[%u] sent Text: %s\n", num, payload); - String command = String((char *)(payload)); + Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal + 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. - DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive. + DynamicJsonDocument rdoc(200); // Create instance of DynamicJsonDocument on heap, 200 Bytes + DeserializationError error = deserializeJson(rdoc, command); // Convert command string to JSONDocument and capture any errors if (error) { Serial.print("deserializeJson() failed: "); @@ -161,12 +229,62 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) return; } - int MVM_F_status = doc["MVM_F"]; - int MVM_L_status = doc["MVM_L"]; - int MVM_R_status = doc["MVM_R"]; - int MVM_B_status = doc["MVM_B"]; + RoverInstruction instr; + int mode = rdoc["mode"]; + switch (mode) + { + 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; 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")); 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"); +} \ No newline at end of file From 39603efd3c2b04e7530b243fa0ccd03e944bd703 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 16:51:46 +0100 Subject: [PATCH 11/15] Add odometer reset flag to Drive<->Control reference doc --- Control/ref/drive.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Control/ref/drive.cpp b/Control/ref/drive.cpp index 8a59cdc..e10695f 100644 --- a/Control/ref/drive.cpp +++ b/Control/ref/drive.cpp @@ -21,6 +21,7 @@ void loop() int distance = rdoc["dist"]; float speed = rdoc["sp"]; int currentHeading = rdoc["cH"]; + bool resetDistanceTravelled = rdoc["rstD"]; bool commandComplete = 0; float powerUsage_mW = 0.0; From 0483397ae61855325a486e7ed5741f1557ff8aa2 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 17:26:50 +0100 Subject: [PATCH 12/15] Add emergency stop function --- Control/ref/drive.cpp | 4 ++-- Control/src/main.cpp | 12 ++++++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/Control/ref/drive.cpp b/Control/ref/drive.cpp index e10695f..a04c7cd 100644 --- a/Control/ref/drive.cpp +++ b/Control/ref/drive.cpp @@ -18,8 +18,8 @@ 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 speed = rdoc["sp"]; + int distance = rdoc["dist"]; // -1 for emergency stop + float speed = rdoc["sp"]; // -1 for emergency stop int currentHeading = rdoc["cH"]; bool resetDistanceTravelled = rdoc["rstD"]; diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 637e9d8..b592441 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -357,12 +357,20 @@ void recvFromVision() // TO DO { } -void emergencyStop() // TO DO +void emergencyStop() { - // Send stop signals to drive, energy + DynamicJsonDocument tdoc(1024); + tdoc["rH"] = heading; + tdoc["dist"] = -1; + tdoc["sp"] = -1; + tdoc["cH"] = heading; + tdoc["ch"] = 0; + serializeJson(tdoc, Serial1); // Send stop signals to Drive + serializeJson(tdoc, Serial2); // Send stop signals to Energy while (InstrQueue.size()) { InstrQueue.pop(); // Clear Instruction Queue } + Status = CS_IDLE; // Reset rover to idle state Serial.println("Instruction Queue cleared"); } \ No newline at end of file From f3ca90fb2e4cf2575b8281b0896d156df31ffa8f Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 17:59:21 +0100 Subject: [PATCH 13/15] Add battery cycle tracking to Command<->Control reference doc --- Control/ref/command.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index 0a65d23..e4ff859 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -15,8 +15,9 @@ 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 - batteryVoltage = rdoc["bV"]; - batteryLevel = rdoc["bL"]; + batteryVoltage = rdoc["bV"]; + batteryLevel = rdoc["bL"]; + batteryCycles = rdoc["bC"]; totalTripDistance = rdoc["tD"]; currentHeading = rdoc["cH"]; current_x = rdoc["pos"][0]; From dc927495ec9922e9154572824aa9117abcc65261 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 18:26:00 +0100 Subject: [PATCH 14/15] Fix type errors for battery stat reporting --- Control/ref/command.cpp | 4 ++-- Control/src/main.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/Control/ref/command.cpp b/Control/ref/command.cpp index e4ff859..3a014df 100644 --- a/Control/ref/command.cpp +++ b/Control/ref/command.cpp @@ -3,8 +3,8 @@ #include #define WebSocket 0 -int state, batteryLevel, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command -float batteryVoltage; // Info Control ==> Command +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 float reqSpeed; // Info Command ==> Control diff --git a/Control/src/main.cpp b/Control/src/main.cpp index b592441..52dca76 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -54,7 +54,8 @@ std::queue InstrQueue; #pragma region Global variables ControlStatus_t Status; float batteryVoltage; -int batteryLevel; +float batteryLevel; +float batteryCycles; int odometer; int heading; int xpos, ypos; From 24bd7db3a8a5b732da2d5953b5ddd876a39a4ed1 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Sun, 13 Jun 2021 18:36:13 +0100 Subject: [PATCH 15/15] Rover Control Program Complete To be tested --- Control/src/main.cpp | 140 ++++++++++++++++++++++++++++++------------- 1 file changed, 100 insertions(+), 40 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 52dca76..ead562f 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -37,10 +37,11 @@ void queueInstruction(RoverInstruction instruction); void sendToCommand(); void sendToDrive(RoverInstruction instruction); void recvFromDrive(); -void sendToEnergy(RoverInstruction instruction); +void sendToEnergy(bool instruction); void recvFromEnergy(); void sendToVision(); void recvFromVision(); +void emergencyStop(); #pragma endregion #pragma region Global objects @@ -60,7 +61,11 @@ int odometer; int heading; int xpos, ypos; int signalStrength; -int lastCompletedCommand; +int lastExecutedCommand, lastCompletedCommand; +bool driveCommandComplete; +int bb_left, bb_right, bb_top, bb_bottom; +int bb_centre_x, bb_centre_y; +float chargeGoal; #pragma endregion void setup() @@ -78,12 +83,16 @@ void setup() Status = CS_IDLE; batteryVoltage = 0; batteryLevel = 0; + batteryCycles = 0; odometer = 0; heading = 0; xpos = 0; ypos = 0; signalStrength = 0; + lastExecutedCommand = 0; lastCompletedCommand = 0; + driveCommandComplete = 1; + chargeGoal = 0; if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -116,9 +125,12 @@ void setup() ticker.attach(0.5, sendToCommand); // Set up recurring function to forward rover status to Command } -void loop() // TO DO +void loop() { websocketserver.loop(); // Handle incoming client connections + recvFromDrive(); // Update stats from Drive + recvFromEnergy(); // Update stats from Energy + recvFromVision(); // Update stats from Vision switch (Status) { case CS_ERROR: @@ -129,27 +141,24 @@ void loop() // TO DO break; case CS_IDLE: { - if (InstrQueue.empty()) // If Rover idle and InstrQueue empty: + if (!InstrQueue.empty()) // If Rover idle and InstrQueue NOT empty: Do the next command in the queue { - // TO DO: Collect all data (recvFrom) and - sendToCommand(); // Update command panel - // Maybe wait 1s? Possibly prevent from looping too fast - } - else - { - // Do the next command in the queue - RoverInstruction *instr = &InstrQueue.front(); - switch (instr->instr) + RoverInstruction *instr = &InstrQueue.front(); // Get next command + switch (instr->instr) // Determine command type { - case INSTR_RESET: + case INSTR_RESET: // Reset telemetry values (zeroing position/distance) { odometer = 0; xpos = 0; ypos = 0; + DynamicJsonDocument tdoc(128); + tdoc["rstD"] = 1; + serializeJson(tdoc, Serial1); // Send reset odometer signal to Drive } break; - case INSTR_STOP: + case INSTR_STOP: // Emergency stop { + Status = CS_ERROR; while (1) { Serial.println("Emergency Stop should not get queued, hold and print"); @@ -157,16 +166,18 @@ void loop() // TO DO } } break; - case INSTR_MOVE: + case INSTR_MOVE: // Normal movement { - Status = CS_MOVING; - sendToDrive(*instr); + Status = CS_MOVING; // Set moving state + driveCommandComplete = 0; + sendToDrive(*instr); // Forward to Drive handler } break; - case INSTR_CHARGE: + case INSTR_CHARGE: // Normal charge { - Status = CS_CHARGING; - sendToEnergy(*instr); + Status = CS_CHARGING; // Set charging state + chargeGoal = (float)instr->charge; // Set charging goal + sendToEnergy(1); // Forward to Energy handler } break; default: @@ -175,17 +186,36 @@ void loop() // TO DO } break; } + lastExecutedCommand = instr->id; // Update tracker of last processed command } } break; case CS_MOVING: { - // TO DO + if (driveCommandComplete) // If movement command complete: + { + Status = CS_IDLE; // Set rover state back to idle + lastCompletedCommand = lastExecutedCommand; // Update last completed command + } + else // If movement command NOT complete: + { // Send (up to date) current heading to Drive + DynamicJsonDocument tdoc(128); + tdoc["rH"] = -1; + tdoc["cH"] = heading; + serializeJson(tdoc, Serial1); + } } break; case CS_CHARGING: { - // TO DO + if (batteryLevel >= chargeGoal) // Compare batteryLevel to chargeGoal + { + Status = CS_IDLE; + lastCompletedCommand = lastExecutedCommand; // Update last completed command + sendToEnergy(0); // Stop charging if goal reached + } + // Otherwise continue charging, no change + } break; default: @@ -193,7 +223,9 @@ void loop() // TO DO Serial.println("Unknown rover state, exiting..."); exit(1); } + break; } + delay(500); } void notFound(AsyncWebServerRequest *request) @@ -201,7 +233,7 @@ void notFound(AsyncWebServerRequest *request) 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) // TO DO +void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) { switch (type) { @@ -241,7 +273,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) instr.instr = INSTR_RESET; // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] - /* Put reset command in commandFIFO */ + queueInstruction(instr); // Put reset command in InstrQueue } break; case 0: // Stop immediately, clear command cache @@ -250,7 +282,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) // 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 */ + emergencyStop(); } break; case 1: // Normal movement command, added to end of command cache @@ -263,7 +295,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) instr.speed = rdoc["rS"]; // Ignore rdoc["rC"] - /* Put movement command in commandFIFO */ + queueInstruction(instr); // Put movement command in InstrQueue } break; case 2: // Normal charge command, results in no motion, added to end of command cache @@ -274,18 +306,17 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) instr.charge = rdoc["rC"]; // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"] - /* Put charge command in commandFIFO */ + queueInstruction(instr); // Put charge command in InstrQueue } 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"] + // Default case, print and continue + Serial.println("Unknown Command type received, ignoring"); + // Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] } break; } - - queueInstruction(instr); } break; case WStype_PONG: @@ -313,6 +344,7 @@ void sendToCommand() tdoc["st"] = Status; tdoc["bV"] = batteryVoltage; tdoc["bL"] = batteryLevel; + tdoc["bC"] = batteryCycles; tdoc["tD"] = odometer; tdoc["cH"] = heading; tdoc["pos"][0] = xpos; @@ -334,19 +366,36 @@ void sendToDrive(RoverInstruction instruction) serializeJson(tdoc, Serial1); } -void recvFromDrive() // TO DO +void recvFromDrive() // Update telemetry data and state info from Drive packet { + if (Serial1.available()) // Check for input from UART1 (Connected to Drive) + { + DynamicJsonDocument rdoc(1024); + deserializeJson(rdoc, Serial1); + driveCommandComplete = rdoc["comp"]; + odometer = rdoc["mm"]; + xpos = rdoc["pos"][0]; + ypos = rdoc["pos"][1]; + } } -void sendToEnergy(RoverInstruction instruction) +void sendToEnergy(bool instruction) { - DynamicJsonDocument tdoc(1024); - tdoc["ch"] = instruction.charge; + DynamicJsonDocument tdoc(128); + tdoc["ch"] = instruction; // Start charging serializeJson(tdoc, Serial2); } -void recvFromEnergy() // TO DO +void recvFromEnergy() // Update telemetry data and state info from Energy packet { + if (Serial2.available()) // Check for input from UART2 (Connected to Energy) + { + DynamicJsonDocument rdoc(1024); + deserializeJson(rdoc, Serial2); + batteryLevel = rdoc["soc"]; + batteryVoltage = rdoc["mV"]; + batteryCycles = rdoc["cyc"]; + } } void sendToVision() @@ -354,8 +403,20 @@ void sendToVision() Serial3.print("R"); // Request new data from Vision } -void recvFromVision() // TO DO +void recvFromVision() // Update bounding box and obstacle detection data from Vision packet { + if (Serial3.available()) // Check for input from UART3 (Connected to Vision) + { + DynamicJsonDocument rdoc(1024); + deserializeJson(rdoc, Serial3); + bb_left = rdoc["bb"][0]; + bb_right = rdoc["bb"][1]; + bb_top = rdoc["bb"][2]; + bb_bottom = rdoc["bb"][3]; + bb_centre_x = rdoc["cen"][0]; + bb_centre_y = rdoc["cen"][1]; + heading = rdoc["cH"]; + } } void emergencyStop() @@ -365,9 +426,8 @@ void emergencyStop() tdoc["dist"] = -1; tdoc["sp"] = -1; tdoc["cH"] = heading; - tdoc["ch"] = 0; serializeJson(tdoc, Serial1); // Send stop signals to Drive - serializeJson(tdoc, Serial2); // Send stop signals to Energy + sendToEnergy(0); // Send stop signal to Energy while (InstrQueue.size()) { InstrQueue.pop(); // Clear Instruction Queue