File content as of revision 2:2d87957b577b:

#include "mbed.h"
#include "rtos.h"
#include "SongPlayer.h"
 
RawSerial  pc(USBTX, USBRX);
RawSerial  dev(p28,p27);
DigitalOut led1(LED1);
DigitalOut led4(LED4);

// Set up the motor pins
PwmOut motorACtrl(p21);
PwmOut motorBCtrl(p22);
DigitalOut backA(p20);
DigitalOut fwdA(p19);
DigitalOut stby(p18);
DigitalOut fwdB(p17);
DigitalOut backB(p16);
Timer t;

// Set up Hall-effect control
DigitalIn EncR(p13); // right motor
DigitalIn EncL(p14); // left motor
int oldEncR = 0; // Previous encoder values
int oldEncL = 0;
int ticksR = 0; // Encoder wheel state change counts
int ticksL = 0;
int E; // Error  between the 2 wheel speeds
float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
float speedR = 0; //Same for right wheel
Ticker Sampler; //Interrupt Routine to sample encoder ticks.

// Distance sensor stuff
AnalogIn irSensor(p15);         // Sensor
bool emergencyStop = false;

float timeout = 0.05f;
int  count,ended;
char buf[256];
char motorCmd[] = "stop";
 
/**
 *  Get the reply string from the WiFi module
 */
void getreply()
{
    memset(buf, '\0', sizeof(buf));
    t.start();
    ended=0;
    count=0;
    while(!ended) {
        if(dev.readable()) {
            buf[count] = dev.getc();
            count++;
        }
        if(t.read() > timeout) {
            ended = 1;
            t.stop();
            t.reset();
        }
    }
}

/**
 *
 */
void getMotorCommand() {
    int index = 3;
    // iterate backwards through the message buffer
    for (int i = sizeof(buf) - 1; i > 2; i--){
        // Check if the current character is a null
        if (buf[i] != '\0') {
            motorCmd[index] = buf[i - 2];   // Copy the character
            if (index == 0){
                break;      // Break if we have completed getting the command
            } else {
                index--;    // Decrement otherwise
            }
        }
    }
}

/**
 *  Run the motor based on the command passed through the serial interface.
 */
void runMotor(){
    if (strcmp(motorCmd, (char *)"fwrd") == 0) {
           stby = 1;        // Start H-bridge driver
           fwdA = 1;
           fwdB = 1;
           backA = 0;
           backB = 0;
           motorACtrl = speedL;
           motorBCtrl = speedR;
    } else if (strcmp(motorCmd, (char *)"back") == 0) {
           stby = 1;        // Start H-bridge driver
           fwdA = 0;
           fwdB = 0;
           backA = 1;
           backB = 1;
           motorACtrl = speedL;
           motorBCtrl = speedR;
    } else if (strcmp(motorCmd, (char *)"left") == 0) {
           stby = 1;        // Start H-bridge driver
           fwdA = 1;
           fwdB = 0;
           backA = 0;
           backB = 1;
           motorACtrl = speedL;
           motorBCtrl = speedR;
    } else if (strcmp(motorCmd, (char *)"rght") == 0) {
           stby = 1;        // Start H-bridge driver
           fwdA = 0;
           fwdB = 1;
           backA = 1;
           backB = 0;
           motorACtrl = speedL;
           motorBCtrl = speedR;
    } else if (strcmp(motorCmd, (char *)"stop") == 0) {
           stby = 1;        // Start H-bridge driver
           fwdA = 0;
           fwdB = 0;
           backA = 0;
           backB = 0;
           motorACtrl = 0.0f;   // Set speed to 0
           motorBCtrl = 0.0f;
           stby = 0;        // Turn off H-bridge driver
    }
}

/**
 * Sample encoders and find error on right wheel. Assume Left wheel is always 
 * correct speed.
 */
void sampleEncoder()
{
    // Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
    if(((strcmp(motorCmd, (char *)"fwrd") == 0) || 
            (strcmp(motorCmd, (char *)"back") == 0)) && (ticksL != 0 || ticksR != 0)) { 
        E = ticksL - ticksR; // Find error
        pc.printf("Error = %d\n\r", E);
        speedR = speedR + float(E) / 255.0f; // Assign a scaled increment to the right wheel based on error
        pc.printf("Updating right motor speed to %f\n\r", speedR);
        runMotor();
    }
    ticksR = 0; //Restart the counters
    ticksL = 0;
}

void dev_recv()
{
    led1 = !led1;
    getreply();         // Get the serial message
    getMotorCommand();  // Extract the motor command from the message
    pc.printf(buf);
    // Check whether we are in emergency stop mode (robot is too close to a wall)
    if (emergencyStop == true && strcmp(motorCmd, (char *)"back") != 0) {
        // If so, only allow a "back" command
        strcpy(motorCmd, "stop");
    }
    runMotor();         // Run the motor based on the command
}
 
void pc_recv()
{
    led4 = !led4;
    while(pc.readable()) {
        dev.putc(pc.getc());
    }
}

/**
 *  Do distance measurement
 */
void distance_thread(void const *args) {
    SongPlayer speaker(p23);
    float alert1[2] = {440.0, 0.0};
    float alert1duration[2] = {0.05, 0.0};
    float alert2[2] = {880.0, 0.0};
    float alert2duration[2] = {0.05, 0.0};
    float alert3[2] = {1760.0, 0.0};
    float alert3duration[2] = {0.05, 0.0};
    while(1){
        float reading = irSensor;
        pc.printf("Distance: %f\n\r", reading);
        if (reading > 0.7f) {
            if (strcmp(motorCmd, (char *)"fwrd") == 0){
                strcpy(motorCmd, "stop");
            }
            emergencyStop = true;
            runMotor();
        } else if (reading > 0.65f) {
            speaker.PlaySong(alert3, alert3duration);
            emergencyStop = false;
            Thread::wait(60);
        } else if (reading > 0.55f) {
            speaker.PlaySong(alert2, alert2duration);
            emergencyStop = false;
            Thread::wait(65);
        } else if (reading > 0.45f) {
            speaker.PlaySong(alert1, alert1duration);
            emergencyStop = false;
            Thread::wait(70);
        } else {
            Thread::wait(20);       // Wait 20 ms
        }
    }
}
 
int main()
{
    pc.baud(115200);
    dev.baud(115200);
 
    pc.attach(&pc_recv, Serial::RxIrq);
    dev.attach(&dev_recv, Serial::RxIrq);
    
    // Initialize the motors
    motorACtrl.period(0.01f);
    motorBCtrl.period(0.01f);
    speedL = 0.9f;
    speedR = 0.9f;
    
    // Initialize hall-effect control
    Sampler.attach(&sampleEncoder, .1); //Sampler uses sampleEncoder function every 20ms
    EncL.mode(PullUp); // Use internal pullups
    EncR.mode(PullUp);
    
    Thread t0(distance_thread);
 
    while(1) {
        if((strcmp(motorCmd, (char *)"fwrd") == 0) || 
            (strcmp(motorCmd, (char *)"back") == 0)) { // Only increment tick values if moving forward or reverse
            int tmpL = EncL;    // Sample the current value of the encoders
            int tmpR = EncR;
            pc.printf("Encoder values %d, %d\n\r", tmpL, tmpR);
            if(tmpL != oldEncL) { // Increment ticks every time the state has switched. 
                ticksL++;
                oldEncL = tmpL;
            }
            if(tmpR != oldEncR) {
                ticksR++;
                oldEncR = tmpR;
            }
        }
        wait(0.02f);        // Sleep 20 ms
    }
}