From be103e30721cf3b4fade3c516ac6d7f148fad6d9 Mon Sep 17 00:00:00 2001 From: NikitaK Date: Wed, 9 Jun 2021 18:04:51 +0100 Subject: [PATCH 01/17] Add files via upload --- Drive/20210706_Drive_Mars_Rover.ino | 798 ++++++++++++++++++++++++++++ 1 file changed, 798 insertions(+) create mode 100644 Drive/20210706_Drive_Mars_Rover.ino diff --git a/Drive/20210706_Drive_Mars_Rover.ino b/Drive/20210706_Drive_Mars_Rover.ino new file mode 100644 index 0000000..ecfd6c2 --- /dev/null +++ b/Drive/20210706_Drive_Mars_Rover.ino @@ -0,0 +1,798 @@ +#include +#include +#include +#include +#include +#include "SPI.h" + +#define RXpin 0 // Define your RX pin here +#define TXpin 0 // Define your TX pin here + +bool debug = true; + +//TO IMPLEMENT +//DONE 2 way serial +//DONE F<>,B<>,S,L<>,R<>,p<0--1023> +//DONE Obtain current and power usage, get voltage from analog pin +//request angle facing +//DONE speed control 0-1 +//speed calibration, 0 stop and max speed to match +//distance travveled and x and y at request + + +//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// +INA219_WE ina219; // this is the instantiation of the library for the current sensor + +float open_loop, closed_loop; // Duty Cycles +float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables +unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration +float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals +float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period. +float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid. +float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time +float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid. +float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller +float uv_max = 4, uv_min = 0; //anti-windup limitation +float ui_max = 1, ui_min = 0; //anti-windup limitation +float current_limit = 1.0; +boolean Boost_mode = 0; +boolean CL_mode = 0; + + +unsigned int loopTrigger; +unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging. + + +//************************** Motor Constants **************************// +unsigned long previousMillis = 0; //initializing time counter +const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor +const long r_i = 20000; //time to rotate clockwise +const long b_i = 30000; //time to move backwards +const long l_i = 40000; //time to move anticlockwise +const long s_i = 50000; +int DIRRstate = LOW; //initializing direction states +int DIRLstate = HIGH; + +int DIRL = 20; //defining left direction pin +int DIRR = 21; //defining right direction pin + +int pwmr = 5; //pin to control right wheel speed using pwm +int pwml = 9; //pin to control left wheel speed using pwm +//*******************************************************************// +//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// + +//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------// +#define PIN_SS 10 +#define PIN_MISO 12 +#define PIN_MOSI 11 +#define PIN_SCK 13 + + + +#define PIN_MOUSECAM_RESET 8 +#define PIN_MOUSECAM_CS 7 + +#define ADNS3080_PIXELS_X 30 +#define ADNS3080_PIXELS_Y 30 + +#define ADNS3080_PRODUCT_ID 0x00 +#define ADNS3080_REVISION_ID 0x01 +#define ADNS3080_MOTION 0x02 +#define ADNS3080_DELTA_X 0x03 +#define ADNS3080_DELTA_Y 0x04 +#define ADNS3080_SQUAL 0x05 +#define ADNS3080_PIXEL_SUM 0x06 +#define ADNS3080_MAXIMUM_PIXEL 0x07 +#define ADNS3080_CONFIGURATION_BITS 0x0a +#define ADNS3080_EXTENDED_CONFIG 0x0b +#define ADNS3080_DATA_OUT_LOWER 0x0c +#define ADNS3080_DATA_OUT_UPPER 0x0d +#define ADNS3080_SHUTTER_LOWER 0x0e +#define ADNS3080_SHUTTER_UPPER 0x0f +#define ADNS3080_FRAME_PERIOD_LOWER 0x10 +#define ADNS3080_FRAME_PERIOD_UPPER 0x11 +#define ADNS3080_MOTION_CLEAR 0x12 +#define ADNS3080_FRAME_CAPTURE 0x13 +#define ADNS3080_SROM_ENABLE 0x14 +#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19 +#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a +#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b +#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c +#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e +#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e +#define ADNS3080_SROM_ID 0x1f +#define ADNS3080_OBSERVATION 0x3d +#define ADNS3080_INVERSE_PRODUCT_ID 0x3f +#define ADNS3080_PIXEL_BURST 0x40 +#define ADNS3080_MOTION_BURST 0x50 +#define ADNS3080_SROM_LOAD 0x60 + +#define ADNS3080_PRODUCT_ID_VAL 0x17 + + + + +int total_x = 0; + +int total_y = 0; + + +int total_x1 = 0; + +int total_y1 = 0; + + +int x=0; +int y=0; + +int a=0; +int b=0; + + + + +int distance_x=0; +int distance_y=0; + + + +volatile byte movementflag=0; +volatile int xydat[2]; + + + +int convTwosComp(int b){ + //Convert from 2's complement + if(b & 0x80){ + b = -1 * ((b ^ 0xff) + 1); + } + return b; + } + + + + int tdistance = 0; + + +void mousecam_reset() +{ + digitalWrite(PIN_MOUSECAM_RESET,HIGH); + delay(1); // reset pulse >10us + digitalWrite(PIN_MOUSECAM_RESET,LOW); + delay(35); // 35ms from reset to functional +} + + +int mousecam_init() +{ + pinMode(PIN_MOUSECAM_RESET,OUTPUT); + pinMode(PIN_MOUSECAM_CS,OUTPUT); + + digitalWrite(PIN_MOUSECAM_CS,HIGH); + + mousecam_reset(); + + int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID); + if(pid != ADNS3080_PRODUCT_ID_VAL) + return -1; + + // turn on sensitive mode + mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19); + + return 0; +} + +void mousecam_write_reg(int reg, int val) +{ + digitalWrite(PIN_MOUSECAM_CS, LOW); + SPI.transfer(reg | 0x80); + SPI.transfer(val); + digitalWrite(PIN_MOUSECAM_CS,HIGH); + delayMicroseconds(50); +} + +int mousecam_read_reg(int reg) +{ + digitalWrite(PIN_MOUSECAM_CS, LOW); + SPI.transfer(reg); + delayMicroseconds(75); + int ret = SPI.transfer(0xff); + digitalWrite(PIN_MOUSECAM_CS,HIGH); + delayMicroseconds(1); + return ret; +} + +struct MD +{ + byte motion; + char dx, dy; + byte squal; + word shutter; + byte max_pix; +}; + + +void mousecam_read_motion(struct MD *p) +{ + digitalWrite(PIN_MOUSECAM_CS, LOW); + SPI.transfer(ADNS3080_MOTION_BURST); + delayMicroseconds(75); + p->motion = SPI.transfer(0xff); + p->dx = SPI.transfer(0xff); + p->dy = SPI.transfer(0xff); + p->squal = SPI.transfer(0xff); + p->shutter = SPI.transfer(0xff)<<8; + p->shutter |= SPI.transfer(0xff); + p->max_pix = SPI.transfer(0xff); + digitalWrite(PIN_MOUSECAM_CS,HIGH); + delayMicroseconds(5); +} + +// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y +// you must call mousecam_reset() after this if you want to go back to normal operation +int mousecam_frame_capture(byte *pdata) +{ + mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83); + + digitalWrite(PIN_MOUSECAM_CS, LOW); + + SPI.transfer(ADNS3080_PIXEL_BURST); + delayMicroseconds(50); + + int pix; + byte started = 0; + int count; + int timeout = 0; + int ret = 0; + for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; ) + { + pix = SPI.transfer(0xff); + delayMicroseconds(10); + if(started==0) + { + if(pix&0x40) + started = 1; + else + { + timeout++; + if(timeout==100) + { + ret = -1; + break; + } + } + } + if(started==1) + { + pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range + } + } + + digitalWrite(PIN_MOUSECAM_CS,HIGH); + delayMicroseconds(14); + + return ret; +} + +//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------// + +//Tracker Variables +int current_x = 0; +int current_y = 0; +int goal_x = 0; +int goal_y = 0; +int distanceGoal; +bool commandComplete = 1; +float powerUsage_mWh = 0; +int distTravelled_mm = 0; +bool initialAngleSet=false; + +//calibration varibles +int angularDrift = 0; //+ve to right, -ve to left +int leftStart = 80; //pwm min for left motor +int leftStop = 255; //pwm max for left motor +int rightStart = 80; //pwm min for right motor +int rightStop = 255; //pwm max for right motor + + +//Energy Usage Variables +unsigned long previousMillis_Energy = 0; // will store last time energy use was updated +const long interval_Energy = 1000; //energy usaged update frequency +float totalEnergyUsed = 0; +float powerUsed = 0; +int loopCount = 0; +float motorVoltage = 0; + +int getPWMfromSpeed(float speedr,bool left) { + if (speedr >= 1) { + return 512; + } + else if (speedr < 0) { + return 0; + } + else { + int speedpercentage = (speedr * 100); + if(left){ + return map(speedpercentage,0,100,leftStart,leftStop); + } + else{ + return map(speedpercentage,0,100,rightStart,rightStop); + } + } +} + +void setup() +{ + //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// + //************************** Motor Pins Defining **************************// + pinMode(DIRR, OUTPUT); + pinMode(DIRL, OUTPUT); + pinMode(pwmr, OUTPUT); + pinMode(pwml, OUTPUT); + digitalWrite(pwmr, HIGH); //setting right motor speed at maximum + digitalWrite(pwml, HIGH); //setting left motor speed at maximum + //*******************************************************************// + + //Basic pin setups + + noInterrupts(); //disable all interrupts + pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller + pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch + pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch + analogReference(EXTERNAL); // We are using an external analogue reference for the ADC + + // TimerA0 initialization for control-loop interrupt. + + TCA0.SINGLE.PER = 999; // + TCA0.SINGLE.CMP1 = 999; // + TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M. + TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm; + + // TimerB0 initialization for PWM output + + pinMode(6, OUTPUT); + TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz + analogWrite(6, 120); + + interrupts(); //enable interrupts. + Wire.begin(); // We need this for the i2c comms for the current sensor + ina219.init(); // this initiates the current sensor + Wire.setClock(700000); // set the comms speed for i2c + //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// + Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) + Serial1.begin(9600); // Set up hardware UART1 + + //Serial.println(getPWMfromSpeed(-1)); + //Serial.println(getPWMfromSpeed(256)); + //Serial.println(getPWMfromSpeed(0.5)); + // Other Drive setup stuff + /////////currentHeading = REQUEST HEADING HERE; + + analogWrite(pwmr,0); + analogWrite(pwml,0); + //digitalWrite(DIRR, LOW); + //digitalWrite(DIRL, HIGH); + + + + pinMode(PIN_SS,OUTPUT); + pinMode(PIN_MISO,INPUT); + pinMode(PIN_MOSI,OUTPUT); + pinMode(PIN_SCK,OUTPUT); + + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV32); + SPI.setDataMode(SPI_MODE3); + SPI.setBitOrder(MSBFIRST); + + Serial.begin(9600); + + if(mousecam_init()==-1) + { + Serial.println("Mouse cam failed to init"); + while(1); + } + + +} + +bool commandCompletionStatus = 1; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete +int requiredHeading = 0; + int distance = 0; + float spd = 0; + int currentHeading = 0; + //reset variables for update on completion + +unsigned long previousMillis_Command = 0; +const long interval_Command = 1000; +DeserializationError error; + +char asciiart(int k) +{ + static char foo[] = "WX86*3I>!;~:,`. "; + return foo[k>>4]; +} + +byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y]; + + +void loop() +{ + + analogWrite(pwmr,512); + analogWrite(pwml,512); + digitalWrite(DIRR, HIGH); + digitalWrite(DIRL, LOW); + Serial.println(pwmr); + + //Serial.println("Here"); + DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be + + //deserializeJson(rdoc, Serial1); // Take JSON input from UART1 + + //Check for updates every 1s... + unsigned long currentMillis_Command = millis(); + + if (currentMillis_Command - previousMillis_Command >= interval_Command) { + // save the last time you blinked the LED + previousMillis_Command = currentMillis_Command; + error = deserializeJson(rdoc, Serial1); + //Serial.println("H"); + } + + + // Test if parsing succeeds. + if (error) { + //Serial.print(F("deserializeJson() failed: ")); + //Serial.println(error.f_str()); + return; + } + else{ + //parsing success, prepare command and pull request information + commandCompletionStatus = 1; + requiredHeading = rdoc["rH"]; + distance = rdoc["dist"]; + spd = rdoc["sp"]; + currentHeading = rdoc["cH"]; + //reset variables for update on completion + commandComplete = 0; + powerUsage_mWh = 0.0; + distTravelled_mm = 0; + } + +//if(current_x!=goal_x) + + // Do Drive stuff, set the 5 values above + + if(commandCompletionStatus == 0){ //noCommand + //Do Nothing just wait + } + if(commandCompletionStatus == 1){ //newCommand + //set goals + goal_x += distance*sin(requiredHeading); + goal_y += distance*cos(requiredHeading); + total_y = 0; + total_x = 0; + commandCompletionStatus = 2; + + initialAngleSet=false; + } + if(commandCompletionStatus == 2){ //ongoingCommand + //start moving towards goal + + //set angle first + if(!initialAngleSet){ + //turn to angle + currentHeading = getCurrentHeading(); + if(currentHeadingrequiredHeading){ //turn left + analogWrite(pwmr,getPWMfromSpeed(spd,false)); + analogWrite(pwml,getPWMfromSpeed(spd,true)); + digitalWrite(DIRR, HIGH); + digitalWrite(DIRL, HIGH); + } + else{ + //heading correct therefore move to next step... + //STOP!!!! + digitalWrite(pwmr,LOW); + digitalWrite(pwml,LOW); + + initialAngleSet = true; + } + } + else{ //then move forwards but check angle for drift + if(total_x - distance > 0){ //go forwards + analogWrite(pwmr,getPWMfromSpeed(spd,false)); + analogWrite(pwml,getPWMfromSpeed(spd,true)); + digitalWrite(DIRR, HIGH); + digitalWrite(DIRL, LOW); + } + else if(total_x - distance < 0){//go backwards + analogWrite(pwmr,getPWMfromSpeed(spd,false)); + analogWrite(pwml,getPWMfromSpeed(spd,true)); + digitalWrite(DIRR, LOW); + digitalWrite(DIRL, HIGH); + } + else if((total_x == distance)) {//distance met + //STOP!!!!! + digitalWrite(pwmr,LOW); + digitalWrite(pwml,LOW); + commandCompletionStatus = 3; + initialAngleSet = true; + } + } + + } + if(commandCompletionStatus == 3){ //currentPosMatchesOrExceedsRequest + ///finish moving + + //send update via UART + + //prepare feedback variables + commandComplete = true; + current_x = goal_x; + current_y = goal_y; + distTravelled_mm += distance; + //compile energy use + unsigned long currentMillis_Energy = millis(); + + totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60); + previousMillis_Energy = currentMillis_Energy; + if (debug) { + Serial.print(motorVoltage); + Serial.print("Energy Used: "); + Serial.print(totalEnergyUsed); + Serial.println("mWh"); + } + + loopCount = 0; //reset counter to zero + powerUsed = 0;//reset power usage + powerUsage_mWh = totalEnergyUsed; + totalEnergyUsed = 0; + + DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be + tdoc["comp"] = commandComplete; + 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 + commandCompletionStatus = 0; + + } + + //Handle power usage + //find motor voltage + //int motorVSensor = analogRead(A5); + //float motorVoltage = motorVSensor * (5.0 / 1023.0); + float motorVoltage = 5; + + //find average power + + if(current_mA >=0){ + powerUsed += current_mA * motorVoltage; + } + Serial.println(powerUsed); + + //calculate averages for energy use calculations + loopCount += 1; //handle loop quantity for averaging + + //find average current + //find average voltage + + //update command/control + + + if(movementflag){ + + tdistance = tdistance + convTwosComp(xydat[0]); + Serial.println("Distance = " + String(tdistance)); + movementflag=0; + delay(3); + } + int val = mousecam_read_reg(ADNS3080_PIXEL_SUM); + MD md; + mousecam_read_motion(&md); + for(int i=0; i +SoftwareSerial Serial4 = SoftwareSerial(6, 10); #define RXpin 0 // Define your RX pin here #define TXpin 0 // Define your TX pin here @@ -360,7 +362,7 @@ void setup() Wire.setClock(700000); // set the comms speed for i2c //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) - Serial1.begin(9600); // Set up hardware UART1 + Serial4.begin(9600); // Set up software UART //Serial.println(getPWMfromSpeed(-1)); //Serial.println(getPWMfromSpeed(256)); @@ -414,20 +416,12 @@ char asciiart(int k) } byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y]; - + DynamicJsonDocument rdoc(1024); void loop() { - - analogWrite(pwmr,512); - analogWrite(pwml,512); - digitalWrite(DIRR, HIGH); - digitalWrite(DIRL, LOW); - Serial.println(pwmr); - //Serial.println("Here"); - DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be - + //Serial.println("Here"); //deserializeJson(rdoc, Serial1); // Take JSON input from UART1 //Check for updates every 1s... @@ -436,8 +430,11 @@ void loop() if (currentMillis_Command - previousMillis_Command >= interval_Command) { // save the last time you blinked the LED previousMillis_Command = currentMillis_Command; - error = deserializeJson(rdoc, Serial1); - //Serial.println("H"); + // receive doc, not sure how big this needs to be + error = deserializeJson(rdoc, Serial4); + + //Serial.println + Serial.println("I am here!"); } @@ -682,7 +679,7 @@ Serial.println("Distance_y = " + String(total_y)); //*******************************************************************// - //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// + //----------------s---------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// } @@ -795,4 +792,4 @@ float pidi(float pid_input) { e2i = e1i; //update last last time's error e1i = e0i; // update last time's error return u0i; -} +} \ No newline at end of file From f40452c255a8f210b4dc2f66bdd485c3bb8d16d9 Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Thu, 10 Jun 2021 14:40:00 +0100 Subject: [PATCH 03/17] Integrated Command and Drive --- Control/platformio.ini | 6 +- Control/ref/drive.cpp | 6 +- Control/src/main.cpp | 26 +- Drive/.gitignore | 5 + Drive/.vscode/extensions.json | 7 + Drive/20210706_Drive_Mars_Rover.ino | 795 --------------------------- Drive/include/README | 39 ++ Drive/lib/README | 46 ++ Drive/platformio.ini | 21 + Drive/src/main.cpp | 805 ++++++++++++++++++++++++++++ Drive/test/README | 11 + 11 files changed, 962 insertions(+), 805 deletions(-) create mode 100644 Drive/.gitignore create mode 100644 Drive/.vscode/extensions.json delete mode 100644 Drive/20210706_Drive_Mars_Rover.ino create mode 100644 Drive/include/README create mode 100644 Drive/lib/README create mode 100644 Drive/platformio.ini create mode 100644 Drive/src/main.cpp create mode 100644 Drive/test/README diff --git a/Control/platformio.ini b/Control/platformio.ini index 0f97db2..a198800 100644 --- a/Control/platformio.ini +++ b/Control/platformio.ini @@ -13,11 +13,11 @@ platform = espressif32 board = esp32dev framework = arduino monitor_speed = 115200 -upload_port = COM[3] -monitor_filters = +upload_port = COM13 +monitor_filters = send_on_enter esp32_exception_decoder -build_flags = +build_flags = -DCORE_DEBUG_LEVEL=5 -Wno-unknown-pragmas build_type = debug diff --git a/Control/ref/drive.cpp b/Control/ref/drive.cpp index 8a59cdc..324831d 100644 --- a/Control/ref/drive.cpp +++ b/Control/ref/drive.cpp @@ -19,11 +19,11 @@ void loop() 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"]; + float spd = rdoc["sp"]; int currentHeading = rdoc["cH"]; bool commandComplete = 0; - float powerUsage_mW = 0.0; + float powerUsage_mWh = 0.0; int distTravelled_mm = 0; int current_x = 0; int current_y = 0; @@ -32,7 +32,7 @@ void loop() 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["mW"] = powerUsage_mW; + tdoc["mWh"] = powerUsage_mWh; tdoc["mm"] = distTravelled_mm; tdoc["pos"][0] = current_x; tdoc["pos"][1] = current_y; diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 245331e..b8fbaa7 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -1,6 +1,6 @@ #include #include -// #include Software Serial not currently needed +#include // Software Serial not currently needed #include #include #include @@ -41,7 +41,8 @@ void setup() 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) + Serial2.begin(9600, SERIAL_8N1, 16, 17); // RX 9, TX 8 + // Set up remaining communication ports here (Energy, Vision) if (!SPIFFS.begin(true)) // Mount SPIFFS { @@ -111,7 +112,7 @@ void returnSensorData() battery_voltage = 4; } String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}"; - Serial.println(JSON_Data); + // Serial.println(JSON_Data); websocketserver.broadcastTXT(JSON_Data); } @@ -137,7 +138,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) break; case WStype_TEXT: { - Serial.printf("Client[%u] sent Text: %s\n", num, payload); + // Serial.printf("Client[%u] sent Text: %s\n", num, payload); String command = String((char *)(payload)); DynamicJsonDocument doc(200); //creating an instance of a DynamicJsonDocument allocating 200bytes on the heap. @@ -155,6 +156,23 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) int MVM_B_status = doc["MVM_B"]; Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>'); + + Serial.println("UpArrow=" + String(MVM_F_status)); + + if (MVM_F_status == 1){ + DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be + tdoc["rH"] = 0; + tdoc["dist"] = 100; + tdoc["sp"] = 1; + serializeJson(tdoc, Serial2); // Build JSON and send on UART1 + } + if (MVM_B_status == 1){ + DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be + tdoc["rH"] = 0; + tdoc["dist"] = -100; + tdoc["sp"] = 1; + serializeJson(tdoc, Serial2); // Build JSON and send on UART1 + } } break; case WStype_PONG: diff --git a/Drive/.gitignore b/Drive/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Drive/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Drive/.vscode/extensions.json b/Drive/.vscode/extensions.json new file mode 100644 index 0000000..e80666b --- /dev/null +++ b/Drive/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/Drive/20210706_Drive_Mars_Rover.ino b/Drive/20210706_Drive_Mars_Rover.ino deleted file mode 100644 index 2c27fef..0000000 --- a/Drive/20210706_Drive_Mars_Rover.ino +++ /dev/null @@ -1,795 +0,0 @@ -#include -#include -#include -#include -#include -#include "SPI.h" -#include -SoftwareSerial Serial4 = SoftwareSerial(6, 10); - -#define RXpin 0 // Define your RX pin here -#define TXpin 0 // Define your TX pin here - -bool debug = true; - -//TO IMPLEMENT -//DONE 2 way serial -//DONE F<>,B<>,S,L<>,R<>,p<0--1023> -//DONE Obtain current and power usage, get voltage from analog pin -//request angle facing -//DONE speed control 0-1 -//speed calibration, 0 stop and max speed to match -//distance travveled and x and y at request - - -//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// -INA219_WE ina219; // this is the instantiation of the library for the current sensor - -float open_loop, closed_loop; // Duty Cycles -float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables -unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration -float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals -float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period. -float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid. -float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time -float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid. -float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller -float uv_max = 4, uv_min = 0; //anti-windup limitation -float ui_max = 1, ui_min = 0; //anti-windup limitation -float current_limit = 1.0; -boolean Boost_mode = 0; -boolean CL_mode = 0; - - -unsigned int loopTrigger; -unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging. - - -//************************** Motor Constants **************************// -unsigned long previousMillis = 0; //initializing time counter -const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor -const long r_i = 20000; //time to rotate clockwise -const long b_i = 30000; //time to move backwards -const long l_i = 40000; //time to move anticlockwise -const long s_i = 50000; -int DIRRstate = LOW; //initializing direction states -int DIRLstate = HIGH; - -int DIRL = 20; //defining left direction pin -int DIRR = 21; //defining right direction pin - -int pwmr = 5; //pin to control right wheel speed using pwm -int pwml = 9; //pin to control left wheel speed using pwm -//*******************************************************************// -//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// - -//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------// -#define PIN_SS 10 -#define PIN_MISO 12 -#define PIN_MOSI 11 -#define PIN_SCK 13 - - - -#define PIN_MOUSECAM_RESET 8 -#define PIN_MOUSECAM_CS 7 - -#define ADNS3080_PIXELS_X 30 -#define ADNS3080_PIXELS_Y 30 - -#define ADNS3080_PRODUCT_ID 0x00 -#define ADNS3080_REVISION_ID 0x01 -#define ADNS3080_MOTION 0x02 -#define ADNS3080_DELTA_X 0x03 -#define ADNS3080_DELTA_Y 0x04 -#define ADNS3080_SQUAL 0x05 -#define ADNS3080_PIXEL_SUM 0x06 -#define ADNS3080_MAXIMUM_PIXEL 0x07 -#define ADNS3080_CONFIGURATION_BITS 0x0a -#define ADNS3080_EXTENDED_CONFIG 0x0b -#define ADNS3080_DATA_OUT_LOWER 0x0c -#define ADNS3080_DATA_OUT_UPPER 0x0d -#define ADNS3080_SHUTTER_LOWER 0x0e -#define ADNS3080_SHUTTER_UPPER 0x0f -#define ADNS3080_FRAME_PERIOD_LOWER 0x10 -#define ADNS3080_FRAME_PERIOD_UPPER 0x11 -#define ADNS3080_MOTION_CLEAR 0x12 -#define ADNS3080_FRAME_CAPTURE 0x13 -#define ADNS3080_SROM_ENABLE 0x14 -#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19 -#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a -#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b -#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c -#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e -#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e -#define ADNS3080_SROM_ID 0x1f -#define ADNS3080_OBSERVATION 0x3d -#define ADNS3080_INVERSE_PRODUCT_ID 0x3f -#define ADNS3080_PIXEL_BURST 0x40 -#define ADNS3080_MOTION_BURST 0x50 -#define ADNS3080_SROM_LOAD 0x60 - -#define ADNS3080_PRODUCT_ID_VAL 0x17 - - - - -int total_x = 0; - -int total_y = 0; - - -int total_x1 = 0; - -int total_y1 = 0; - - -int x=0; -int y=0; - -int a=0; -int b=0; - - - - -int distance_x=0; -int distance_y=0; - - - -volatile byte movementflag=0; -volatile int xydat[2]; - - - -int convTwosComp(int b){ - //Convert from 2's complement - if(b & 0x80){ - b = -1 * ((b ^ 0xff) + 1); - } - return b; - } - - - - int tdistance = 0; - - -void mousecam_reset() -{ - digitalWrite(PIN_MOUSECAM_RESET,HIGH); - delay(1); // reset pulse >10us - digitalWrite(PIN_MOUSECAM_RESET,LOW); - delay(35); // 35ms from reset to functional -} - - -int mousecam_init() -{ - pinMode(PIN_MOUSECAM_RESET,OUTPUT); - pinMode(PIN_MOUSECAM_CS,OUTPUT); - - digitalWrite(PIN_MOUSECAM_CS,HIGH); - - mousecam_reset(); - - int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID); - if(pid != ADNS3080_PRODUCT_ID_VAL) - return -1; - - // turn on sensitive mode - mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19); - - return 0; -} - -void mousecam_write_reg(int reg, int val) -{ - digitalWrite(PIN_MOUSECAM_CS, LOW); - SPI.transfer(reg | 0x80); - SPI.transfer(val); - digitalWrite(PIN_MOUSECAM_CS,HIGH); - delayMicroseconds(50); -} - -int mousecam_read_reg(int reg) -{ - digitalWrite(PIN_MOUSECAM_CS, LOW); - SPI.transfer(reg); - delayMicroseconds(75); - int ret = SPI.transfer(0xff); - digitalWrite(PIN_MOUSECAM_CS,HIGH); - delayMicroseconds(1); - return ret; -} - -struct MD -{ - byte motion; - char dx, dy; - byte squal; - word shutter; - byte max_pix; -}; - - -void mousecam_read_motion(struct MD *p) -{ - digitalWrite(PIN_MOUSECAM_CS, LOW); - SPI.transfer(ADNS3080_MOTION_BURST); - delayMicroseconds(75); - p->motion = SPI.transfer(0xff); - p->dx = SPI.transfer(0xff); - p->dy = SPI.transfer(0xff); - p->squal = SPI.transfer(0xff); - p->shutter = SPI.transfer(0xff)<<8; - p->shutter |= SPI.transfer(0xff); - p->max_pix = SPI.transfer(0xff); - digitalWrite(PIN_MOUSECAM_CS,HIGH); - delayMicroseconds(5); -} - -// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y -// you must call mousecam_reset() after this if you want to go back to normal operation -int mousecam_frame_capture(byte *pdata) -{ - mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83); - - digitalWrite(PIN_MOUSECAM_CS, LOW); - - SPI.transfer(ADNS3080_PIXEL_BURST); - delayMicroseconds(50); - - int pix; - byte started = 0; - int count; - int timeout = 0; - int ret = 0; - for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; ) - { - pix = SPI.transfer(0xff); - delayMicroseconds(10); - if(started==0) - { - if(pix&0x40) - started = 1; - else - { - timeout++; - if(timeout==100) - { - ret = -1; - break; - } - } - } - if(started==1) - { - pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range - } - } - - digitalWrite(PIN_MOUSECAM_CS,HIGH); - delayMicroseconds(14); - - return ret; -} - -//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------// - -//Tracker Variables -int current_x = 0; -int current_y = 0; -int goal_x = 0; -int goal_y = 0; -int distanceGoal; -bool commandComplete = 1; -float powerUsage_mWh = 0; -int distTravelled_mm = 0; -bool initialAngleSet=false; - -//calibration varibles -int angularDrift = 0; //+ve to right, -ve to left -int leftStart = 80; //pwm min for left motor -int leftStop = 255; //pwm max for left motor -int rightStart = 80; //pwm min for right motor -int rightStop = 255; //pwm max for right motor - - -//Energy Usage Variables -unsigned long previousMillis_Energy = 0; // will store last time energy use was updated -const long interval_Energy = 1000; //energy usaged update frequency -float totalEnergyUsed = 0; -float powerUsed = 0; -int loopCount = 0; -float motorVoltage = 0; - -int getPWMfromSpeed(float speedr,bool left) { - if (speedr >= 1) { - return 512; - } - else if (speedr < 0) { - return 0; - } - else { - int speedpercentage = (speedr * 100); - if(left){ - return map(speedpercentage,0,100,leftStart,leftStop); - } - else{ - return map(speedpercentage,0,100,rightStart,rightStop); - } - } -} - -void setup() -{ - //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// - //************************** Motor Pins Defining **************************// - pinMode(DIRR, OUTPUT); - pinMode(DIRL, OUTPUT); - pinMode(pwmr, OUTPUT); - pinMode(pwml, OUTPUT); - digitalWrite(pwmr, HIGH); //setting right motor speed at maximum - digitalWrite(pwml, HIGH); //setting left motor speed at maximum - //*******************************************************************// - - //Basic pin setups - - noInterrupts(); //disable all interrupts - pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller - pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch - pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch - analogReference(EXTERNAL); // We are using an external analogue reference for the ADC - - // TimerA0 initialization for control-loop interrupt. - - TCA0.SINGLE.PER = 999; // - TCA0.SINGLE.CMP1 = 999; // - TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M. - TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm; - - // TimerB0 initialization for PWM output - - pinMode(6, OUTPUT); - TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz - analogWrite(6, 120); - - interrupts(); //enable interrupts. - Wire.begin(); // We need this for the i2c comms for the current sensor - ina219.init(); // this initiates the current sensor - Wire.setClock(700000); // set the comms speed for i2c - //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// - Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) - Serial4.begin(9600); // Set up software UART - - //Serial.println(getPWMfromSpeed(-1)); - //Serial.println(getPWMfromSpeed(256)); - //Serial.println(getPWMfromSpeed(0.5)); - // Other Drive setup stuff - /////////currentHeading = REQUEST HEADING HERE; - - analogWrite(pwmr,0); - analogWrite(pwml,0); - //digitalWrite(DIRR, LOW); - //digitalWrite(DIRL, HIGH); - - - - pinMode(PIN_SS,OUTPUT); - pinMode(PIN_MISO,INPUT); - pinMode(PIN_MOSI,OUTPUT); - pinMode(PIN_SCK,OUTPUT); - - SPI.begin(); - SPI.setClockDivider(SPI_CLOCK_DIV32); - SPI.setDataMode(SPI_MODE3); - SPI.setBitOrder(MSBFIRST); - - Serial.begin(9600); - - if(mousecam_init()==-1) - { - Serial.println("Mouse cam failed to init"); - while(1); - } - - -} - -bool commandCompletionStatus = 1; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete -int requiredHeading = 0; - int distance = 0; - float spd = 0; - int currentHeading = 0; - //reset variables for update on completion - -unsigned long previousMillis_Command = 0; -const long interval_Command = 1000; -DeserializationError error; - -char asciiart(int k) -{ - static char foo[] = "WX86*3I>!;~:,`. "; - return foo[k>>4]; -} - -byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y]; - DynamicJsonDocument rdoc(1024); - -void loop() -{ - - //Serial.println("Here"); - //deserializeJson(rdoc, Serial1); // Take JSON input from UART1 - - //Check for updates every 1s... - unsigned long currentMillis_Command = millis(); - - if (currentMillis_Command - previousMillis_Command >= interval_Command) { - // save the last time you blinked the LED - previousMillis_Command = currentMillis_Command; - // receive doc, not sure how big this needs to be - error = deserializeJson(rdoc, Serial4); - - //Serial.println - Serial.println("I am here!"); - } - - - // Test if parsing succeeds. - if (error) { - //Serial.print(F("deserializeJson() failed: ")); - //Serial.println(error.f_str()); - return; - } - else{ - //parsing success, prepare command and pull request information - commandCompletionStatus = 1; - requiredHeading = rdoc["rH"]; - distance = rdoc["dist"]; - spd = rdoc["sp"]; - currentHeading = rdoc["cH"]; - //reset variables for update on completion - commandComplete = 0; - powerUsage_mWh = 0.0; - distTravelled_mm = 0; - } - -//if(current_x!=goal_x) - - // Do Drive stuff, set the 5 values above - - if(commandCompletionStatus == 0){ //noCommand - //Do Nothing just wait - } - if(commandCompletionStatus == 1){ //newCommand - //set goals - goal_x += distance*sin(requiredHeading); - goal_y += distance*cos(requiredHeading); - total_y = 0; - total_x = 0; - commandCompletionStatus = 2; - - initialAngleSet=false; - } - if(commandCompletionStatus == 2){ //ongoingCommand - //start moving towards goal - - //set angle first - if(!initialAngleSet){ - //turn to angle - currentHeading = getCurrentHeading(); - if(currentHeadingrequiredHeading){ //turn left - analogWrite(pwmr,getPWMfromSpeed(spd,false)); - analogWrite(pwml,getPWMfromSpeed(spd,true)); - digitalWrite(DIRR, HIGH); - digitalWrite(DIRL, HIGH); - } - else{ - //heading correct therefore move to next step... - //STOP!!!! - digitalWrite(pwmr,LOW); - digitalWrite(pwml,LOW); - - initialAngleSet = true; - } - } - else{ //then move forwards but check angle for drift - if(total_x - distance > 0){ //go forwards - analogWrite(pwmr,getPWMfromSpeed(spd,false)); - analogWrite(pwml,getPWMfromSpeed(spd,true)); - digitalWrite(DIRR, HIGH); - digitalWrite(DIRL, LOW); - } - else if(total_x - distance < 0){//go backwards - analogWrite(pwmr,getPWMfromSpeed(spd,false)); - analogWrite(pwml,getPWMfromSpeed(spd,true)); - digitalWrite(DIRR, LOW); - digitalWrite(DIRL, HIGH); - } - else if((total_x == distance)) {//distance met - //STOP!!!!! - digitalWrite(pwmr,LOW); - digitalWrite(pwml,LOW); - commandCompletionStatus = 3; - initialAngleSet = true; - } - } - - } - if(commandCompletionStatus == 3){ //currentPosMatchesOrExceedsRequest - ///finish moving - - //send update via UART - - //prepare feedback variables - commandComplete = true; - current_x = goal_x; - current_y = goal_y; - distTravelled_mm += distance; - //compile energy use - unsigned long currentMillis_Energy = millis(); - - totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60); - previousMillis_Energy = currentMillis_Energy; - if (debug) { - Serial.print(motorVoltage); - Serial.print("Energy Used: "); - Serial.print(totalEnergyUsed); - Serial.println("mWh"); - } - - loopCount = 0; //reset counter to zero - powerUsed = 0;//reset power usage - powerUsage_mWh = totalEnergyUsed; - totalEnergyUsed = 0; - - DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be - tdoc["comp"] = commandComplete; - 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 - commandCompletionStatus = 0; - - } - - //Handle power usage - //find motor voltage - //int motorVSensor = analogRead(A5); - //float motorVoltage = motorVSensor * (5.0 / 1023.0); - float motorVoltage = 5; - - //find average power - - if(current_mA >=0){ - powerUsed += current_mA * motorVoltage; - } - Serial.println(powerUsed); - - //calculate averages for energy use calculations - loopCount += 1; //handle loop quantity for averaging - - //find average current - //find average voltage - - //update command/control - - - if(movementflag){ - - tdistance = tdistance + convTwosComp(xydat[0]); - Serial.println("Distance = " + String(tdistance)); - movementflag=0; - delay(3); - } - int val = mousecam_read_reg(ADNS3080_PIXEL_SUM); - MD md; - mousecam_read_motion(&md); - for(int i=0; i THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Drive/platformio.ini b/Drive/platformio.ini new file mode 100644 index 0000000..e30e663 --- /dev/null +++ b/Drive/platformio.ini @@ -0,0 +1,21 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env] +lib_deps = + bblanchon/ArduinoJson @ ^6.18.0 + wollewald/INA219_WE @ ^1.1.6 +monitor_speed = 115200 +upload_speed = 115200 + +[env:nano_every] +platform = atmelmegaavr +board = nano_every +framework = arduino diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp new file mode 100644 index 0000000..5d13435 --- /dev/null +++ b/Drive/src/main.cpp @@ -0,0 +1,805 @@ +#include +#include +// #include +#include +#include +#include "SPI.h" +#include + +// #define RXpin 4 // Define your RX pin here +// #define TXpin 13 // Define your TX pin here + +// SoftwareSerial mySerial(RXpin, TXpin); + +bool debug = false; + +//TO IMPLEMENT +//DONE 2 way serial +//DONE F<>,B<>,S,L<>,R<>,p<0--1023> +//DONE Obtain current and power usage, get voltage from analog pin +//request angle facing +//DONE speed control 0-1 +//speed calibration, 0 stop and max speed to match +//distance travveled and x and y at request + +//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// +INA219_WE ina219; // this is the instantiation of the library for the current sensor + +float open_loop, closed_loop; // Duty Cycles +float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables +unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration +float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals +float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period. +float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid. +float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time +float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid. +float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller +float uv_max = 4, uv_min = 0; //anti-windup limitation +float ui_max = 1, ui_min = 0; //anti-windup limitation +float current_limit = 1.0; +boolean Boost_mode = 0; +boolean CL_mode = 0; + +unsigned int loopTrigger; +unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging. + +//************************** Motor Constants **************************// +unsigned long previousMillis = 0; //initializing time counter +const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor +const long r_i = 20000; //time to rotate clockwise +const long b_i = 30000; //time to move backwards +const long l_i = 40000; //time to move anticlockwise +const long s_i = 50000; +int DIRRstate = LOW; //initializing direction states +int DIRLstate = HIGH; + +int DIRL = 20; //defining left direction pin +int DIRR = 21; //defining right direction pin + +int pwmr = 5; //pin to control right wheel speed using pwm +int pwml = 9; //pin to control left wheel speed using pwm +//*******************************************************************// +//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// + +//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------// +#define PIN_SS 10 +#define PIN_MISO 12 +#define PIN_MOSI 11 +#define PIN_SCK 13 + +#define PIN_MOUSECAM_RESET 8 +#define PIN_MOUSECAM_CS 7 + +#define ADNS3080_PIXELS_X 30 +#define ADNS3080_PIXELS_Y 30 + +#define ADNS3080_PRODUCT_ID 0x00 +#define ADNS3080_REVISION_ID 0x01 +#define ADNS3080_MOTION 0x02 +#define ADNS3080_DELTA_X 0x03 +#define ADNS3080_DELTA_Y 0x04 +#define ADNS3080_SQUAL 0x05 +#define ADNS3080_PIXEL_SUM 0x06 +#define ADNS3080_MAXIMUM_PIXEL 0x07 +#define ADNS3080_CONFIGURATION_BITS 0x0a +#define ADNS3080_EXTENDED_CONFIG 0x0b +#define ADNS3080_DATA_OUT_LOWER 0x0c +#define ADNS3080_DATA_OUT_UPPER 0x0d +#define ADNS3080_SHUTTER_LOWER 0x0e +#define ADNS3080_SHUTTER_UPPER 0x0f +#define ADNS3080_FRAME_PERIOD_LOWER 0x10 +#define ADNS3080_FRAME_PERIOD_UPPER 0x11 +#define ADNS3080_MOTION_CLEAR 0x12 +#define ADNS3080_FRAME_CAPTURE 0x13 +#define ADNS3080_SROM_ENABLE 0x14 +#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19 +#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a +#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b +#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c +#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e +#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e +#define ADNS3080_SROM_ID 0x1f +#define ADNS3080_OBSERVATION 0x3d +#define ADNS3080_INVERSE_PRODUCT_ID 0x3f +#define ADNS3080_PIXEL_BURST 0x40 +#define ADNS3080_MOTION_BURST 0x50 +#define ADNS3080_SROM_LOAD 0x60 + +#define ADNS3080_PRODUCT_ID_VAL 0x17 + +int total_x = 0; +int total_y = 0; + +int total_x1 = 0; +int total_y1 = 0; + +int x = 0; +int y = 0; + +int a = 0; +int b = 0; + +int distance_x = 0; +int distance_y = 0; + +volatile byte movementflag = 0; +volatile int xydat[2]; + +// FUNCTION DELCARATIONS // + +float pidi(float pid_input); +float pidv(float pid_input); +void pwm_modulate(float pwm_input); +float saturation(float sat_input, float uplim, float lowlim); +void sampling(); +void mousecam_write_reg(int reg, int val); +int mousecam_read_reg(int reg); +void mousecam_reset(); +int getCurrentHeading(); + +int convTwosComp(int b) +{ + //Convert from 2's complement + if (b & 0x80) + { + b = -1 * ((b ^ 0xff) + 1); + } + return b; +} + +int tdistance = 0; + +void mousecam_reset(){ + digitalWrite(PIN_MOUSECAM_RESET, HIGH); + delay(1); // reset pulse >10us + digitalWrite(PIN_MOUSECAM_RESET, LOW); + delay(35); // 35ms from reset to functional +} + +int mousecam_init(){ + pinMode(PIN_MOUSECAM_RESET, OUTPUT); + pinMode(PIN_MOUSECAM_CS, OUTPUT); + + digitalWrite(PIN_MOUSECAM_CS, HIGH); + + mousecam_reset(); + + int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID); + if (pid != ADNS3080_PRODUCT_ID_VAL) + return -1; + + // turn on sensitive mode + mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19); + + return 0; +} + +void mousecam_write_reg(int reg, int val){ + digitalWrite(PIN_MOUSECAM_CS, LOW); + SPI.transfer(reg | 0x80); + SPI.transfer(val); + digitalWrite(PIN_MOUSECAM_CS, HIGH); + delayMicroseconds(50); +} + +int mousecam_read_reg(int reg){ + digitalWrite(PIN_MOUSECAM_CS, LOW); + SPI.transfer(reg); + delayMicroseconds(75); + int ret = SPI.transfer(0xff); + digitalWrite(PIN_MOUSECAM_CS, HIGH); + delayMicroseconds(1); + return ret; +} + +struct MD{ + byte motion; + char dx, dy; + byte squal; + word shutter; + byte max_pix; +}; + +void mousecam_read_motion(struct MD *p){ + digitalWrite(PIN_MOUSECAM_CS, LOW); + SPI.transfer(ADNS3080_MOTION_BURST); + delayMicroseconds(75); + p->motion = SPI.transfer(0xff); + p->dx = SPI.transfer(0xff); + p->dy = SPI.transfer(0xff); + p->squal = SPI.transfer(0xff); + p->shutter = SPI.transfer(0xff) << 8; + p->shutter |= SPI.transfer(0xff); + p->max_pix = SPI.transfer(0xff); + digitalWrite(PIN_MOUSECAM_CS, HIGH); + delayMicroseconds(5); +} + +// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y +// you must call mousecam_reset() after this if you want to go back to normal operation +int mousecam_frame_capture(byte *pdata) +{ + mousecam_write_reg(ADNS3080_FRAME_CAPTURE, 0x83); + + digitalWrite(PIN_MOUSECAM_CS, LOW); + + SPI.transfer(ADNS3080_PIXEL_BURST); + delayMicroseconds(50); + + int pix; + byte started = 0; + int count; + int timeout = 0; + int ret = 0; + for (count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y;) + { + pix = SPI.transfer(0xff); + delayMicroseconds(10); + if (started == 0) + { + if (pix & 0x40) + started = 1; + else + { + timeout++; + if (timeout == 100) + { + ret = -1; + break; + } + } + } + if (started == 1) + { + pdata[count++] = (pix & 0x3f) << 2; // scale to normal grayscale byte range + } + } + + digitalWrite(PIN_MOUSECAM_CS, HIGH); + delayMicroseconds(14); + + return ret; +} + +//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------// + +//Tracker Variables +int current_x = 0; +int current_y = 0; +int goal_x = 0; +int goal_y = 0; +int distanceGoal; +bool commandComplete = 1; +float powerUsage_mWh = 0; +int distTravelled_mm = 0; +bool initialAngleSet = false; + +//calibration varibles +int angularDrift = 0; //+ve to right, -ve to left +int leftStart = 80; //pwm min for left motor +int leftStop = 255; //pwm max for left motor +int rightStart = 80; //pwm min for right motor +int rightStop = 255; //pwm max for right motor + +//Energy Usage Variables +unsigned long previousMillis_Energy = 0; // will store last time energy use was updated +const long interval_Energy = 1000; //energy usaged update frequency +float totalEnergyUsed = 0; +float powerUsed = 0; +int loopCount = 0; +float motorVoltage = 0; + +int getPWMfromSpeed(float speedr, bool left) +{ + if (speedr >= 1) + { + return 512; + } + else if (speedr < 0) + { + return 0; + } + else + { + int speedpercentage = (speedr * 100); + if (left) + { + return map(speedpercentage, 0, 100, leftStart, leftStop); + } + else + { + return map(speedpercentage, 0, 100, rightStart, rightStop); + } + } +} + +void setup() +{ + //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// + //************************** Motor Pins Defining **************************// + pinMode(DIRR, OUTPUT); + pinMode(DIRL, OUTPUT); + pinMode(pwmr, OUTPUT); + pinMode(pwml, OUTPUT); + digitalWrite(pwmr, HIGH); //setting right motor speed at maximum + digitalWrite(pwml, HIGH); //setting left motor speed at maximum + //*******************************************************************// + + //Basic pin setups + + noInterrupts(); //disable all interrupts + pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller + pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch + pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch + analogReference(EXTERNAL); // We are using an external analogue reference for the ADC + + // TimerA0 initialization for control-loop interrupt. + + TCA0.SINGLE.PER = 999; // + TCA0.SINGLE.CMP1 = 999; // + TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M. + TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm; + + // TimerB0 initialization for PWM output + + pinMode(6, OUTPUT); + TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz + analogWrite(6, 120); + + interrupts(); //enable interrupts. + Wire.begin(); // We need this for the i2c comms for the current sensor + ina219.init(); // this initiates the current sensor + Wire.setClock(700000); // set the comms speed for i2c + //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------// + Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) + Serial1.begin(9600); // Set up hardware UART + + //Serial.println(getPWMfromSpeed(-1)); + //Serial.println(getPWMfromSpeed(256)); + //Serial.println(getPWMfromSpeed(0.5)); + // Other Drive setup stuff + /////////currentHeading = REQUEST HEADING HERE; + + analogWrite(pwmr, 0); + analogWrite(pwml, 0); + //digitalWrite(DIRR, LOW); + //digitalWrite(DIRL, HIGH); + + pinMode(PIN_SS, OUTPUT); + pinMode(PIN_MISO, INPUT); + pinMode(PIN_MOSI, OUTPUT); + pinMode(PIN_SCK, OUTPUT); + + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV32); + SPI.setDataMode(SPI_MODE3); + SPI.setBitOrder(MSBFIRST); + + if (mousecam_init() == -1) + { + Serial.println("Mouse cam failed to init"); + while (1) + ; + } +} + +int commandCompletionStatus = 0; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete +int requiredHeading = 0; +int distance = 0; +float spd = 0; +int currentHeading = 0; +//reset variables for update on completion + +unsigned long previousMillis_Command = 0; +const long interval_Command = 1000; +DeserializationError error; + +char asciiart(int k) +{ + static char foo[] = "WX86*3I>!;~:,`. "; + return foo[k >> 4]; +} + +byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y]; +DynamicJsonDocument rdoc(1024); + +void loop() +{ + if (Serial1.available() && (commandCompletionStatus == 0)){ + // receive doc, not sure how big this needs to be + error = deserializeJson(rdoc, Serial1); + + Serial.println("Got serial"); + + // Test if parsing succeeds. + if (error) + { + //Serial.print(F("deserializeJson() failed: ")); + //Serial.println(error.f_str()); + return; + } + else + { + //parsing success, prepare command and pull request information + commandCompletionStatus = 1; + requiredHeading = rdoc["rH"]; + distance = rdoc["dist"]; + spd = rdoc["sp"]; + currentHeading = rdoc["cH"]; + + Serial.println("rH = " + String(requiredHeading) + " dist = " + String(distance) + " speed = " + String(spd)); + + //reset variables for update on completion + commandComplete = 0; + powerUsage_mWh = 0.0; + distTravelled_mm = 0; + } + } + + //if(current_x!=goal_x) + + // Do Drive stuff, set the 5 values above + + if (commandCompletionStatus == 0) + { //noCommand + //Do Nothing just wait + //Serial.println("status0"); + } + if (commandCompletionStatus == 1) + { Serial.println("status1"); + //newCommand + //set goals + goal_x += distance * sin(requiredHeading); + goal_y += distance * cos(requiredHeading); + total_y = 0; + total_x = 0; + commandCompletionStatus = 2; + + initialAngleSet = false; + } + if (commandCompletionStatus == 2) + { //Serial.println("status2"); + //ongoingCommand + //start moving towards goal + + //set angle first + if (!initialAngleSet) + { + //turn to angle + currentHeading = getCurrentHeading(); + if (currentHeading < requiredHeading) + { //turn right + Serial.println("turning right"); + analogWrite(pwmr, getPWMfromSpeed(spd, false)); + analogWrite(pwml, getPWMfromSpeed(spd, true)); + digitalWrite(DIRR, LOW); + digitalWrite(DIRL, LOW); + } + else if (currentHeading > requiredHeading) + { //turn left + Serial.println("turning left"); + analogWrite(pwmr, getPWMfromSpeed(spd, false)); + analogWrite(pwml, getPWMfromSpeed(spd, true)); + digitalWrite(DIRR, HIGH); + digitalWrite(DIRL, HIGH); + } + else + { + //heading correct therefore move to next step... + //STOP!!!! + digitalWrite(pwmr, LOW); + digitalWrite(pwml, LOW); + + initialAngleSet = true; + } + } + else + { //then move forwards but check angle for drift + if (total_x - distance < 0) + { //go forwards + Serial.println("going forwards"); + analogWrite(pwmr, getPWMfromSpeed(spd, false)); + analogWrite(pwml, getPWMfromSpeed(spd, true)); + digitalWrite(DIRR, LOW); + digitalWrite(DIRL, HIGH); + } + else if (total_x - distance > 0) + { //go backwards + Serial.println("going backwards"); + analogWrite(pwmr, getPWMfromSpeed(spd, false)); + analogWrite(pwml, getPWMfromSpeed(spd, true)); + digitalWrite(DIRR, HIGH); + digitalWrite(DIRL, LOW); + } + else if ((total_x == distance)) + { //distance met + //STOP!!!!! + digitalWrite(pwmr, LOW); + digitalWrite(pwml, LOW); + commandCompletionStatus = 3; + initialAngleSet = true; + } + } + } + if (commandCompletionStatus == 3) + { Serial.println("status3"); + //currentPosMatchesOrExceedsRequest + ///finish moving + + //send update via UART + + //prepare feedback variables + commandComplete = true; + current_x = goal_x; + current_y = goal_y; + distTravelled_mm += distance; + + //compile energy use + unsigned long currentMillis_Energy = millis(); + + totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60); + previousMillis_Energy = currentMillis_Energy; + + if (debug){ + Serial.print(motorVoltage); + Serial.print("Energy Used: "); + Serial.print(totalEnergyUsed); + Serial.println("mWh"); + } + + loopCount = 0; //reset counter to zero + powerUsed = 0; //reset power usage + powerUsage_mWh = totalEnergyUsed; + totalEnergyUsed = 0; + total_x1 = 0; + total_y1 = 0; + + DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be + tdoc["comp"] = commandComplete; + tdoc["mWh"] = powerUsage_mWh; + tdoc["mm"] = distTravelled_mm; + tdoc["pos"][0] = current_x; + tdoc["pos"][1] = current_y; + serializeJson(tdoc, Serial); // Build JSON and send on UART1 + commandCompletionStatus = 0; + } + + //Handle power usage + //find motor voltage + //int motorVSensor = analogRead(A5); + //float motorVoltage = motorVSensor * (5.0 / 1023.0); + float motorVoltage = 5; + + //find average power + + if (current_mA >= 0){ + powerUsed += current_mA * motorVoltage; + } + if (debug){ + Serial.println(powerUsed); + } + + //calculate averages for energy use calculations + loopCount += 1; //handle loop quantity for averaging + + //find average current + //find average voltage + + //update command/control + + if (movementflag){ + + tdistance = tdistance + convTwosComp(xydat[0]); + // Serial.println("Distance = " + String(tdistance)); + movementflag = 0; + delay(3); + } + int val = mousecam_read_reg(ADNS3080_PIXEL_SUM); + MD md; + mousecam_read_motion(&md); + /* for (int i = 0; i < md.squal / 4; i++) + Serial.print('*'); + Serial.print(' '); + Serial.print((val * 100) / 351); + Serial.print(' '); + Serial.print(md.shutter); + Serial.print(" ("); + Serial.print((int)md.dx); + Serial.print(','); + Serial.print((int)md.dy); + Serial.println(')'); */ + + // Serial.println(md.max_pix); + delay(100); + + distance_x = md.dx; //convTwosComp(md.dx); + distance_y = md.dy; //convTwosComp(md.dy); + + total_x1 = (total_x1 + distance_x); + total_y1 = (total_y1 + distance_y); + + total_y = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch) + total_x = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch) + + /* Serial.print('\n'); + + Serial.println("Distance_x = " + String(total_x)); + + Serial.println("Distance_y = " + String(total_y)); + Serial.print('\n'); */ + //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// + unsigned long currentMillis = millis(); + if (loopTrigger) + { // This loop is triggered, it wont run unless there is an interrupt + + digitalWrite(13, HIGH); // set pin 13. Pin13 shows the time consumed by each control cycle. It's used for debugging. + + // Sample all of the measurements and check which control mode we are in + sampling(); + CL_mode = digitalRead(3); // input from the OL_CL switch + Boost_mode = digitalRead(2); // input from the Buck_Boost switch + + if (Boost_mode) + { + if (CL_mode) + { //Closed Loop Boost + pwm_modulate(1); // This disables the Boost as we are not using this mode + } + else + { // Open Loop Boost + pwm_modulate(1); // This disables the Boost as we are not using this mode + } + } + else + { + if (CL_mode) + { // Closed Loop Buck + // The closed loop path has a voltage controller cascaded with a current controller. The voltage controller + // creates a current demand based upon the voltage error. This demand is saturated to give current limiting. + // The current loop then gives a duty cycle demand based upon the error between demanded current and measured + // current + current_limit = 3; // Buck has a higher current limit + ev = vref - vb; //voltage error at this time + cv = pidv(ev); //voltage pid + cv = saturation(cv, current_limit, 0); //current demand saturation + ei = cv - iL; //current error + closed_loop = pidi(ei); //current pid + closed_loop = saturation(closed_loop, 0.99, 0.01); //duty_cycle saturation + pwm_modulate(closed_loop); //pwm modulation + } + else + { // Open Loop Buck + current_limit = 3; // Buck has a higher current limit + oc = iL - current_limit; // Calculate the difference between current measurement and current limit + if (oc > 0) + { + open_loop = open_loop - 0.001; // We are above the current limit so less duty cycle + } + else + { + open_loop = open_loop + 0.001; // We are below the current limit so more duty cycle + } + open_loop = saturation(open_loop, dutyref, 0.02); // saturate the duty cycle at the reference or a min of 0.01 + pwm_modulate(open_loop); // and send it out + } + } + // closed loop control path + + digitalWrite(13, LOW); // reset pin13. + loopTrigger = 0; + } +} + +int getCurrentHeading(){ + int currentAngle = 0; //-------------___<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< + return currentAngle; +} + +// Timer A CMP1 interrupt. Every 800us the program enters this interrupt. +// This, clears the incoming interrupt flag and triggers the main loop. + +ISR(TCA0_CMP1_vect){ + TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag + loopTrigger = 1; +} + +// This subroutine processes all of the analogue samples, creating the required values for the main loop + +void sampling(){ + + // Make the initial sampling operations for the circuit measurements + + sensorValue0 = analogRead(A0); //sample Vb + sensorValue2 = analogRead(A2); //sample Vref + sensorValue3 = analogRead(A3); //sample Vpd + current_mA = ina219.getCurrent_mA(); // sample the inductor current (via the sensor chip) + + // Process the values so they are a bit more usable/readable + // The analogRead process gives a value between 0 and 1023 + // representing a voltage between 0 and the analogue reference which is 4.096V + + vb = sensorValue0 * (4.096 / 1023.0); // Convert the Vb sensor reading to volts + vref = sensorValue2 * (4.096 / 1023.0); // Convert the Vref sensor reading to volts + vpd = sensorValue3 * (4.096 / 1023.0); // Convert the Vpd sensor reading to volts + + // The inductor current is in mA from the sensor so we need to convert to amps. + // We want to treat it as an input current in the Boost, so its also inverted + // For open loop control the duty cycle reference is calculated from the sensor + // differently from the Vref, this time scaled between zero and 1. + // The boost duty cycle needs to be saturated with a 0.33 minimum to prevent high output voltages + + if (Boost_mode == 1){ + iL = -current_mA / 1000.0; + dutyref = saturation(sensorValue2 * (1.0 / 1023.0), 0.99, 0.33); + }else{ + iL = current_mA / 1000.0; + dutyref = sensorValue2 * (1.0 / 1023.0); + } +} + +float saturation(float sat_input, float uplim, float lowlim){ // Saturation function + if (sat_input > uplim) + sat_input = uplim; + else if (sat_input < lowlim) + sat_input = lowlim; + else + ; + return sat_input; +} + +void pwm_modulate(float pwm_input){ // PWM function + analogWrite(6, (int)(255 - pwm_input * 255)); +} + +// This is a PID controller for the voltage + +float pidv(float pid_input){ + float e_integration; + e0v = pid_input; + e_integration = e0v; + + //anti-windup, if last-time pid output reaches the limitation, this time there won't be any intergrations. + if (u1v >= uv_max){ + e_integration = 0; + }else if (u1v <= uv_min){ + e_integration = 0; + } + + delta_uv = kpv * (e0v - e1v) + kiv * Ts * e_integration + kdv / Ts * (e0v - 2 * e1v + e2v); //incremental PID programming avoids integrations.there is another PID program called positional PID. + u0v = u1v + delta_uv; //this time's control output + + //output limitation + saturation(u0v, uv_max, uv_min); + + u1v = u0v; //update last time's control output + e2v = e1v; //update last last time's error + e1v = e0v; // update last time's error + return u0v; +} + +// This is a PID controller for the current + +float pidi(float pid_input){ + float e_integration; + e0i = pid_input; + e_integration = e0i; + + //anti-windup + if (u1i >= ui_max){ + e_integration = 0; + } + else if (u1i <= ui_min){ + e_integration = 0; + } + + delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations. + u0i = u1i + delta_ui; //this time's control output + + //output limitation + saturation(u0i, ui_max, ui_min); + + u1i = u0i; //update last time's control output + e2i = e1i; //update last last time's error + e1i = e0i; // update last time's error + return u0i; +} \ No newline at end of file diff --git a/Drive/test/README b/Drive/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/Drive/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html From bb33d95697298c2b02a773fed9ef8d2863f66db3 Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Thu, 10 Jun 2021 17:34:22 +0100 Subject: [PATCH 04/17] Implemented PID control for forward and backward motion --- Drive/src/main.cpp | 34 ++++++++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp index 5d13435..1688916 100644 --- a/Drive/src/main.cpp +++ b/Drive/src/main.cpp @@ -122,6 +122,14 @@ int b = 0; int distance_x = 0; int distance_y = 0; +int dist_to_move_prev_fl = 0; +int dist_to_move_prev_fr = 0; +unsigned long time_pid_prev_fl = 0; +unsigned long time_pid_prev_fr = 0; + +float kpdrive = 0.055; +float kddrive = 4.700; + volatile byte movementflag = 0; volatile int xydat[2]; @@ -136,6 +144,7 @@ void mousecam_write_reg(int reg, int val); int mousecam_read_reg(int reg); void mousecam_reset(); int getCurrentHeading(); +float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds); int convTwosComp(int b) { @@ -498,16 +507,16 @@ void loop() if (total_x - distance < 0) { //go forwards Serial.println("going forwards"); - analogWrite(pwmr, getPWMfromSpeed(spd, false)); - analogWrite(pwml, getPWMfromSpeed(spd, true)); + analogWrite(pwmr, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive), false)); + analogWrite(pwml, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive), true)); digitalWrite(DIRR, LOW); digitalWrite(DIRL, HIGH); } else if (total_x - distance > 0) { //go backwards Serial.println("going backwards"); - analogWrite(pwmr, getPWMfromSpeed(spd, false)); - analogWrite(pwml, getPWMfromSpeed(spd, true)); + analogWrite(pwmr, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive), false)); + analogWrite(pwml, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive), true)); digitalWrite(DIRR, HIGH); digitalWrite(DIRL, LOW); } @@ -802,4 +811,21 @@ float pidi(float pid_input){ e2i = e1i; //update last last time's error e1i = e0i; // update last time's error return u0i; +} + +// This is a P!ID contrller for motor speed + +float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds){ + + int T_diff = millis() - *time_pid_prev; + float speed = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev)); + *time_pid_prev = millis(); + + Serial.println(speed); + + if (speed >= 1) speed = 1; + else if (speed <= 0.5) speed = 0.5; + + *dist_to_move_prev = dist_to_move; + return speed; } \ No newline at end of file From 5dba48bee355424f3d151886fc85dc7d0d7a34e3 Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Fri, 11 Jun 2021 00:52:32 +0100 Subject: [PATCH 05/17] Attempt at angle PID --- Drive/src/main.cpp | 70 ++++++++++++++++++++++++++++++++++++---------- 1 file changed, 56 insertions(+), 14 deletions(-) diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp index 1688916..750d3fb 100644 --- a/Drive/src/main.cpp +++ b/Drive/src/main.cpp @@ -127,9 +127,17 @@ int dist_to_move_prev_fr = 0; unsigned long time_pid_prev_fl = 0; unsigned long time_pid_prev_fr = 0; +int dist_to_move_prev_sl = 0; +int dist_to_move_prev_sr = 0; +unsigned long time_pid_prev_sl = 0; +unsigned long time_pid_prev_sr = 0; + float kpdrive = 0.055; float kddrive = 4.700; +float kpheading = 0.055; +float kdheading = 4.700; + volatile byte movementflag = 0; volatile int xydat[2]; @@ -145,6 +153,7 @@ int mousecam_read_reg(int reg); void mousecam_reset(); int getCurrentHeading(); float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds); +float pid_h_ms(bool left, float speed, int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds); int convTwosComp(int b) { @@ -503,24 +512,32 @@ void loop() } } else - { //then move forwards but check angle for drift - if (total_x - distance < 0) + { //then move forwards but check angle for drift using optical flow + if (total_y - distance < 0) { //go forwards - Serial.println("going forwards"); - analogWrite(pwmr, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive), false)); - analogWrite(pwml, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive), true)); + //Serial.println("going forwards"); + float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive); + float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive); + float speed_r_head = pid_h_ms(0, speed_r, -total_x, &dist_to_move_prev_sr, &time_pid_prev_sr, kpheading, kdheading); + float speed_l_head = pid_h_ms(1, speed_l, -total_x, &dist_to_move_prev_sl, &time_pid_prev_sl, kpheading, kdheading); + analogWrite(pwmr, getPWMfromSpeed(speed_r, false)); + analogWrite(pwml, getPWMfromSpeed(speed_l, true)); digitalWrite(DIRR, LOW); digitalWrite(DIRL, HIGH); } - else if (total_x - distance > 0) + else if (total_y - distance > 0) { //go backwards - Serial.println("going backwards"); - analogWrite(pwmr, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive), false)); - analogWrite(pwml, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive), true)); + //Serial.println("going backwards"); + float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive); + float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive); + float speed_r_head = pid_h_ms(0, speed_r, -total_x, &dist_to_move_prev_sr, &time_pid_prev_sr, kpheading, kdheading); + float speed_l_head = pid_h_ms(1, speed_l, -total_x, &dist_to_move_prev_sl, &time_pid_prev_sl, kpheading, kdheading); + analogWrite(pwmr, getPWMfromSpeed(speed_r, false)); + analogWrite(pwml, getPWMfromSpeed(speed_l, true)); digitalWrite(DIRR, HIGH); digitalWrite(DIRL, LOW); } - else if ((total_x == distance)) + else if ((total_y == distance)) { //distance met //STOP!!!!! digitalWrite(pwmr, LOW); @@ -627,15 +644,15 @@ void loop() total_x1 = (total_x1 + distance_x); total_y1 = (total_y1 + distance_y); - total_y = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch) - total_x = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch) + total_x = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch) + total_y = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch) - /* Serial.print('\n'); + Serial.print('\n'); Serial.println("Distance_x = " + String(total_x)); Serial.println("Distance_y = " + String(total_y)); - Serial.print('\n'); */ + Serial.print('\n'); //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// unsigned long currentMillis = millis(); if (loopTrigger) @@ -828,4 +845,29 @@ float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_p *dist_to_move_prev = dist_to_move; return speed; +} + +// This is a P!ID contrller for heading using optical flow + +float pid_h_ms(bool left, float speed, int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds){ + + int T_diff = millis() - *time_pid_prev; + float speed_aug; + + if ((dist_to_move > 0 && !left )||( dist_to_move < 0 && left)){ + float speed_aug_mult = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev)); + if (speed_aug_mult >= 2) speed_aug_mult = 2; + else if (speed_aug_mult <= 1) speed_aug_mult = 1; + speed_aug = speed / dist_to_move; + }else{ + speed_aug = speed; + } + + *time_pid_prev = millis(); + + if (speed_aug >= 1) speed_aug = 1; + else if (speed_aug <= 0.3) speed_aug = 0.3; + + *dist_to_move_prev = dist_to_move; + return speed_aug; } \ No newline at end of file From 385eac760f606623a086b22f5edd828f3d462199 Mon Sep 17 00:00:00 2001 From: jc4419 <60656643+jc4419@users.noreply.github.com> Date: Fri, 11 Jun 2021 17:44:12 +0400 Subject: [PATCH 06/17] Changed layout fomatting to use tables instead of 'float' property. Removed com port field so it works in my environment. --- Control/data/index.html | 93 ++++++++++++++++++----------------------- Control/platformio.ini | 1 - 2 files changed, 40 insertions(+), 54 deletions(-) diff --git a/Control/data/index.html b/Control/data/index.html index 152b6f2..2585688 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -14,8 +14,8 @@ .section_container { float: left; - width: 50%; - padding: 10px; + width: 33%; + padding: 0px; } .flex-container { @@ -49,13 +49,13 @@ meter { width: 100%; height: 40px; - transform: translateY(-8px); + transform: translateY(8px); } meter::after { content: attr(value) attr(title); top: -28px; - left: 0px; + left: 45%; position: relative; } @@ -241,55 +241,42 @@

ROVER COMMAND CENTER

- -
-
-

Movement Control

-
- -
-
- - - -
- -
-
- -
-
-

Sensor Data

-
    - -
  • -
    - -
    -
    - -
    -
    • - - -
  • -
    - -
    -
    - 28mm -
    -
    • - -
-
- -
+ + + + + + + + + + +

Control Panel

Telemetry

Command Line

+
+ +
+
+ + + +
+ 		+ + + + + + + + + +
28mm
+
diff --git a/Control/platformio.ini b/Control/platformio.ini index 0f97db2..b6309b8 100644 --- a/Control/platformio.ini +++ b/Control/platformio.ini @@ -13,7 +13,6 @@ platform = espressif32 board = esp32dev framework = arduino monitor_speed = 115200 -upload_port = COM[3] monitor_filters = send_on_enter esp32_exception_decoder From f5ca382c491fe2a1bbd2e4b74b1f57a406ebae3b Mon Sep 17 00:00:00 2001 From: jc4419 <60656643+jc4419@users.noreply.github.com> Date: Fri, 11 Jun 2021 22:45:00 +0400 Subject: [PATCH 07/17] Added extra telemetry data parameters, and the corresponding javascript websocket back end. In short rover to client interface is done, needs to be tested. Also changed button interface to accomodate new 'command buffer' system. No real time control. Also made layout static so that button layout doesnt break when resizing window. --- Control/data/index.html | 211 ++++++++++++++++++++++------------------ 1 file changed, 118 insertions(+), 93 deletions(-) diff --git a/Control/data/index.html b/Control/data/index.html index 2585688..53242ae 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -12,38 +12,17 @@ box-sizing: border-box; } - .section_container { - float: left; - width: 33%; - padding: 0px; - } - .flex-container { display: flex; flex-wrap: nowrap; } - ul { - list-style-type: none; - margin: 0; - padding: 0; - } - - li { - padding: 0px; - margin-bottom: 0px; - } - :is(h1, h2, h3, h4, h5, h6, label, strong, meter) { font-family: Arial, Helvetica, sans-serif; } - .movement_control { - text-align: center; - } - - .sensor_data { - text-align: center; + h2{ + margin: 0px; } meter { @@ -59,10 +38,11 @@ position: relative; } - .button { + button { + width: 100%; display: inline-block; - padding: 15px 25px; - font-size: 24px; + padding: 7px 7px; + font-size: 12px; cursor: pointer; text-align: center; text-decoration: none; @@ -70,17 +50,17 @@ color: rgb(255, 255, 255); background-color: #161616; border: none; - border-radius: 15px; - box-shadow: 0 9px rgb(161, 161, 161); + border-radius: 5px; + box-shadow: 0 3px rgb(161, 161, 161); } - .button:hover { + button:hover { background-color: #585858 } - .button:active { + button:active { background-color: #107C10; - box-shadow: 0 5px rgb(161, 161, 161); + box-shadow: 0 3px rgb(161, 161, 161); transform: translateY(4px); } @@ -100,22 +80,47 @@ var connection = new WebSocket('ws://' + location.hostname + ':81/'); - var MVM_F_status = 0; - var MVM_L_status = 0; - var MVM_R_status = 0; - var MVM_B_status = 0; + var command_id = 0; + var mode = 0; + var reqHeading = 0; + var reqSpeed = 0; + var reqCharge = 0; + + var batteryVoltage = 0; + var totalTripDistance = 0; + var currentHeading = rdoc["cH"]; + var current_pos = [,]; + var current_x = 0; + var current_y = 0; + var signal_strength = 0; + var lastCompletedCommand_id = rdoc["LCCid"]; + var ChargeStatus = 0; - var BTRY_VOLT = 0; - var ODO_DIST = 0; connection.onmessage = function (event) { var raw_data = event.data; console.log(raw_data); var data = JSON.parse(raw_data); - BTRY_VOLT = data.BTRY_VOLT; - ODO_DIST = data.ODO_DIST; - document.getElementById("btry_meter").value = BTRY_VOLT; - document.getElementById("Odometer").innerHTML = ODO_DIST; + + batteryVoltage = data.bV; + totalTripDistance = data.tD; + currentHeading = data.cH; + current_pos = data.pos; + current_x = current_pos[0]; + current_y = current_pos[1]; + signal_strength = data.rssi; + lastCompletedCommand_id = data.LCCid; + ChargeStatus = data.cS; + + document.getElementById("SigStr").value = signal_strength; + document.getElementById("PosX").innerHTML = current_x; + document.getElementById("PosY").innerHTML = current_y; + document.getElementById("Hdg").innerHTML = currentHeading; + document.getElementById("TrpDist").innerHTML = totalTripDistance; + document.getElementById("btry_meter").value = batteryVoltage; + document.getElementById("ChgStat").innerHTML = (ChargeStatus ? "Charging" : "Discharging"); + + } function send_data() { @@ -124,6 +129,10 @@ console.log(raw_data); } + function cIdIncrement() { + command_id++; + } + function left_pressed() { MVM_L_status = 1; send_data(); @@ -191,48 +200,6 @@ left_unpressed(); } - - document.onkeydown = function (e) { - switch (e.keyCode) { - case 37: - document.getElementById("left_arrow").className = "button pressed"; - left_pressed(); - break; - case 38: - document.getElementById("up_arrow").className = "button pressed"; - up_pressed(); - break; - case 39: - document.getElementById("right_arrow").className = "button pressed"; - right_pressed(); - break; - case 40: - document.getElementById("down_arrow").className = "button pressed"; - down_pressed(); - break; - } - }; - document.onkeyup = function (e) { - switch (e.keyCode) { - case 37: - document.getElementById("left_arrow").className = "button"; - left_unpressed(); - break; - case 38: - document.getElementById("up_arrow").className = "button"; - up_unpressed(); - break; - case 39: - document.getElementById("right_arrow").className = "button"; - right_unpressed(); - break; - case 40: - document.getElementById("down_arrow").className = "button"; - down_unpressed(); - break; - } - }; - @@ -241,15 +208,54 @@

ROVER COMMAND CENTER

- +
- - - + + + +
From 44f9d8ce7b09ac660be74d32e811567f9d4231ea Mon Sep 17 00:00:00 2001 From: jc4419 <60656643+jc4419@users.noreply.github.com> Date: Sat, 12 Jun 2021 04:18:04 +0400 Subject: [PATCH 08/17] Removed redundant leftover code from previous versions. Tweaked some coloring and formatting(again). Added the 'Command Buffer' section. Added the JSON format for sending data, yet to be implemented. --- Control/data/index.html | 136 ++++++++++++++-------------------------- 1 file changed, 47 insertions(+), 89 deletions(-) diff --git a/Control/data/index.html b/Control/data/index.html index 53242ae..592791d 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -25,6 +25,16 @@ margin: 0px; } + #command_space { + width:100%; + height: 200px; + line-height: normal; + overflow: auto; + background-color: rgb(248, 248, 248); + color: black; + border: 1px solid black; + } + meter { width: 100%; height: 40px; @@ -48,7 +58,7 @@ text-decoration: none; outline: none; color: rgb(255, 255, 255); - background-color: #161616; + background-color: #222222; border: none; border-radius: 5px; box-shadow: 0 3px rgb(161, 161, 161); @@ -59,15 +69,9 @@ } button:active { - background-color: #107C10; + background-color: #349134; box-shadow: 0 3px rgb(161, 161, 161); - transform: translateY(4px); - } - - .pressed { - background-color: #107C10; - box-shadow: 0 5px rgb(161, 161, 161); - transform: translateY(4px); + transform: translateY(1px); } .clearfix::after { @@ -83,6 +87,7 @@ var command_id = 0; var mode = 0; var reqHeading = 0; + var reqDistance = 0; var reqSpeed = 0; var reqCharge = 0; @@ -124,81 +129,27 @@ } function send_data() { - var raw_data = '{"MVM_F":' + MVM_F_status + ',"MVM_L":' + MVM_L_status + ',"MVM_R":' + MVM_R_status + ',"MVM_B":' + MVM_B_status + '}'; + var raw_data = '{"Cid":' + command_id + ',"mode":' + mode + ',"rH":' + reqHeading + ',"rD":' + reqDistance + ',"rS":' + reqSpeed + ',"rC":' + reqCharge + '}'; connection.send(raw_data); console.log(raw_data); } + function echoCommandbuf() { + document.getElementById("command_buffer").innerHTML += '[' + command_id + '] ' + document.getElementById("commandInput").value + "
"; + cIdIncrement(); + document.getElementById("commandInput").value = ""; + console.log('test'); + } + function cIdIncrement() { command_id++; } - function left_pressed() { - MVM_L_status = 1; - send_data(); - } - function left_unpressed() { - MVM_L_status = 0; - send_data(); - } - function up_pressed() { - MVM_F_status = 1; - send_data(); - } - function up_unpressed() { - MVM_F_status = 0; - send_data(); - } - function right_pressed() { - MVM_R_status = 1; - send_data(); - } - function right_unpressed() { - MVM_R_status = 0; - send_data(); - } - function down_pressed() { - MVM_B_status = 1; - send_data(); - } - function down_unpressed() { - MVM_B_status = 0; - send_data(); - } - var timer = null; - function up_mouseDown() { - timer = setInterval(up_pressed, 100); - } - function up_mouseUp() { - clearInterval(timer); - up_unpressed(); - } - function down_mouseDown() { - timer = setInterval(down_pressed, 100); - } - function down_mouseUp() { - clearInterval(timer); - down_unpressed(); - } - function right_mouseDown() { - timer = setInterval(right_pressed, 100); - } - function right_mouseUp() { - clearInterval(timer); - right_unpressed(); - } - function left_mouseDown() { - timer = setInterval(left_pressed, 100); - } - function left_mouseUp() { - clearInterval(timer); - left_unpressed(); - } @@ -215,18 +166,20 @@
+ + - - - - + + + diff --git a/Control/src/main.cpp b/Control/src/main.cpp index aa24669..fdc00c4 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -111,15 +111,17 @@ void setup() { delay(500); } - while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/" + while (!MDNS.begin("rover2")) // Set up mDNS cast at "rover.local/" { Serial.println("Error setting up mDNS, retrying in 5s"); delay(5000); } - Serial.println("mDNS set up, access Control Panel at 'rover.local/'"); + Serial.println("mDNS set up, access Control Panel at 'rover2.local/'"); webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request) { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page + webserver.on("/command.js", HTTP_GET, [](AsyncWebServerRequest *request) + { request->send(SPIFFS, "/command.js", "text/js"); }); // Serve "command.js" for root page to accessj webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request) { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon webserver.onNotFound(notFound); // Set up basic 404NotFound page From c6f93039a164fcfe5604d64cfc6300d97307b04d Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Mon, 14 Jun 2021 20:32:07 +0100 Subject: [PATCH 15/17] IMU Stuff --- Drive/src/main.cpp | 5 ++--- IMU/platformio.ini | 3 +++ IMU/src/main.cpp | 12 +++++++++--- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp index 750d3fb..da06bdf 100644 --- a/Drive/src/main.cpp +++ b/Drive/src/main.cpp @@ -484,7 +484,6 @@ void loop() if (!initialAngleSet) { //turn to angle - currentHeading = getCurrentHeading(); if (currentHeading < requiredHeading) { //turn right Serial.println("turning right"); @@ -647,12 +646,12 @@ void loop() total_x = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch) total_y = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch) - Serial.print('\n'); +/* Serial.print('\n'); Serial.println("Distance_x = " + String(total_x)); Serial.println("Distance_y = " + String(total_y)); - Serial.print('\n'); + Serial.print('\n'); */ //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------// unsigned long currentMillis = millis(); if (loopTrigger) diff --git a/IMU/platformio.ini b/IMU/platformio.ini index ac15fe7..f89137d 100644 --- a/IMU/platformio.ini +++ b/IMU/platformio.ini @@ -15,4 +15,7 @@ framework = arduino lib_deps = tanakamasayuki/I2C MPU6886 IMU@^1.0.0 m5stack/M5StickC@^0.2.0 + bblanchon/ArduinoJson@^6.18.0 monitor_speed = 115200 +monitor_port = COM8 +upload_port = COM8 diff --git a/IMU/src/main.cpp b/IMU/src/main.cpp index 96031d8..c04011f 100644 --- a/IMU/src/main.cpp +++ b/IMU/src/main.cpp @@ -1,5 +1,6 @@ #include #include +#include float pitch, roll, yaw; @@ -7,13 +8,18 @@ void setup() { M5.begin(); M5.IMU.Init(); Serial.begin(115200); - Serial1.begin(115200, SERIAL_8N1, 26, 0); + Serial1.begin(9600, SERIAL_8N1, 26, 0); } void loop() { M5.IMU.getAhrsData(&pitch, &roll, &yaw); M5.Lcd.setCursor(0, 45); M5.Lcd.printf("%5.2f\n", roll); - Serial.printf("{\"cH\":%5.2f}\n", roll); - Serial1.printf("{\"cH\":%5.2f}\n", roll); + // Serial.printf("{\"cH\":%5.2f}\n", roll); + // Serial1.printf("{\"cH\":%5.2f}\n", roll); + DynamicJsonDocument tdoc(1024); + tdoc["cH"] = roll; + serializeJson(tdoc, Serial1); + + delay(50); } \ No newline at end of file From 5e95a8d86027ad1a5507665296362a85db4220f7 Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Tue, 15 Jun 2021 00:51:46 +0100 Subject: [PATCH 16/17] Control updates --- Control/src/main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index fdc00c4..5a80e17 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -23,10 +23,10 @@ #pragma endregion #pragma region Definitions eg pins -#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 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 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 @@ -82,7 +82,7 @@ void setup() 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(115200, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass) + Serial4.begin(9600, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass) // Set global variable startup values Status = CS_IDLE; @@ -232,7 +232,7 @@ void loop() } break; } - delay(500); + // delay(500); } void notFound(AsyncWebServerRequest *request) @@ -430,7 +430,7 @@ void recvFromCompass() { if (Serial4.available()) { - DynamicJsonDocument rdoc(128); + DynamicJsonDocument rdoc(1024); deserializeJson(rdoc, Serial4); heading = rdoc["cH"]; } From 72021e7208b4b0c7c82a534c3ae42cef7bd73317 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Tue, 15 Jun 2021 01:33:52 +0100 Subject: [PATCH 17/17] Fix Instruction Queue and Emergency Stop --- Control/src/main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 5a80e17..ff2cb42 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -195,6 +195,7 @@ void loop() break; } lastExecutedCommand = instr->id; // Update tracker of last processed command + InstrQueue.pop(); } } break; @@ -439,10 +440,7 @@ void recvFromCompass() void emergencyStop() { DynamicJsonDocument tdoc(1024); - tdoc["rH"] = heading; - tdoc["dist"] = -1; - tdoc["sp"] = -1; - tdoc["cH"] = heading; + tdoc["stp"] = 1; serializeJson(tdoc, Serial1); // Send stop signals to Drive sendToEnergy(0); // Send stop signal to Energy while (InstrQueue.size())

Control Panel

Telemetry

Command Line

Control Panel

Telemetry

Command Buffer
-
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Main
Rotation
Translation
+
- +
+ + + + + + + + + + + + + + + + - + - - + + +
X,Y
H°
X mm
28mmDisconnect

Command Buffer
- + - @@ -259,11 +209,11 @@
+ - + - + @@ -255,21 +208,13 @@
Main
- 		- + + + + - +
+ + + + +
+ + + + + + + +

+ +
From 6c221f6d12990b9a3f2ebfbfd48a2368fae65042 Mon Sep 17 00:00:00 2001 From: jc4419 <60656643+jc4419@users.noreply.github.com> Date: Sun, 13 Jun 2021 22:42:00 +0400 Subject: [PATCH 09/17] Fully implemented the console. Console required the additon of a parser written in .jison api and output as command.js which is linked in html. Tested working sending correct JSON packets. --- Control/data/command.js | 681 ++++++++++++++++++++++++++++++++++++++ Control/data/index.html | 46 ++- Control/lib/command.jison | 87 +++++ 3 files changed, 796 insertions(+), 18 deletions(-) create mode 100644 Control/data/command.js create mode 100644 Control/lib/command.jison diff --git a/Control/data/command.js b/Control/data/command.js new file mode 100644 index 0000000..76df3fe --- /dev/null +++ b/Control/data/command.js @@ -0,0 +1,681 @@ +/* parser generated by jison 0.4.18 */ +/* + Returns a Parser object of the following structure: + + Parser: { + yy: {} + } + + Parser.prototype: { + yy: {}, + trace: function(), + symbols_: {associative list: name ==> number}, + terminals_: {associative list: number ==> name}, + productions_: [...], + performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate, $$, _$), + table: [...], + defaultActions: {...}, + parseError: function(str, hash), + parse: function(input), + + lexer: { + EOF: 1, + parseError: function(str, hash), + setInput: function(input), + input: function(), + unput: function(str), + more: function(), + less: function(n), + pastInput: function(), + upcomingInput: function(), + showPosition: function(), + test_match: function(regex_match_array, rule_index), + next: function(), + lex: function(), + begin: function(condition), + popState: function(), + _currentRules: function(), + topState: function(), + pushState: function(condition), + + options: { + ranges: boolean (optional: true ==> token location info will include a .range[] member) + flex: boolean (optional: true ==> flex-like lexing behaviour where the rules are tested exhaustively to find the longest match) + backtrack_lexer: boolean (optional: true ==> lexer regexes are tested in order and for each matching regex the action code is invoked; the lexer terminates the scan when a token is returned by the action code) + }, + + performAction: function(yy, yy_, $avoiding_name_collisions, YY_START), + rules: [...], + conditions: {associative list: name ==> set}, + } + } + + + token location info (@$, _$, etc.): { + first_line: n, + last_line: n, + first_column: n, + last_column: n, + range: [start_number, end_number] (where the numbers are indexes into the input string, regular zero-based) + } + + + the parseError function receives a 'hash' object with these members for lexer and parser errors: { + text: (matched text) + token: (the produced terminal token, if any) + line: (yylineno) + } + while parser (grammar) errors will also provide these members, i.e. parser errors deliver a superset of attributes: { + loc: (yylloc) + expected: (string describing the set of expected tokens) + recoverable: (boolean: TRUE when the parser has a error recovery rule available for this particular error) + } +*/ +var command = (function(){ +var o=function(k,v,o,l){for(o=o||{},l=k.length;l--;o[k[l]]=v);return o}; +var parser = {trace: function trace () { }, +yy: {}, +symbols_: {"error":2,"command":3,"expr":4,"EOF":5,"MOVE":6,"WHITESPACE":7,"DIST":8,"HEADING":9,"PERCENTAGE":10,"STOP":11,"PSTOP":12,"CHARGETO":13,"TELERST":14,"$accept":0,"$end":1}, +terminals_: {2:"error",5:"EOF",6:"MOVE",7:"WHITESPACE",8:"DIST",9:"HEADING",10:"PERCENTAGE",11:"STOP",12:"PSTOP",13:"CHARGETO",14:"TELERST"}, +productions_: [0,[3,2],[4,7],[4,1],[4,1],[4,3],[4,1]], +performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate /* action[1] */, $$ /* vstack */, _$ /* lstack */) { +/* this == yyval */ + +var $0 = $$.length - 1; +switch (yystate) { +case 2: + + var inDist = String($$[$0-4]).substr(0, ((String($$[$0-4]).length) - 2)); + var inHdg = String($$[$0-2]).substr(0, ((String($$[$0-2]).length) - 3)); + var inSpd = String($$[$0]).substr(0, ((String($$[$0]).length) - 1)); + mode = 1; + reqDistance = Number(inDist); + reqHeading = Number(inHdg); + reqSpeed = Number(inSpd); + reqCharge = 0; + send_data(); + updateCommandBuffer(); + command_id++; + +break; +case 3: + + mode = 0; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + reqCharge = 0; + send_data(); + command_id = 0; + updateCommandBuffer() + +break; +case 4: + + mode = 1; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + reqCharge = 0; + send_data(); + updateCommandBuffer(); + command_id++; + +break; +case 5: + + mode = 1; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + + var inChrg = String($$[$0]).substr(0, ((String($$[$0]).length) - 1)); + reqCharge = Number(inChrg); + + send_data(); + updateCommandBuffer(); + command_id++; + +break; +case 6: +mode = 1; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + reqCharge = 0; + send_data(); + updateCommandBuffer(); + command_id++; +break; +} +}, +table: [{3:1,4:2,6:[1,3],11:[1,4],12:[1,5],13:[1,6],14:[1,7]},{1:[3]},{5:[1,8]},{7:[1,9]},{5:[2,3]},{5:[2,4]},{7:[1,10]},{5:[2,6]},{1:[2,1]},{8:[1,11]},{10:[1,12]},{7:[1,13]},{5:[2,5]},{9:[1,14]},{7:[1,15]},{10:[1,16]},{5:[2,2]}], +defaultActions: {4:[2,3],5:[2,4],7:[2,6],8:[2,1],12:[2,5],16:[2,2]}, +parseError: function parseError (str, hash) { + if (hash.recoverable) { + this.trace(str); + } else { + var error = new Error(str); + error.hash = hash; + throw error; + } +}, +parse: function parse(input) { + var self = this, stack = [0], tstack = [], vstack = [null], lstack = [], table = this.table, yytext = '', yylineno = 0, yyleng = 0, recovering = 0, TERROR = 2, EOF = 1; + var args = lstack.slice.call(arguments, 1); + var lexer = Object.create(this.lexer); + var sharedState = { yy: {} }; + for (var k in this.yy) { + if (Object.prototype.hasOwnProperty.call(this.yy, k)) { + sharedState.yy[k] = this.yy[k]; + } + } + lexer.setInput(input, sharedState.yy); + sharedState.yy.lexer = lexer; + sharedState.yy.parser = this; + if (typeof lexer.yylloc == 'undefined') { + lexer.yylloc = {}; + } + var yyloc = lexer.yylloc; + lstack.push(yyloc); + var ranges = lexer.options && lexer.options.ranges; + if (typeof sharedState.yy.parseError === 'function') { + this.parseError = sharedState.yy.parseError; + } else { + this.parseError = Object.getPrototypeOf(this).parseError; + } + function popStack(n) { + stack.length = stack.length - 2 * n; + vstack.length = vstack.length - n; + lstack.length = lstack.length - n; + } + _token_stack: + var lex = function () { + var token; + token = lexer.lex() || EOF; + if (typeof token !== 'number') { + token = self.symbols_[token] || token; + } + return token; + }; + var symbol, preErrorSymbol, state, action, a, r, yyval = {}, p, len, newState, expected; + while (true) { + state = stack[stack.length - 1]; + if (this.defaultActions[state]) { + action = this.defaultActions[state]; + } else { + if (symbol === null || typeof symbol == 'undefined') { + symbol = lex(); + } + action = table[state] && table[state][symbol]; + } + if (typeof action === 'undefined' || !action.length || !action[0]) { + var errStr = ''; + expected = []; + for (p in table[state]) { + if (this.terminals_[p] && p > TERROR) { + expected.push('\'' + this.terminals_[p] + '\''); + } + } + if (lexer.showPosition) { + errStr = 'Parse error on line ' + (yylineno + 1) + ':\n' + lexer.showPosition() + '\nExpecting ' + expected.join(', ') + ', got \'' + (this.terminals_[symbol] || symbol) + '\''; + } else { + errStr = 'Parse error on line ' + (yylineno + 1) + ': Unexpected ' + (symbol == EOF ? 'end of input' : '\'' + (this.terminals_[symbol] || symbol) + '\''); + } + this.parseError(errStr, { + text: lexer.match, + token: this.terminals_[symbol] || symbol, + line: lexer.yylineno, + loc: yyloc, + expected: expected + }); + } + if (action[0] instanceof Array && action.length > 1) { + throw new Error('Parse Error: multiple actions possible at state: ' + state + ', token: ' + symbol); + } + switch (action[0]) { + case 1: + stack.push(symbol); + vstack.push(lexer.yytext); + lstack.push(lexer.yylloc); + stack.push(action[1]); + symbol = null; + if (!preErrorSymbol) { + yyleng = lexer.yyleng; + yytext = lexer.yytext; + yylineno = lexer.yylineno; + yyloc = lexer.yylloc; + if (recovering > 0) { + recovering--; + } + } else { + symbol = preErrorSymbol; + preErrorSymbol = null; + } + break; + case 2: + len = this.productions_[action[1]][1]; + yyval.$ = vstack[vstack.length - len]; + yyval._$ = { + first_line: lstack[lstack.length - (len || 1)].first_line, + last_line: lstack[lstack.length - 1].last_line, + first_column: lstack[lstack.length - (len || 1)].first_column, + last_column: lstack[lstack.length - 1].last_column + }; + if (ranges) { + yyval._$.range = [ + lstack[lstack.length - (len || 1)].range[0], + lstack[lstack.length - 1].range[1] + ]; + } + r = this.performAction.apply(yyval, [ + yytext, + yyleng, + yylineno, + sharedState.yy, + action[1], + vstack, + lstack + ].concat(args)); + if (typeof r !== 'undefined') { + return r; + } + if (len) { + stack = stack.slice(0, -1 * len * 2); + vstack = vstack.slice(0, -1 * len); + lstack = lstack.slice(0, -1 * len); + } + stack.push(this.productions_[action[1]][0]); + vstack.push(yyval.$); + lstack.push(yyval._$); + newState = table[stack[stack.length - 2]][stack[stack.length - 1]]; + stack.push(newState); + break; + case 3: + return true; + } + } + return true; +}}; +/* generated by jison-lex 0.3.4 */ +var lexer = (function(){ +var lexer = ({ + +EOF:1, + +parseError:function parseError(str, hash) { + if (this.yy.parser) { + this.yy.parser.parseError(str, hash); + } else { + throw new Error(str); + } + }, + +// resets the lexer, sets new input +setInput:function (input, yy) { + this.yy = yy || this.yy || {}; + this._input = input; + this._more = this._backtrack = this.done = false; + this.yylineno = this.yyleng = 0; + this.yytext = this.matched = this.match = ''; + this.conditionStack = ['INITIAL']; + this.yylloc = { + first_line: 1, + first_column: 0, + last_line: 1, + last_column: 0 + }; + if (this.options.ranges) { + this.yylloc.range = [0,0]; + } + this.offset = 0; + return this; + }, + +// consumes and returns one char from the input +input:function () { + var ch = this._input[0]; + this.yytext += ch; + this.yyleng++; + this.offset++; + this.match += ch; + this.matched += ch; + var lines = ch.match(/(?:\r\n?|\n).*/g); + if (lines) { + this.yylineno++; + this.yylloc.last_line++; + } else { + this.yylloc.last_column++; + } + if (this.options.ranges) { + this.yylloc.range[1]++; + } + + this._input = this._input.slice(1); + return ch; + }, + +// unshifts one char (or a string) into the input +unput:function (ch) { + var len = ch.length; + var lines = ch.split(/(?:\r\n?|\n)/g); + + this._input = ch + this._input; + this.yytext = this.yytext.substr(0, this.yytext.length - len); + //this.yyleng -= len; + this.offset -= len; + var oldLines = this.match.split(/(?:\r\n?|\n)/g); + this.match = this.match.substr(0, this.match.length - 1); + this.matched = this.matched.substr(0, this.matched.length - 1); + + if (lines.length - 1) { + this.yylineno -= lines.length - 1; + } + var r = this.yylloc.range; + + this.yylloc = { + first_line: this.yylloc.first_line, + last_line: this.yylineno + 1, + first_column: this.yylloc.first_column, + last_column: lines ? + (lines.length === oldLines.length ? this.yylloc.first_column : 0) + + oldLines[oldLines.length - lines.length].length - lines[0].length : + this.yylloc.first_column - len + }; + + if (this.options.ranges) { + this.yylloc.range = [r[0], r[0] + this.yyleng - len]; + } + this.yyleng = this.yytext.length; + return this; + }, + +// When called from action, caches matched text and appends it on next action +more:function () { + this._more = true; + return this; + }, + +// When called from action, signals the lexer that this rule fails to match the input, so the next matching rule (regex) should be tested instead. +reject:function () { + if (this.options.backtrack_lexer) { + this._backtrack = true; + } else { + return this.parseError('Lexical error on line ' + (this.yylineno + 1) + '. You can only invoke reject() in the lexer when the lexer is of the backtracking persuasion (options.backtrack_lexer = true).\n' + this.showPosition(), { + text: "", + token: null, + line: this.yylineno + }); + + } + return this; + }, + +// retain first n characters of the match +less:function (n) { + this.unput(this.match.slice(n)); + }, + +// displays already matched input, i.e. for error messages +pastInput:function () { + var past = this.matched.substr(0, this.matched.length - this.match.length); + return (past.length > 20 ? '...':'') + past.substr(-20).replace(/\n/g, ""); + }, + +// displays upcoming input, i.e. for error messages +upcomingInput:function () { + var next = this.match; + if (next.length < 20) { + next += this._input.substr(0, 20-next.length); + } + return (next.substr(0,20) + (next.length > 20 ? '...' : '')).replace(/\n/g, ""); + }, + +// displays the character position where the lexing error occurred, i.e. for error messages +showPosition:function () { + var pre = this.pastInput(); + var c = new Array(pre.length + 1).join("-"); + return pre + this.upcomingInput() + "\n" + c + "^"; + }, + +// test the lexed token: return FALSE when not a match, otherwise return token +test_match:function(match, indexed_rule) { + var token, + lines, + backup; + + if (this.options.backtrack_lexer) { + // save context + backup = { + yylineno: this.yylineno, + yylloc: { + first_line: this.yylloc.first_line, + last_line: this.last_line, + first_column: this.yylloc.first_column, + last_column: this.yylloc.last_column + }, + yytext: this.yytext, + match: this.match, + matches: this.matches, + matched: this.matched, + yyleng: this.yyleng, + offset: this.offset, + _more: this._more, + _input: this._input, + yy: this.yy, + conditionStack: this.conditionStack.slice(0), + done: this.done + }; + if (this.options.ranges) { + backup.yylloc.range = this.yylloc.range.slice(0); + } + } + + lines = match[0].match(/(?:\r\n?|\n).*/g); + if (lines) { + this.yylineno += lines.length; + } + this.yylloc = { + first_line: this.yylloc.last_line, + last_line: this.yylineno + 1, + first_column: this.yylloc.last_column, + last_column: lines ? + lines[lines.length - 1].length - lines[lines.length - 1].match(/\r?\n?/)[0].length : + this.yylloc.last_column + match[0].length + }; + this.yytext += match[0]; + this.match += match[0]; + this.matches = match; + this.yyleng = this.yytext.length; + if (this.options.ranges) { + this.yylloc.range = [this.offset, this.offset += this.yyleng]; + } + this._more = false; + this._backtrack = false; + this._input = this._input.slice(match[0].length); + this.matched += match[0]; + token = this.performAction.call(this, this.yy, this, indexed_rule, this.conditionStack[this.conditionStack.length - 1]); + if (this.done && this._input) { + this.done = false; + } + if (token) { + return token; + } else if (this._backtrack) { + // recover context + for (var k in backup) { + this[k] = backup[k]; + } + return false; // rule action called reject() implying the next rule should be tested instead. + } + return false; + }, + +// return next match in input +next:function () { + if (this.done) { + return this.EOF; + } + if (!this._input) { + this.done = true; + } + + var token, + match, + tempMatch, + index; + if (!this._more) { + this.yytext = ''; + this.match = ''; + } + var rules = this._currentRules(); + for (var i = 0; i < rules.length; i++) { + tempMatch = this._input.match(this.rules[rules[i]]); + if (tempMatch && (!match || tempMatch[0].length > match[0].length)) { + match = tempMatch; + index = i; + if (this.options.backtrack_lexer) { + token = this.test_match(tempMatch, rules[i]); + if (token !== false) { + return token; + } else if (this._backtrack) { + match = false; + continue; // rule action called reject() implying a rule MISmatch. + } else { + // else: this is a lexer rule which consumes input without producing a token (e.g. whitespace) + return false; + } + } else if (!this.options.flex) { + break; + } + } + } + if (match) { + token = this.test_match(match, rules[index]); + if (token !== false) { + return token; + } + // else: this is a lexer rule which consumes input without producing a token (e.g. whitespace) + return false; + } + if (this._input === "") { + return this.EOF; + } else { + return this.parseError('Lexical error on line ' + (this.yylineno + 1) + '. Unrecognized text.\n' + this.showPosition(), { + text: "", + token: null, + line: this.yylineno + }); + } + }, + +// return next match that has a token +lex:function lex () { + var r = this.next(); + if (r) { + return r; + } else { + return this.lex(); + } + }, + +// activates a new lexer condition state (pushes the new lexer condition state onto the condition stack) +begin:function begin (condition) { + this.conditionStack.push(condition); + }, + +// pop the previously active lexer condition state off the condition stack +popState:function popState () { + var n = this.conditionStack.length - 1; + if (n > 0) { + return this.conditionStack.pop(); + } else { + return this.conditionStack[0]; + } + }, + +// produce the lexer rule set which is active for the currently active lexer condition state +_currentRules:function _currentRules () { + if (this.conditionStack.length && this.conditionStack[this.conditionStack.length - 1]) { + return this.conditions[this.conditionStack[this.conditionStack.length - 1]].rules; + } else { + return this.conditions["INITIAL"].rules; + } + }, + +// return the currently active lexer condition state; when an index argument is provided it produces the N-th previous condition state, if available +topState:function topState (n) { + n = this.conditionStack.length - 1 - Math.abs(n || 0); + if (n >= 0) { + return this.conditionStack[n]; + } else { + return "INITIAL"; + } + }, + +// alias for begin(condition) +pushState:function pushState (condition) { + this.begin(condition); + }, + +// return the number of states currently on the stack +stateStackSize:function stateStackSize() { + return this.conditionStack.length; + }, +options: {}, +performAction: function anonymous(yy,yy_,$avoiding_name_collisions,YY_START) { +var YYSTATE=YY_START; +switch($avoiding_name_collisions) { +case 0:return 7 +break; +case 1:return 8 +break; +case 2:return 9 +break; +case 3:return 10 +break; +case 4:return 6 +break; +case 5:return 12 +break; +case 6:return 11 +break; +case 7:return 13 +break; +case 8:return 14 +break; +case 9:return 5 +break; +case 10:return 'INVALID' +break; +} +}, +rules: [/^(?:\s)/,/^(?:\b[0-9]+mm\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])deg\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|100)%)/,/^(?:\bmove\b)/,/^(?:\bpstop\b)/,/^(?:\bstop\b)/,/^(?:\bcharge\sto\b)/,/^(?:\btelemetry\sreset\b)/,/^(?:$)/,/^(?:.)/], +conditions: {"INITIAL":{"rules":[0,1,2,3,4,5,6,7,8,9,10],"inclusive":true}} +}); +return lexer; +})(); +parser.lexer = lexer; +function Parser () { + this.yy = {}; +} +Parser.prototype = parser;parser.Parser = Parser; +return new Parser; +})(); + + +if (typeof require !== 'undefined' && typeof exports !== 'undefined') { +exports.parser = command; +exports.Parser = command.Parser; +exports.parse = function () { return command.parse.apply(command, arguments); }; +exports.main = function commonjsMain (args) { + if (!args[1]) { + console.log('Usage: '+args[0]+' FILE'); + process.exit(1); + } + var source = require('fs').readFileSync(require('path').normalize(args[1]), "utf8"); + return exports.parser.parse(source); +}; +if (typeof module !== 'undefined' && require.main === module) { + exports.main(process.argv.slice(1)); +} +} \ No newline at end of file diff --git a/Control/data/index.html b/Control/data/index.html index 592791d..998e6f2 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -80,25 +80,28 @@ display: table; } + @@ -253,7 +263,7 @@

diff --git a/Control/lib/command.jison b/Control/lib/command.jison new file mode 100644 index 0000000..1278857 --- /dev/null +++ b/Control/lib/command.jison @@ -0,0 +1,87 @@ +%lex +%% + +\s return 'WHITESPACE' +\b[0-9]+"mm"\b return 'DIST' +\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])"deg"\b return 'HEADING' +\b([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'PERCENTAGE' +\bmove\b return 'MOVE' +\bpstop\b return 'PSTOP' +\bstop\b return 'STOP' +\bcharge\sto\b return 'CHARGETO' +\btelemetry\sreset\b return 'TELERST' +<> return 'EOF' +. return 'INVALID' + +/lex + +%start command + +%% + +command + : expr EOF + ; + +expr + : MOVE WHITESPACE DIST WHITESPACE HEADING WHITESPACE PERCENTAGE + { + var inDist = String($3).substr(0, ((String($3).length) - 2)); + var inHdg = String($5).substr(0, ((String($5).length) - 3)); + var inSpd = String($7).substr(0, ((String($7).length) - 1)); + mode = 1; + reqDistance = Number(inDist); + reqHeading = Number(inHdg); + reqSpeed = Number(inSpd); + reqCharge = 0; + send_data(); + updateCommandBuffer(); + command_id++; + } + | STOP + { + mode = 0; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + reqCharge = 0; + send_data(); + command_id = 0; + updateCommandBuffer() + } + | PSTOP + { + mode = 1; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + reqCharge = 0; + send_data(); + updateCommandBuffer(); + command_id++; + } + | CHARGETO WHITESPACE PERCENTAGE + { + mode = 1; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + + var inChrg = String($3).substr(0, ((String($3).length) - 1)); + reqCharge = Number(inChrg); + + send_data(); + updateCommandBuffer(); + command_id++; + } + | TELERST + {mode = 1; + reqDistance = 0; + reqHeading = 0; + reqSpeed = 0; + reqCharge = 0; + send_data(); + updateCommandBuffer(); + command_id++;} + ; + From aadb52990ee7d142e82c335f134f53e38d4e1133 Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Mon, 14 Jun 2021 17:25:57 +0100 Subject: [PATCH 10/17] IMU stuff --- IMU/.gitignore | 5 ++++ IMU/.vscode/extensions.json | 7 ++++++ IMU/include/README | 39 +++++++++++++++++++++++++++++++ IMU/lib/README | 46 +++++++++++++++++++++++++++++++++++++ IMU/platformio.ini | 18 +++++++++++++++ IMU/src/main.cpp | 19 +++++++++++++++ IMU/test/README | 11 +++++++++ 7 files changed, 145 insertions(+) create mode 100644 IMU/.gitignore create mode 100644 IMU/.vscode/extensions.json create mode 100644 IMU/include/README create mode 100644 IMU/lib/README create mode 100644 IMU/platformio.ini create mode 100644 IMU/src/main.cpp create mode 100644 IMU/test/README diff --git a/IMU/.gitignore b/IMU/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/IMU/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/IMU/.vscode/extensions.json b/IMU/.vscode/extensions.json new file mode 100644 index 0000000..e80666b --- /dev/null +++ b/IMU/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/IMU/include/README b/IMU/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/IMU/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/IMU/lib/README b/IMU/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/IMU/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/IMU/platformio.ini b/IMU/platformio.ini new file mode 100644 index 0000000..ac15fe7 --- /dev/null +++ b/IMU/platformio.ini @@ -0,0 +1,18 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:m5stick-c] +platform = espressif32 +board = m5stick-c +framework = arduino +lib_deps = + tanakamasayuki/I2C MPU6886 IMU@^1.0.0 + m5stack/M5StickC@^0.2.0 +monitor_speed = 115200 diff --git a/IMU/src/main.cpp b/IMU/src/main.cpp new file mode 100644 index 0000000..75cdd6e --- /dev/null +++ b/IMU/src/main.cpp @@ -0,0 +1,19 @@ +#include +#include + +float pitch, roll, yaw; + +void setup() { + M5.begin(); + M5.IMU.Init(); + Serial.begin(115200); + Serial1.begin(115200, SERIAL_8N1, 26, 36); +} +void loop() { + M5.IMU.getAhrsData(&pitch, &roll, &yaw); + M5.Lcd.setCursor(0, 45); + M5.Lcd.printf("%5.2f\n", roll); + Serial.printf("{\"cH\":%5.2f}\n", roll); + Serial1.printf("{\"cH\":%5.2f}\n", roll); + delay(50); +} \ No newline at end of file diff --git a/IMU/test/README b/IMU/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/IMU/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html From 46519cfe19f36cddcca510a43e7daec42f11b735 Mon Sep 17 00:00:00 2001 From: jc4419 <60656643+jc4419@users.noreply.github.com> Date: Mon, 14 Jun 2021 20:37:05 +0400 Subject: [PATCH 11/17] Alpha stage commit. Console done. Console Changes: changed the name of the tokens so that more helpful error messages. Added help text. Moved command.jison to src. Other Changes: Added more telemetry data about battery and rover status. Finished gui commands, replaced to sliders from buttons and linked them to command console. --- Control/data/command.js | 95 ++++----- Control/data/index.html | 393 +++++++++++++++++++++++++++----------- Control/lib/command.jison | 87 --------- Control/src/command.jison | 59 ++++++ 4 files changed, 372 insertions(+), 262 deletions(-) delete mode 100644 Control/lib/command.jison create mode 100644 Control/src/command.jison diff --git a/Control/data/command.js b/Control/data/command.js index 76df3fe..091ecc2 100644 --- a/Control/data/command.js +++ b/Control/data/command.js @@ -75,9 +75,9 @@ var command = (function(){ var o=function(k,v,o,l){for(o=o||{},l=k.length;l--;o[k[l]]=v);return o}; var parser = {trace: function trace () { }, yy: {}, -symbols_: {"error":2,"command":3,"expr":4,"EOF":5,"MOVE":6,"WHITESPACE":7,"DIST":8,"HEADING":9,"PERCENTAGE":10,"STOP":11,"PSTOP":12,"CHARGETO":13,"TELERST":14,"$accept":0,"$end":1}, -terminals_: {2:"error",5:"EOF",6:"MOVE",7:"WHITESPACE",8:"DIST",9:"HEADING",10:"PERCENTAGE",11:"STOP",12:"PSTOP",13:"CHARGETO",14:"TELERST"}, -productions_: [0,[3,2],[4,7],[4,1],[4,1],[4,3],[4,1]], +symbols_: {"error":2,"command":3,"expr":4,"EOF":5,"move":6,"whitespace":7,"distance":8,"heading_angle":9,"percentage":10,"stop":11,"pstop":12,"stop_duration":13,"charge_to":14,"telemetry_reset":15,"help":16,"$accept":0,"$end":1}, +terminals_: {2:"error",5:"EOF",6:"move",7:"whitespace",8:"distance",9:"heading_angle",10:"percentage",11:"stop",12:"pstop",13:"stop_duration",14:"charge_to",15:"telemetry_reset",16:"help"}, +productions_: [0,[3,2],[4,7],[4,1],[4,3],[4,3],[4,1],[4,1]], performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate /* action[1] */, $$ /* vstack */, _$ /* lstack */) { /* this == yyval */ @@ -85,72 +85,43 @@ var $0 = $$.length - 1; switch (yystate) { case 2: - var inDist = String($$[$0-4]).substr(0, ((String($$[$0-4]).length) - 2)); - var inHdg = String($$[$0-2]).substr(0, ((String($$[$0-2]).length) - 3)); - var inSpd = String($$[$0]).substr(0, ((String($$[$0]).length) - 1)); - mode = 1; - reqDistance = Number(inDist); - reqHeading = Number(inHdg); - reqSpeed = Number(inSpd); - reqCharge = 0; - send_data(); - updateCommandBuffer(); - command_id++; + var inDist = Number(String($$[$0-4]).substr(0, ((String($$[$0-4]).length) - 2))); + var inHdg = Number(String($$[$0-2]).substr(0, ((String($$[$0-2]).length) - 3))); + var inSpd = Number(String($$[$0]).substr(0, ((String($$[$0]).length) - 1))); + moveCmd(inDist,inHdg,inSpd); break; case 3: - mode = 0; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - reqCharge = 0; - send_data(); - command_id = 0; - updateCommandBuffer() + stpCmd(); break; case 4: - mode = 1; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - reqCharge = 0; - send_data(); - updateCommandBuffer(); - command_id++; + var inStpDur = Number(String($$[$0]).substr(0, ((String($$[$0]).length) - 1))); + pstpCmd(inStpDur); break; case 5: - mode = 1; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - - var inChrg = String($$[$0]).substr(0, ((String($$[$0]).length) - 1)); - reqCharge = Number(inChrg); - - send_data(); - updateCommandBuffer(); - command_id++; + var inChrg = Number(String($$[$0]).substr(0, ((String($$[$0]).length) - 1))); + chrgCmd(inChrg); break; case 6: -mode = 1; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - reqCharge = 0; - send_data(); - updateCommandBuffer(); - command_id++; + + telRst(); + +break; +case 7: + + printHelpDetails(); + break; } }, -table: [{3:1,4:2,6:[1,3],11:[1,4],12:[1,5],13:[1,6],14:[1,7]},{1:[3]},{5:[1,8]},{7:[1,9]},{5:[2,3]},{5:[2,4]},{7:[1,10]},{5:[2,6]},{1:[2,1]},{8:[1,11]},{10:[1,12]},{7:[1,13]},{5:[2,5]},{9:[1,14]},{7:[1,15]},{10:[1,16]},{5:[2,2]}], -defaultActions: {4:[2,3],5:[2,4],7:[2,6],8:[2,1],12:[2,5],16:[2,2]}, +table: [{3:1,4:2,6:[1,3],11:[1,4],12:[1,5],14:[1,6],15:[1,7],16:[1,8]},{1:[3]},{5:[1,9]},{7:[1,10]},{5:[2,3]},{7:[1,11]},{7:[1,12]},{5:[2,6]},{5:[2,7]},{1:[2,1]},{8:[1,13]},{13:[1,14]},{10:[1,15]},{7:[1,16]},{5:[2,4]},{5:[2,5]},{9:[1,17]},{7:[1,18]},{10:[1,19]},{5:[2,2]}], +defaultActions: {4:[2,3],7:[2,6],8:[2,7],9:[2,1],14:[2,4],15:[2,5],19:[2,2]}, parseError: function parseError (str, hash) { if (hash.recoverable) { this.trace(str); @@ -633,24 +604,28 @@ case 2:return 9 break; case 3:return 10 break; -case 4:return 6 +case 4:return 13 break; -case 5:return 12 +case 5:return 6 break; -case 6:return 11 +case 6:return 12 break; -case 7:return 13 +case 7:return 11 break; -case 8:return 14 +case 8:return 16 break; -case 9:return 5 +case 9:return 14 break; -case 10:return 'INVALID' +case 10:return 15 +break; +case 11:return 5 +break; +case 12:return 'invalid_command' break; } }, -rules: [/^(?:\s)/,/^(?:\b[0-9]+mm\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])deg\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|100)%)/,/^(?:\bmove\b)/,/^(?:\bpstop\b)/,/^(?:\bstop\b)/,/^(?:\bcharge\sto\b)/,/^(?:\btelemetry\sreset\b)/,/^(?:$)/,/^(?:.)/], -conditions: {"INITIAL":{"rules":[0,1,2,3,4,5,6,7,8,9,10],"inclusive":true}} +rules: [/^(?:\s)/,/^(?:\b[0-9]+mm\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])deg\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|100)%)/,/^(?:\b[0-9]+s\b)/,/^(?:\bmove\b)/,/^(?:\bpstop\b)/,/^(?:\bstop\b)/,/^(?:\bhelp\b)/,/^(?:\bcharge\sto\b)/,/^(?:\btelemetry\sreset\b)/,/^(?:$)/,/^(?:.)/], +conditions: {"INITIAL":{"rules":[0,1,2,3,4,5,6,7,8,9,10,11,12],"inclusive":true}} }); return lexer; })(); diff --git a/Control/data/index.html b/Control/data/index.html index 998e6f2..f83a83c 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -25,6 +25,30 @@ margin: 0px; } + .slider { + -webkit-appearance: none; + width: 100%; + height: 25px; + background: #d3d3d3; + outline: none; + opacity: 0.7; + -webkit-transition: .2s; + transition: opacity .2s; + } + + .slider:hover { + opacity: 1; + } + + .slider::-webkit-slider-thumb { + -webkit-appearance: none; + appearance: none; + width: 25px; + height: 25px; + background: #000000; + cursor: pointer; + } + #command_space { width:100%; height: 200px; @@ -62,6 +86,8 @@ border: none; border-radius: 5px; box-shadow: 0 3px rgb(161, 161, 161); + -webkit-transition: .2s; + transition: background-color .2s; } button:hover { @@ -69,7 +95,6 @@ } button:active { - background-color: #349134; box-shadow: 0 3px rgb(161, 161, 161); transform: translateY(1px); } @@ -80,100 +105,18 @@ display: table; } - - -

ROVER COMMAND CENTER

- +
- +
@@ -193,28 +136,25 @@ - - - - - - - - - - - - - - + - - - - - - + + + + + + + + + + + + + + + +

Control Panel

Telemetry

Command Buffer

Command Console
Rotation
Translation Set Heading to: 270°
Set Translation to: 180mm
Set Speed to: 50%
@@ -224,12 +164,18 @@
- + + + + + + @@ -242,16 +188,20 @@ + - + - - - + + + + + + +
X +
X,Y X mm
XV
DisconnectX%
X
 - - + + - +

@@ -272,7 +222,220 @@
- + + + \ No newline at end of file diff --git a/Control/lib/command.jison b/Control/lib/command.jison deleted file mode 100644 index 1278857..0000000 --- a/Control/lib/command.jison +++ /dev/null @@ -1,87 +0,0 @@ -%lex -%% - -\s return 'WHITESPACE' -\b[0-9]+"mm"\b return 'DIST' -\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])"deg"\b return 'HEADING' -\b([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'PERCENTAGE' -\bmove\b return 'MOVE' -\bpstop\b return 'PSTOP' -\bstop\b return 'STOP' -\bcharge\sto\b return 'CHARGETO' -\btelemetry\sreset\b return 'TELERST' -<> return 'EOF' -. return 'INVALID' - -/lex - -%start command - -%% - -command - : expr EOF - ; - -expr - : MOVE WHITESPACE DIST WHITESPACE HEADING WHITESPACE PERCENTAGE - { - var inDist = String($3).substr(0, ((String($3).length) - 2)); - var inHdg = String($5).substr(0, ((String($5).length) - 3)); - var inSpd = String($7).substr(0, ((String($7).length) - 1)); - mode = 1; - reqDistance = Number(inDist); - reqHeading = Number(inHdg); - reqSpeed = Number(inSpd); - reqCharge = 0; - send_data(); - updateCommandBuffer(); - command_id++; - } - | STOP - { - mode = 0; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - reqCharge = 0; - send_data(); - command_id = 0; - updateCommandBuffer() - } - | PSTOP - { - mode = 1; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - reqCharge = 0; - send_data(); - updateCommandBuffer(); - command_id++; - } - | CHARGETO WHITESPACE PERCENTAGE - { - mode = 1; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - - var inChrg = String($3).substr(0, ((String($3).length) - 1)); - reqCharge = Number(inChrg); - - send_data(); - updateCommandBuffer(); - command_id++; - } - | TELERST - {mode = 1; - reqDistance = 0; - reqHeading = 0; - reqSpeed = 0; - reqCharge = 0; - send_data(); - updateCommandBuffer(); - command_id++;} - ; - diff --git a/Control/src/command.jison b/Control/src/command.jison new file mode 100644 index 0000000..f36dfd9 --- /dev/null +++ b/Control/src/command.jison @@ -0,0 +1,59 @@ +%lex +%% + +\s return 'whitespace' +\b[0-9]+"mm"\b return 'distance' +\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])"deg"\b return 'heading_angle' +\b([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'percentage' +\b[0-9]+"s"\b return 'stop_duration' +\bmove\b return 'move' +\bpstop\b return 'pstop' +\bstop\b return 'stop' +\bhelp\b return 'help' +\bcharge\sto\b return 'charge_to' +\btelemetry\sreset\b return 'telemetry_reset' +<> return 'EOF' +. return 'invalid_command' + +/lex + +%start command + +%% + +command + : expr EOF + ; + +expr + : move whitespace distance whitespace heading_angle whitespace percentage + { + var inDist = Number(String($3).substr(0, ((String($3).length) - 2))); + var inHdg = Number(String($5).substr(0, ((String($5).length) - 3))); + var inSpd = Number(String($7).substr(0, ((String($7).length) - 1))); + moveCmd(inDist,inHdg,inSpd); + } + | stop + { + stpCmd(); + } + | pstop whitespace stop_duration + { + var inStpDur = Number(String($3).substr(0, ((String($3).length) - 1))); + pstpCmd(inStpDur); + } + | charge_to whitespace percentage + { + var inChrg = Number(String($3).substr(0, ((String($3).length) - 1))); + chrgCmd(inChrg); + } + | telemetry_reset + { + telRst(); + } + | help + { + printHelpDetails(); + } + ; + From 2c135f118a513a3d9ae9b510ac8ab1107724ec68 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Mon, 14 Jun 2021 17:50:15 +0100 Subject: [PATCH 12/17] Add external compass input --- Control/src/main.cpp | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/Control/src/main.cpp b/Control/src/main.cpp index 09106b2..aa24669 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -29,6 +29,8 @@ #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 +#define RX4pin 15 // Pin 12 on expansion board, UART4 +#define TX4pin 2 // Pin 13 on expansion board, UART4 #pragma endregion #pragma region Function Declarations @@ -42,6 +44,7 @@ void sendToEnergy(bool instruction); void recvFromEnergy(); void sendToVision(); void recvFromVision(); +void recvFromCompass(); void emergencyStop(); #pragma endregion @@ -49,7 +52,7 @@ void emergencyStop(); AsyncWebServer webserver(80); WebSocketsServer websocketserver(81); Ticker ticker; -SoftwareSerial Serial3; +SoftwareSerial Serial3, Serial4; std::queue InstrQueue; #pragma endregion @@ -75,10 +78,11 @@ 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(115200, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass) // Set global variable startup values Status = CS_IDLE; @@ -131,7 +135,8 @@ void loop() websocketserver.loop(); // Handle incoming client connections recvFromDrive(); // Update stats from Drive recvFromEnergy(); // Update stats from Energy - recvFromVision(); // Update stats from Vision + // recvFromVision(); // Update stats from Vision + recvFromCompass(); // Update stats from Compass switch (Status) { case CS_ERROR: @@ -213,10 +218,9 @@ void loop() { Status = CS_IDLE; lastCompletedCommand = lastExecutedCommand; // Update last completed command - sendToEnergy(0); // Stop charging if goal reached + sendToEnergy(0); // Stop charging if goal reached } // Otherwise continue charging, no change - } break; default: @@ -306,7 +310,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) instr.instr = INSTR_CHARGE; instr.charge = rdoc["rC"]; // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"] - + queueInstruction(instr); // Put charge command in InstrQueue } break; @@ -420,6 +424,16 @@ void recvFromVision() // Update bounding box and obstacle detection data from Vi } } +void recvFromCompass() +{ + if (Serial4.available()) + { + DynamicJsonDocument rdoc(128); + deserializeJson(rdoc, Serial4); + heading = rdoc["cH"]; + } +} + void emergencyStop() { DynamicJsonDocument tdoc(1024); From cc113aa0864db93d8c30689b105131dbbd585faf Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Mon, 14 Jun 2021 18:40:58 +0100 Subject: [PATCH 13/17] Pin change --- IMU/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/IMU/src/main.cpp b/IMU/src/main.cpp index 75cdd6e..96031d8 100644 --- a/IMU/src/main.cpp +++ b/IMU/src/main.cpp @@ -7,7 +7,7 @@ void setup() { M5.begin(); M5.IMU.Init(); Serial.begin(115200); - Serial1.begin(115200, SERIAL_8N1, 26, 36); + Serial1.begin(115200, SERIAL_8N1, 26, 0); } void loop() { M5.IMU.getAhrsData(&pitch, &roll, &yaw); From 4fd4e56f6266b279bf8fcc6fe489dc98a0614889 Mon Sep 17 00:00:00 2001 From: Aadi Desai <21363892+supleed2@users.noreply.github.com> Date: Mon, 14 Jun 2021 20:27:49 +0100 Subject: [PATCH 14/17] Add command.js to webserver serve code --- Control/data/index.html | 6 +++--- Control/src/main.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/Control/data/index.html b/Control/data/index.html index f83a83c..12f4278 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -130,9 +130,9 @@