![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
working example of final project code (in development)
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
Fork of ES202FinalProject by
main.cpp
- Committer:
- lddevrie
- Date:
- 2018-04-18
- Revision:
- 4:ceae6ab77baf
- Parent:
- 3:e33676098294
File content as of revision 4:ceae6ab77baf:
// Example program to navigate channel // L. DeVries, USNA 4/18/2018 #include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" #include "TCS3472_I2C.h" // PC comms Serial pc(USBTX,USBRX); // Color sensor TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object // 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 DigitalOut myled(LED1); 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_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.stop(); right.stop(); // Initialize color sensor rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor rgb_sensor.setIntegrationTime(100); // Set integration time of sensor, sensor refreshes every ~0.1 seconds // 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) { // read from color sensor rgb_sensor.getAllColors( rgb_readings ); // read the sensor to get red, green, and blue color data along with overall brightness // 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 // 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 // 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) }