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