File content as of revision 5:a8f6ac485b5d:

/**
 * Drive a robot forwards or backwards by using a PID controller to vary
 * the PWM signal to H-bridges connected to the motors to attempt to maintain
 * a constant velocity.
 */
 /*Sources:     mbed Rover cookbook page: https://developer.mbed.org/cookbook/mbed-Rover
                InterruptIn handbook page: https://developer.mbed.org/handbook/InterruptIn 
 */
 
#include "mbed.h"
#include "motordriver.h"
#include "PID.h"
#include "uLCD_4DGL.h"
#include "SongPlayer.h"
//one full revolution=193 counts


class Counter {
    public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    int read() {
        return _count;
    }
 
private:
    InterruptIn _interrupt;
    volatile int _count;
};

int distTransform(float input) {
    if (input>3) return 6;
    else if (input>2.5) return 8;
    else if (input>2) return 10;
    else if (input>1.5) return 14;
    else if (input>1.1) return 22;
    else if (input>0.9) return 27;
    else if (input>0.75) return 35;
    else if (input>0.6) return 45;
    else if (input>0.5) return 60;
    else if (input>0.4) return 75;
    else return 99;    
}

Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake
Counter leftPulses(p9), rightPulses (p10);
//Tuning parameters calculated from step tests;
//see http://mbed.org/cookbook/PID for examples.
//PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td old
PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
DigitalOut led(LED1), led2(LED2);
AnalogIn ain(p15);
uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
float note[18]= {3520,  3135.96,    2637.02,    2093,   2349.32,    3951.07,    2793.83,    4186.01,    3520,   3135.96,    2637.02,    2093,   2349.32,    3951.07,    2793.83,    4186.01};
float duration[18]= {0.1,   0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1,    0.1};

int main() {
    uLCD.printf("\n I am on an\n important\n mission!"); //Default Green on black text
    
    //Input and output limits have been determined
    //empirically with the specific motors being used.
    //Change appropriately for different motors.
    //Input  units: counts per second.
    //Output units: PwmOut duty cycle as %.
    //Default limits are for moving forward.
    leftPid.setInputLimits(0, 3000);
    leftPid.setOutputLimits(0.0, 0.8);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3000);
    rightPid.setOutputLimits(0.0, 0.8);
    rightPid.setMode(AUTO_MODE);
    Serial pc(USBTX, USBRX);
    SongPlayer mySpeaker(p23);
    
    int leftPrevPulses  = 0, leftActPulses=0; //The previous reading of how far the left wheel
    float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
    int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
    float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
    
    int distance     = 0; //Number of pulses to travel.
    led=0;
    led2=0;
    uLCD.baudrate(3000000);

    wait(1); //Wait a few seconds before we start moving.
    uLCD.cls();
    uLCD.locate(1,2);
    uLCD.printf("I must find\n Ben Kenobi!");

    //Velocity to mantain in pulses per second.
    leftPid.setSetPoint(1000);
    rightPid.setSetPoint(1000);

    while (1) {
        
        if (distTransform(ain)>50) {    //going straight line
            leftActPulses=leftPulses.read();
            leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
            leftPrevPulses = leftActPulses;
            rightActPulses=rightPulses.read();
            rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
            rightPrevPulses = rightActPulses;
    
            leftPid.setProcessValue(fabs(leftVelocity));
            leftMotor.speed(leftPid.compute());
            rightPid.setProcessValue(fabs(rightVelocity));
            rightMotor.speed(rightPid.compute());

        } else { //Don't go straight, turn!
            leftMotor.stop(0.5);
            rightMotor.stop(0.5);
            led2=1;
            uLCD.cls();
            uLCD.locate(1,2);
            //uLCD.printf("He is not here!");
            mySpeaker.PlaySong(note,duration);
            uLCD.filled_circle(64, 64, 63, RED);
            wait(0.2);
            uLCD.filled_circle(64, 64, 63, 0x0000FF);
            wait(0.5);
            uLCD.filled_circle(64, 64, 63, RED);
            wait(0.3);
            //wait(0.5);
            leftPid.setSetPoint(-500);
            rightPid.setSetPoint(500);
            
            leftActPulses=leftPulses.read();
            rightActPulses=rightPulses.read();
            distance=leftActPulses+100;
            while (leftActPulses<distance) { //I'm turning!
                leftMotor.speed(-0.5);
                rightMotor.speed(0.5);
                leftActPulses=leftPulses.read();
                rightActPulses=rightPulses.read();
                
                wait(0.005);
                
            }//Turning while end
            leftMotor.stop(0.5);
            rightMotor.stop(0.5);          
            wait(0.1);
            led2=0;
            uLCD.cls();
            uLCD.locate(1,2);
            uLCD.printf("I must find\n Ben Kenobi!");
            
            leftPid.setSetPoint(1000);
            rightPid.setSetPoint(1000);
        
        } //Going straight/turning if end 
        
        //pc.printf("\n%i", distTransform(ain));
        //uLCD.locate(1,1);
        //uLCD.printf("Distance: %i cm", distTransform(ain));
        wait(0.01);
        led=!led;
        
    }   //end of big while loop

}