USNA ES202 / Mbed 2 deprecated ES202_FinalProject_workingExample

working example of final project code (in development)

Dependencies:   ContinuousServo TCS3472_I2C Tach mbed

Fork of ES202FinalProject by John Donnal

main.cpp@4:ceae6ab77baf, 2018-04-18 (annotated)

Committer:
lddevrie
Date:
Wed Apr 18 17:12:05 2018 +0000
Revision:
4:ceae6ab77baf
Parent:
3:e33676098294 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lddevrie 4:ceae6ab77baf 1 // Example program to navigate channel
lddevrie 4:ceae6ab77baf 2 // L. DeVries, USNA 4/18/2018
lddevrie 4:ceae6ab77baf 3
jdonnal 0:ac6a1723d393 4 #include "mbed.h"
jdonnal 0:ac6a1723d393 5 #include "ContinuousServo.h"
jdonnal 0:ac6a1723d393 6 #include "Tach.h"
lddevrie 4:ceae6ab77baf 7 #include "TCS3472_I2C.h"
jdonnal 0:ac6a1723d393 8
lddevrie 4:ceae6ab77baf 9
lddevrie 4:ceae6ab77baf 10 // PC comms
lddevrie 4:ceae6ab77baf 11 Serial pc(USBTX,USBRX);
lddevrie 4:ceae6ab77baf 12
lddevrie 4:ceae6ab77baf 13 // Color sensor
lddevrie 4:ceae6ab77baf 14 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
lddevrie 2:f72b9dd19159 15
lddevrie 4:ceae6ab77baf 16 // Hall effect sensor
lddevrie 4:ceae6ab77baf 17 DigitalIn hall(p21);
lddevrie 4:ceae6ab77baf 18 DigitalOut hallpwr(p22);
lddevrie 4:ceae6ab77baf 19
lddevrie 4:ceae6ab77baf 20 // Sonar sensor
lddevrie 4:ceae6ab77baf 21 AnalogIn sonar(p19); // range sensor 9.8 mV/inch
lddevrie 4:ceae6ab77baf 22
lddevrie 4:ceae6ab77baf 23 // Servos
jdonnal 0:ac6a1723d393 24 ContinuousServo left(p23);
jdonnal 0:ac6a1723d393 25 ContinuousServo right(p26);
lddevrie 4:ceae6ab77baf 26
jdonnal 0:ac6a1723d393 27 //encoders
jdonnal 0:ac6a1723d393 28 Tach tLeft(p17,64);
jdonnal 0:ac6a1723d393 29 Tach tRight(p13,64);
jdonnal 0:ac6a1723d393 30
lddevrie 4:ceae6ab77baf 31 // Speed controller
lddevrie 3:e33676098294 32 DigitalOut myled(LED1);
lddevrie 3:e33676098294 33
lddevrie 4:ceae6ab77baf 34
lddevrie 4:ceae6ab77baf 35 float om; // turn rate
lddevrie 4:ceae6ab77baf 36 float sp; // forward speed
lddevrie 4:ceae6ab77baf 37 float whl_rad = 3.1; // wheel radius in centimeters
lddevrie 4:ceae6ab77baf 38 float baseL = 11.0; // wheelbase in centimeters
lddevrie 3:e33676098294 39
lddevrie 3:e33676098294 40
lddevrie 4:ceae6ab77baf 41 float vl_des = 0.0; // desired left wheel speed
lddevrie 4:ceae6ab77baf 42 float vr_des = 0.0; // desired right wheel speed
lddevrie 4:ceae6ab77baf 43 float vl = 0.0; // measured left wheel speed
lddevrie 4:ceae6ab77baf 44 float vr = 0.0; // measured right wheel speed
lddevrie 4:ceae6ab77baf 45 float vl_err_int = 0.0; // integral term left wheel
lddevrie 4:ceae6ab77baf 46 float vr_err_int = 0.0; // integral term right wheel
lddevrie 4:ceae6ab77baf 47 float kp = 0.5; // proportional gain on wheel speed controllers
lddevrie 4:ceae6ab77baf 48 float ki = 0.1; // integral gain on wheel speed controllers
lddevrie 4:ceae6ab77baf 49 float dcL; // duty cycle left wheel
lddevrie 4:ceae6ab77baf 50 float dcR; // duty cycle right wheel
lddevrie 4:ceae6ab77baf 51
lddevrie 4:ceae6ab77baf 52
lddevrie 4:ceae6ab77baf 53 float dist = 0.0; // ultrasonic sensor reading
lddevrie 4:ceae6ab77baf 54 int rgb_readings[4]; // declare a 4 element array to store RGB sensor readings
lddevrie 4:ceae6ab77baf 55 int shade = 0.0; // brightness level we want to maintain in the course
lddevrie 4:ceae6ab77baf 56
lddevrie 3:e33676098294 57
lddevrie 3:e33676098294 58 int main()
lddevrie 3:e33676098294 59 {
lddevrie 4:ceae6ab77baf 60 // set pull up resistor on hall effect sensor
lddevrie 4:ceae6ab77baf 61 hall.mode(PullUp);
lddevrie 4:ceae6ab77baf 62 hallpwr = 0; // turn off hall effect power
lddevrie 4:ceae6ab77baf 63 int iters = 50; // iterations to evaluate test code
lddevrie 4:ceae6ab77baf 64
lddevrie 3:e33676098294 65 //wait for MATLAB command
lddevrie 4:ceae6ab77baf 66 left.stop();
lddevrie 4:ceae6ab77baf 67 right.stop();
lddevrie 3:e33676098294 68
lddevrie 4:ceae6ab77baf 69 // Initialize color sensor
lddevrie 4:ceae6ab77baf 70 rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor
lddevrie 4:ceae6ab77baf 71 rgb_sensor.setIntegrationTime(100); // Set integration time of sensor, sensor refreshes every ~0.1 seconds
lddevrie 3:e33676098294 72
lddevrie 4:ceae6ab77baf 73 // Calibrate color sensor to desired gradient color
lddevrie 4:ceae6ab77baf 74 pc.printf("Calibrating color sensor with %d measurements...\r\n",iters);
lddevrie 4:ceae6ab77baf 75 for(int i=0; i<iters; i++) {
lddevrie 4:ceae6ab77baf 76 rgb_sensor.getAllColors( rgb_readings ); // read the sensor to get red, green, and blue color data along with overall brightness
lddevrie 4:ceae6ab77baf 77 shade = shade + rgb_readings[3];
lddevrie 4:ceae6ab77baf 78 wait(0.1);
lddevrie 4:ceae6ab77baf 79 pc.printf( "Measurement:\t%d\r\n", rgb_readings[3]); // Print data to serial port
lddevrie 4:ceae6ab77baf 80 myled = !myled; // toggle LED 2 to indicate control update
lddevrie 4:ceae6ab77baf 81 }
lddevrie 4:ceae6ab77baf 82 shade = shade/iters;
lddevrie 4:ceae6ab77baf 83 pc.printf( "Calibration complete: Brightness: %d\r\n", shade); // Print data to serial port
lddevrie 4:ceae6ab77baf 84 pc.printf("Beginning channel navigation in 5 seconds...\r\n");
lddevrie 4:ceae6ab77baf 85 wait(5.0);
lddevrie 4:ceae6ab77baf 86
lddevrie 4:ceae6ab77baf 87
lddevrie 4:ceae6ab77baf 88 while(1) {
lddevrie 3:e33676098294 89
lddevrie 3:e33676098294 90
lddevrie 4:ceae6ab77baf 91 // read from color sensor
lddevrie 4:ceae6ab77baf 92 rgb_sensor.getAllColors( rgb_readings ); // read the sensor to get red, green, and blue color data along with overall brightness
lddevrie 3:e33676098294 93
lddevrie 4:ceae6ab77baf 94 // read from wheel speed sensors
lddevrie 4:ceae6ab77baf 95 vl = tLeft.getSpeed()*2.0*3.14; // rad/s
lddevrie 4:ceae6ab77baf 96 vr = tRight.getSpeed()*2.0*3.14; // rad/s
lddevrie 4:ceae6ab77baf 97
lddevrie 4:ceae6ab77baf 98 // proportional control on steering based on shade measurement
lddevrie 4:ceae6ab77baf 99 om = 1.0/2000.0*(float)(shade - rgb_readings[3]); // initial gain based on color sweep numbers
lddevrie 4:ceae6ab77baf 100 sp = 3.0; // cm/s, arbitrarily chosen
lddevrie 3:e33676098294 101
lddevrie 4:ceae6ab77baf 102 // desired wheel speeds from differential cart model
lddevrie 4:ceae6ab77baf 103 vl_des = (2.0*sp+om*baseL)/(2.0*whl_rad); // desired left wheel speed
lddevrie 4:ceae6ab77baf 104 vr_des = (2.0*sp-om*baseL)/(2.0*whl_rad); // desired right wheel speed
lddevrie 3:e33676098294 105
lddevrie 3:e33676098294 106
lddevrie 4:ceae6ab77baf 107 // PI controller tracking desired wheel speed
lddevrie 4:ceae6ab77baf 108 dcL = kp*(vl_des-vl) + ki*vl_err_int;
lddevrie 4:ceae6ab77baf 109 dcR = kp*(vr_des-vr) + ki*vr_err_int;
lddevrie 4:ceae6ab77baf 110
lddevrie 4:ceae6ab77baf 111 if(dcL>1.0) {
lddevrie 4:ceae6ab77baf 112 dcL = 1.0;
lddevrie 4:ceae6ab77baf 113 }
lddevrie 4:ceae6ab77baf 114 else if(dcL<-1.0) {
lddevrie 4:ceae6ab77baf 115 dcL = -1.0;
lddevrie 4:ceae6ab77baf 116 }
lddevrie 4:ceae6ab77baf 117 if(dcR>1.0) {
lddevrie 4:ceae6ab77baf 118 dcR = 1.0;
lddevrie 4:ceae6ab77baf 119 }
lddevrie 4:ceae6ab77baf 120 else if(dcR<-1.0) {
lddevrie 4:ceae6ab77baf 121 dcR = -1.0;
lddevrie 4:ceae6ab77baf 122 }
lddevrie 4:ceae6ab77baf 123
lddevrie 4:ceae6ab77baf 124 // motor control
lddevrie 4:ceae6ab77baf 125 left.speed(dcL); // first input is the motor channel, second is duty cycle
lddevrie 4:ceae6ab77baf 126 right.speed(dcR); // first input is the motor channel, second is duty cycle
lddevrie 4:ceae6ab77baf 127
lddevrie 4:ceae6ab77baf 128 wait(0.02);
lddevrie 4:ceae6ab77baf 129
lddevrie 4:ceae6ab77baf 130 // Euler integration of error integral
lddevrie 4:ceae6ab77baf 131 vl_err_int = vl_err_int + (vl_des-vl)*0.02;
lddevrie 4:ceae6ab77baf 132 vl_err_int = vl_err_int + (vl_des-vl)*0.02;
lddevrie 4:ceae6ab77baf 133
lddevrie 4:ceae6ab77baf 134
lddevrie 4:ceae6ab77baf 135 // left.stop();
lddevrie 4:ceae6ab77baf 136 // right.stop();
lddevrie 4:ceae6ab77baf 137
lddevrie 4:ceae6ab77baf 138
lddevrie 4:ceae6ab77baf 139 }// end while(1)
lddevrie 4:ceae6ab77baf 140 }