File content as of revision 5:78264cdf3572:

// 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)
}