From be103e30721cf3b4fade3c516ac6d7f148fad6d9 Mon Sep 17 00:00:00 2001 From: NikitaK Date: Wed, 9 Jun 2021 18:04:51 +0100 Subject: [PATCH] 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