File content as of revision 1:8638bdaf172b:

#include "mbed.h"
#include "rtos.h"
#include "Motor.h"
#include "Servo.h"
#include "LSM9DS1.h"

//MOTOR PINS
Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev / 
//BLUETOOTH PINS
Serial bt(p28,p27);
//LEFT SONAR
DigitalOut triggerL(p14);
DigitalIn echoL(p12);
//RIGHT SONAR
DigitalOut triggerR(p13);
DigitalIn echoR(p11);
//SERVO PINS
Servo angle(p21);
//IMU PINS
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);

int correction = 0;
Timer sonar;

Serial pc(USBTX, USBRX);
Timer timestamp;
// Set up Hall-effect control
DigitalIn EncR(p19); // right motor
DigitalIn EncL(p20); // 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.
int Instr = 0;


//Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
void sampleEncoder()
{
    if((Instr == 1 || Instr == 2) && ticksL != 0) { //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 

        E = ticksL - ticksR; //Find error
        speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
        rwheel.speed(speedR);
    }
    ticksR = 0; //Restart the counters
    ticksL = 0;
}

/*  move(float, int)
    Usage: Moves the robot forward in a straight line at a constant speed.
    s is a float from -1 to 1 representing the speed to move. 
    A negative speed is used for backwards movement.
    t is the time in milliseconds to move.
*/
void move(float s, int t)
{
    Timer timer;
    //Start the timer
    timer.start();
    //Set the robot in motion at speed s
    rwheel.speed(s);
    lwheel.speed(s);
    //Give other threads priority until the elapsed time
    while(timer.read_ms() < t); //Thread::yield();
    //Stop the robot
    rwheel.speed(0);
    lwheel.speed(0);
}

/*  turn(float,int)
    Usage: Turns the robot about its center at constant angular speed.
    s is a float from -1 to 1 representing the speed to turn.
    A positive s indicates a clockwise rotation.
    A negative s indicates a counter-clockwise rotation. 
*/
void turn(float s, int t)
{
    Timer timer;
    //Start the timer
    timer.start();
    //Set the robot in motion turning at speed s
    rwheel.speed(-1*s);
    lwheel.speed(s);
    //Give other threads priority until the elapsed time
    while(timer.read_ms() < t);// Thread::yield();
    //Stop the robot
    rwheel.speed(0);
    lwheel.speed(0);
}

int ping(int i)
{
    int distance = 0;
    switch(i)
    {
        
        case(1): //Ping Left Sonar
        // trigger sonar to send a ping
        triggerL = 1;
        sonar.reset();
        wait_us(10.0);
        triggerL = 0;
        //wait for echo high
        while (echoL==0) {};
        //echo high, so start timer
        sonar.start();
        //wait for echo low
        while (echoL==1) {};
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance = (sonar.read_us()-correction)/58.0;
        //wait so that any echo(s) return before sending another ping
        wait(0.015);
        break;
        case(2): //Ping Right Sonar
        // trigger sonar to send a ping
        triggerR = 1;
        sonar.reset();
        wait_us(10.0);
        triggerR = 0;
        //wait for echo high
        while (echoR==0) {};
        //echo high, so start timer
        sonar.start();
        //wait for echo low
        while (echoR==1) {};
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance = (sonar.read_us()-correction)/58.0;
        //wait so that any echo(s) return before sending another ping
        wait(0.015);
        break;
        default:
        break;
    }
    
    return distance;
}

int main() 
{
    timestamp.start(); 
    // Initialize hall-effect control
    Sampler.attach(&sampleEncoder, 0.02); //Sampler uses sampleEncoder function every 20ms
    EncL.mode(PullUp); // Use internal pullups
    EncR.mode(PullUp);
    while(1) {
                Instr = 1;
                switch (Instr) {
                    case 0://Stop
                        lwheel.speed(0);
                        rwheel.speed(0);
                        break;
                    case 1://Forward

                        speedL = 1;
                        //speedR = speedL;
                        lwheel.speed(speedL);
                        rwheel.speed(speedR);
                        break;
                    case 2://Reverse
                        speedL = -0.5;
                        speedR = speedL;
                        lwheel.speed(speedL);
                        rwheel.speed(speedR);
                        break;
                    case 3://Clockwise Turn
                        speedL = 0.3;
                        lwheel.speed(speedL);
                        rwheel.speed(-speedL);
                        break;
                    case 4://CCW Turn
                        speedL = -0.3;
                        lwheel.speed(-speedL);
                        rwheel.speed(speedL);
                        break;
                }//End Switch

        if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
            if(EncR != oldEncR) { // Increment ticks every time the state has switched. 
                ticksR++;
                oldEncR = EncR;

            }
            if(EncL != oldEncR) {
                ticksL++;
                oldEncL = EncL;
            }
            pc.printf("tickR: %i \n\r",ticksR);
            pc.printf("tickL: %i \n\r",ticksL);
        }
    }
}