From f40452c255a8f210b4dc2f66bdd485c3bb8d16d9 Mon Sep 17 00:00:00 2001 From: Mee2001 Date: Thu, 10 Jun 2021 14:40:00 +0100 Subject: [PATCH] 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