S2Project final failed code
Dependencies: QEI TB6612 VL53L0X
main.cpp
- Committer:
- sas638
- Date:
- 2022-05-08
- Revision:
- 1:2173f1dbfe1c
- Parent:
- 0:bcce413163a4
File content as of revision 1:2173f1dbfe1c:
/* mbed Microcontroller Library * Copyright (c) 2019 ARM Limited * SPDX-License-Identifier: Apache-2.0 */ #include "mbed.h" #include "VL53L0X.h" #include "tb6612.h" #include "QEI.h" I2C i2c(D14, D15); VL53L0X vl_sensors[2] = {(&i2c),(&i2c)}; //may need to use 2 not 3 BusOut vl_shutdown(D0,D12); //D0 and D1 do not seem to work on the nucleo Serial usb(USBTX, USBRX, 115200); TB6612 motorL(D2,D4,D5); TB6612 motorR(D9,D10,D11); QEI ENC1(D7,D8,NC,3); //may need to be 12 not 3 Left motor QEI ENC2(D3,D6,NC,3); //may need to be 12 not 3 Right motor Timer Tim1; int main() { ENC1.reset(); //reset encoders ENC2.reset(); const float motorGoal = 500; //declare motor rpm goal float e_speed_1 = 0; //starting error value float e_speed_sum_1 = 0; //starting error sum value float e_speed_pre_1 = 0; //starting error previous value float e_speed_2 = 0; float e_speed_sum_2 = 0; float e_speed_pre_2 = 0; float kp = 1.2; //proportional PID constant float ki = 0.4; //integral PID constant float kd = 0.6; // derivative PID constant float pvSpeed1; // float pvSpeed2; float e_pwm_1 = 0; // initial pwm error float e_pwm_2 = 0; float e_pwm_sum_1 = 0; //initial pwm sum error float e_pwm_sum_2 = 0; float e_pwm_pre_1 = 0; //initial error pwm previous float e_pwm_pre_2 = 0; float SpeedQ1 = 0.5; //initial Q1 value to be removed from starting rpm to begin error calcs float SpeedQ2 = 0.5; float PWMi1 = 0.2; //initial PWM value passed to motors to begin measurement float PWMi2 = 0.2; usb.printf("Multiple VL53L0X\n\n\r"); uint8_t expander_shutdown_mask = 1; for(uint8_t i = 0; i < 2 ; i++) { vl_shutdown = expander_shutdown_mask; expander_shutdown_mask = (expander_shutdown_mask << 1) + 1; vl_sensors[i].init(); vl_sensors[i].setDeviceAddress(0x40 + i); vl_sensors[i].setModeContinuous(); vl_sensors[i].startContinuous(); } uint16_t results[2]; //Sensor code to assign I2C addresses to each sensor Tim1.start(); Tim1.reset(); float PrevTim = 0; motorL.setSpeed(PWMi1); //set motors to starting pwm sped motorR.setSpeed(PWMi2); while(1) { wait_ms(100); /*int EC1P = ENC1.getPulses(); int EC2P = ENC2.getPulses(); DigitalIn button(BUTTON1); DigitalOut led(LED1); led = button; // Equivalent to led.write(button.read())*/ for(uint8_t i = 0; i < 2 ; i++) { results[i] = vl_sensors[i].getRangeMillimeters(); } //sensor measurements //usb.printf("1: %4imm 2: %4imm", results[0], results[1]); //usb.printf("1 Pulses: %d , 2 Pulses: %d\r\n", EC1P, EC2P); /***** motor speed correction***********/ int EC1 = abs(ENC1.getPulses()); //reads encoder pulses int EC2 = abs(ENC2.getPulses()); float Revm1 = EC1*10; //correct rpm 60pprev ec1=pulse per 0.1 sec, ec1*600=pulse per min; revpermin= pulsepermin/pulseperrev float Revm2 = EC2*10; //correct rpm usb.printf(" Rev1 = %f rpm, Rev2 = %f rpm \r\n", Revm1, Revm2); // usb.printf("1 Pulses: %d , 2 Pulses: %d\r\n", EC1, EC2); float Tchange = Tim1.read() - PrevTim; // calculates change in time since last reading PrevTim = Tim1.read(); //sets new time reading as previous ENC1.reset(); //resets encoder pulses ENC2.reset(); /////////////////PID /////////////////////// //e_speed_1 = motorGoal - Revm1; //rpm error //e_speed_2 = motorGoal - Revm2; //e_pwm_1 = ((e_speed_1 * SpeedQ1)/Revm1); //converts rpm error to pwm error value //e_pwm_2 = ((e_speed_2 * SpeedQ2)/Revm2); //float SpeedN1 = ( ((e_pwm_1*kp + e_pwm_sum_1*ki + ((e_pwm_1 - e_pwm_pre_1)/Tchange)*kd)) + 0.5); //need offset value to vary around instead of 0 point //float SpeedN2 = ( ((e_pwm_2*kp + e_pwm_sum_2*ki + ((e_pwm_2 - e_pwm_pre_2)/Tchange)*kd)) + 0.5); //e_pwm_pre_1 = e_pwm_1; //sets previous speed to current //e_pwm_pre_2 = e_pwm_2; //e_pwm_sum_1 += (e_pwm_1 * Tchange); // adds all errors in speed together multiplied by their time period //e_pwm_sum_2 += (e_pwm_2 * Tchange); //usb.printf("SN1 = %f , SN2 = %f \r\n", SpeedN1, SpeedN2); //usb.printf("error1: %f , error2: %f \r\n", e_pwm_1, e_pwm_2); //SpeedQ1 = SpeedN1; // /10; //SpeedQ2 = SpeedN2; // /10; //usb.printf("SQ1 = %f , SQ2 = %f \r\n", SpeedQ1, SpeedQ2); /*if (SpeedQ1 > 10) { SpeedQ1 = 1; } if (SpeedQ2 > 10) { SpeedQ2 = 1; } if (SpeedQ1 < -10) { SpeedQ1 = -1; } if (SpeedQ2 < -10) { SpeedQ2 = -1; } if (e_pwm_sum_1 > 10) { e_pwm_sum_1 = 10; } if (e_pwm_sum_1 < -10) { e_pwm_sum_1 = -10; } if (e_pwm_sum_2 > 10) { e_pwm_sum_2 = 10; } if (e_pwm_sum_2 < -10) { e_pwm_sum_2 = -10; } usb.printf("SQ2 = %f , SQ1 = %f \r\n", SpeedQ1, SpeedQ2); PWMi1 = PWMi1 - SpeedQ1; PWMi2 = PWMi2 - SpeedQ2; motorR.setSpeed(PWMi2); motorL.setSpeed(PWMi1); usb.printf("PWMi1 = %f , PWMi2 = %f \r\n", PWMi1, PWMi2); */ //need to work out ki kp kd /*if (results[0] <= 200 && results[1]<=200){ motorL.setSpeed(SpeedN1); motorR.setSpeed(SpeedN2); usb.printf("forward\n\r"); } if (results[0] <= 200 && results[1] > 200) { motorL.setSpeed(SpeedN1); motorR.setSpeed(-SpeedN2); usb.printf("right turn\n\r"); } if(results[0] > 200 && results[1] <= 200) { motorL.setSpeed(-SpeedN1); motorR.setSpeed(SpeedN2); usb.printf("left turn\n\r"); } if(results[0] > 200 && results[1] > 200) { motorL.setSpeed(-SpeedN1); motorR.setSpeed(-SpeedN2); usb.printf("back\n\r"); }*/ } }