working example of final project code (in development)
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Fork of ES202FinalProject by
Diff: main.cpp
- Revision:
- 4:ceae6ab77baf
- Parent:
- 3:e33676098294
--- a/main.cpp Tue Mar 27 13:22:52 2018 +0000 +++ b/main.cpp Wed Apr 18 17:12:05 2018 +0000 @@ -1,120 +1,140 @@ +// Example program to navigate channel +// L. DeVries, USNA 4/18/2018 + #include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" +#include "TCS3472_I2C.h" -// Test change + +// PC comms +Serial pc(USBTX,USBRX); + +// Color sensor +TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object -Serial pc(USBTX,USBRX); -//servos +// Hall effect sensor +DigitalIn hall(p21); +DigitalOut hallpwr(p22); + +// Sonar sensor +AnalogIn sonar(p19); // range sensor 9.8 mV/inch + +// Servos ContinuousServo left(p23); ContinuousServo right(p26); + //encoders Tach tLeft(p17,64); Tach tRight(p13,64); -// speed controller -Ticker spdCtrl; +// Speed controller DigitalOut myled(LED1); -// Function definition Prototypes (declarations) -void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists + +float om; // turn rate +float sp; // forward speed +float whl_rad = 3.1; // wheel radius in centimeters +float baseL = 11.0; // wheelbase in centimeters -float vl; -float vr; -float Ts = 0.01; // Control update period (seconds) (100 Hz equivalent) -float speedL,speedR; -float dcL; -float dcR; -float dcpL; -float dcpR; -float errpL; -float errpR; -float spdErrL; -float spdErrR; -float kp = 0.1; -float ki = 0.01; -float whl_rad = 3.1; // wheel radius in centimeters -float baseL = 11.0; // wheelbase in centimeters -float om = 0.0; // angular velocity -float sp = 0.0; // forward speed in cm/s +float vl_des = 0.0; // desired left wheel speed +float vr_des = 0.0; // desired right wheel speed +float vl = 0.0; // measured left wheel speed +float vr = 0.0; // measured right wheel speed +float vl_err_int = 0.0; // integral term left wheel +float vr_err_int = 0.0; // integral term right wheel +float kp = 0.5; // proportional gain on wheel speed controllers +float ki = 0.1; // integral gain on wheel speed controllers +float dcL; // duty cycle left wheel +float dcR; // duty cycle right wheel + + +float dist = 0.0; // ultrasonic sensor reading +int rgb_readings[4]; // declare a 4 element array to store RGB sensor readings +int shade = 0.0; // brightness level we want to maintain in the course + int main() { + // set pull up resistor on hall effect sensor + hall.mode(PullUp); + hallpwr = 0; // turn off hall effect power + int iters = 50; // iterations to evaluate test code + //wait for MATLAB command - left.speed(0.0); - right.speed(0.0); - while(1) { - spdErrL = 0.0; - spdErrR = 0.0; - dcL = 0.0; - dcR = 0.0; - errpL = 0.0; - dcpL = 0.0; - errpR = 0.0; - dcpR = 0.0; - pc.scanf("%f,%f,%f,%f",&sp,&om,&kp,&ki); + left.stop(); + right.stop(); - spdCtrl.attach(&ctrCode,Ts); + // Initialize color sensor + rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor + rgb_sensor.setIntegrationTime(100); // Set integration time of sensor, sensor refreshes every ~0.1 seconds - for(int i=1; i<=200; i++) { - - vl = (2*sp-om*baseL)/(2*whl_rad); // desired left wheel speed in cm/s - vr = (2*sp+om*baseL)/(2*whl_rad); // desired right wheel speed in cm/s - - - - wait_ms(100); - pc.printf("%f,%f,%f,%f\n", speedL,speedR,dcL,dcR); - } - - - - spdCtrl.detach(); - left.stop(); - right.stop(); - }// end while(1) -} + // Calibrate color sensor to desired gradient color + pc.printf("Calibrating color sensor with %d measurements...\r\n",iters); + for(int i=0; i<iters; i++) { + rgb_sensor.getAllColors( rgb_readings ); // read the sensor to get red, green, and blue color data along with overall brightness + shade = shade + rgb_readings[3]; + wait(0.1); + pc.printf( "Measurement:\t%d\r\n", rgb_readings[3]); // Print data to serial port + myled = !myled; // toggle LED 2 to indicate control update + } + shade = shade/iters; + pc.printf( "Calibration complete: Brightness: %d\r\n", shade); // Print data to serial port + pc.printf("Beginning channel navigation in 5 seconds...\r\n"); + wait(5.0); + + + while(1) { -// Additional function definitions -void ctrCode() // function to attach to ticker -{ - myled = !myled; // toggle LED 2 to indicate control update - - speedL = tLeft.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second - speedR = tRight.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second - - // compute error - spdErrL = vl-speedL; - spdErrR = vr-speedR; - - // PI controller - dcL = dcpL+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrL+(Ts/2.0*(ki/kp)-1.0)*errpL); // compute duty cycle for motor using PI control scheme - dcR = dcpR+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrR+(Ts/2.0*(ki/kp)-1.0)*errpR); // compute duty cycle for motor using PI control scheme + // read from color sensor + rgb_sensor.getAllColors( rgb_readings ); // read the sensor to get red, green, and blue color data along with overall brightness - // enforce duty cycle saturation - if(dcL>1.0) { - dcL = 1.0; - } else if(dcL<0.0) { - dcL = 0.0; - } - if(dcR>1.0) { - dcR = 1.0; - } else if(dcR<0.0) { - dcR = 0.0; - } + // read from wheel speed sensors + vl = tLeft.getSpeed()*2.0*3.14; // rad/s + vr = tRight.getSpeed()*2.0*3.14; // rad/s + + // proportional control on steering based on shade measurement + om = 1.0/2000.0*(float)(shade - rgb_readings[3]); // initial gain based on color sweep numbers + sp = 3.0; // cm/s, arbitrarily chosen - // motor control - left.speed(dcL); // first input is the motor channel, second is duty cycle - right.speed(-dcR); // first input is the motor channel, second is duty cycle - - // Age variables - errpL = spdErrL; // age speed error variable - dcpL = dcL; // age duty cycle variable - - errpR = spdErrR; // age speed error variable - dcpR = dcR; // age duty cycle variable + // desired wheel speeds from differential cart model + vl_des = (2.0*sp+om*baseL)/(2.0*whl_rad); // desired left wheel speed + vr_des = (2.0*sp-om*baseL)/(2.0*whl_rad); // desired right wheel speed -} // end ctrCode() + // PI controller tracking desired wheel speed + dcL = kp*(vl_des-vl) + ki*vl_err_int; + dcR = kp*(vr_des-vr) + ki*vr_err_int; + + if(dcL>1.0) { + dcL = 1.0; + } + else if(dcL<-1.0) { + dcL = -1.0; + } + if(dcR>1.0) { + dcR = 1.0; + } + else if(dcR<-1.0) { + dcR = -1.0; + } + + // motor control + left.speed(dcL); // first input is the motor channel, second is duty cycle + right.speed(dcR); // first input is the motor channel, second is duty cycle + + wait(0.02); + + // Euler integration of error integral + vl_err_int = vl_err_int + (vl_des-vl)*0.02; + vl_err_int = vl_err_int + (vl_des-vl)*0.02; + + + // left.stop(); + // right.stop(); + + + }// end while(1) +} \ No newline at end of file