File content as of revision 2:64585b8d8404:

#include "mbed.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(p9, p10, 0xD6, 0x3C);

int correction = 0; //Used to adjust software delay for sonar measurement
Timer sonar;

Serial pc(USBTX, USBRX);
Timer timestamp;
// Set up Hall-effect control
InterruptIn EncR(p25);
InterruptIn EncL(p24);
int ticksR = 0; // Encoder wheel state change counts
int ticksL = 0;
float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
float speedR = 0; //Same for right wheel
int dirL = 1; //1 for fwd, -1 for reverse
int dirR = 1; //1 for fwd, -1 for reverse
Ticker Sampler; //Interrupt Routine to sample encoder ticks.
int tracking = 0; //Flag used to indicate right wheel correction

//Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
void sampleEncoder()
{
    float E; // Error  between the 2 wheel speeds
    if(tracking) { //Right wheel "tracks" left wheel if enabled. 

        E = ticksL - ticksR; //Find error
        //E = (ticksL+1)/(ticksR+1);
        if (dirL == 1)
        {
            speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
            //speedR = E*speedR;
            if (speedR < 0) speedR = 0;
        }
        else if (dirL == -1)
        {
            speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
            //speedR = E*speedR;
            if (speedR > 0) speedR = 0;
        }
        rwheel.speed(speedR);
    }
    //pc.printf("tickL: %i   tickR: %i  Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
    ticksR = 0; //Restart the counters
    ticksL = 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;
}

void incTicksR()
{
    ticksR++;
}

void incTicksL()
{
    ticksL++;
}

int main() 
{
    char cmd;
    timestamp.start(); 
    angle = 0.0f;
    // Initialize hall-effect control
    Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
    EncL.mode(PullUp); // Use internal pullups
    EncR.mode(PullUp);
    EncR.rise(&incTicksR);
    EncL.rise(&incTicksL);
    while(1) 
    {
        //Check if char is ready to be read and put into command buffer;
        if(bt.getc() =='!')
        {
            cmd = bt.getc();
            switch (cmd)
            {
                case 'B':
                pc.printf("Got Command B!\n\r");
                break;
                
                case 'S': //Stop moving
                pc.printf("Got Command STOP!\n\r");
                tracking = 0; //Turn off wheel feedback updates
                speedL = 0;
                speedR = 0;
                lwheel.speed(0);
                rwheel.speed(0);
                break;
                
                case 'F': //Forward
                pc.printf("Got Command FORWARD!\n\r");
                dirL = 1;
                dirR = 1;
                speedL = 0.9f;
                speedR = 0.9f;
                rwheel.speed(speedR);
                lwheel.speed(speedL);
                tracking = 1; //Turn on wheel feedback updates
                break;
                
                case 'R': //Reverse
                pc.printf("Got Command REVERSE!\n\r");
                dirL = -1;
                dirR = -1;
                speedL = -0.9f;
                speedR = -0.9f;
                rwheel.speed(speedR);
                lwheel.speed(speedL);
                tracking = 1;
                break;
                
                default:
                pc.printf("Unknown Command!\n\r");
                break;
                
            }
        }
    }
}