File content as of revision 4:af9973350ffe:

//Works at speed 2.0, KPS 2.0e-2, KD 1.0e-4

//Has tolerance check, Steering doesn't
//Values from Simulation
//#define KPS 0.0896852742245504f
//#define KD 0.072870569295611f
//#define KPS 0.03f

#include "mbed.h"
#include <iostream>

#define TI 0.001f

#define SCALAR 0.53f
#define MINM 0.0f
#define MAXM 0.53f
#define KPM 0.15f //0.1414f
#define KI 19.7408f

#define KPS 2.0E-2f //Original 2.0e-2
#define KD 1.0e-4f
#define SET 0.0f
#define MINS 0.05f
#define MAXS 0.1f
#define BIAS 0.075f
#define TOL 0.02f
#define STEER_FREQ 0.02f //50 Hz
#define STEERUPDATEPERIOD 0.05

AnalogIn _right(PTB1); //Right sensor
AnalogIn _left(PTB3); //Left sensor
AnalogIn speed(PTB2);   //tachometer

PwmOut servoSig(PTA13); //PWM output to control servo
PwmOut gateDrive(PTA4);

Serial bt(PTE0, PTE1); //COM12
DigitalOut myled(LED_BLUE);
DigitalOut brake(PTD0);

Ticker control;
Timer ctrlTimer;

InterruptIn navRt(PTD2);
InterruptIn navLft(PTD3);

volatile int rightCount;
volatile int leftCount;

float data[6];

float Setpoint = 0.0;
float spHolder;
float errSum = 0.0;

float fbPrev = 0.0f;
float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
float Kd = 1.0e-4;

float Kpm = 0.141;

inline void applyBrake()
{  
    spHolder = Setpoint;
    brake.write(1);
    Setpoint = 0.0;
}

inline void releaseBrake()
{
    brake.write(0);
    Setpoint = spHolder;
}

void display()
{
    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
}

void serCb()
{
    char x = bt.getc();
    if (x == 'u')
    {
        Setpoint += 0.025;
        display();
    }
    else if(x == 'h')
    {
        Setpoint -= 0.025;
        display();
    }
    else if (x == 'i')
    {
        Kps += 1.0e-3;
        display();
    }
    else if (x == 'j')
    {
        Kps -= 1.0e-3;
        display();
    }
    else if (x == 'o')
    {
        Kd += 1.0e-5;
        display();
    }
    else if (x == 'k')
    {
        Kd -= 1.0e-5;
        display();
    }
    else if (x == 'b')
    {
        applyBrake();
        display();
       
    }
    else if (x == 'n')
    {
        releaseBrake();
        display();
    }
    else if (x == 'p')
    {
        display();
    }
    else if (x == 'y')
    {
        Kpm += 0.003;
        display();   
    }   
    else if(x == 'g')
    {
        Kpm -= 0.003; 
        display();   
    } 
    else if (x == 'z')
    {
        bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
    }
    else
    {
        bt.printf("Invalid input");
    }
    if(Setpoint > MAXM) Setpoint = MAXM;
    if(Setpoint < MINM) Setpoint = MINM;    
}


void incL()
{      
    leftCount++;
}

void incR()
{
    rightCount++;
}

void steer()
{
    float L = _left.read();
    float R = _right.read();
    if (L == 0.0 && R == 0.0)
    {
        //off track
        Setpoint = 0.0;
    }           
    float fb = L - R; 
    float e = SET - fb;
    float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
    if (Controlleroutput > MAXS) Controlleroutput = MAXS;
    else if (Controlleroutput < MINS) Controlleroutput = MINS;
    if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD) 
    {
        ctrlTimer.reset();
        servoSig.write(Controlleroutput);
    }
    fbPrev = fb;
    data[0] = _right.read();
    data[1] = _left.read();
    data[2] = Controlleroutput;
    data[3] = e;
    
}
void drive()
{
    float error = Setpoint-speed.read();
    errSum +=(error * TI);
    float iTerm = KI*errSum;
    if(iTerm > MAXM) iTerm = MAXM;
    if(iTerm < MINM) iTerm = MINM; 
    float output = KPM*error + iTerm;
    if(output > MAXM) output = MAXM;
    if(output < MINM) output = MINM;        

    gateDrive.write(output);
    data[4] = gateDrive.read();
    data[5] = speed.read(); 
}

void cb()
{
    steer();
    drive();
}

int main()
{   
    //startup checks
    bt.baud(115200);
    bt.attach(&serCb);    
    servoSig.period(STEER_FREQ);
    gateDrive.period(.00005f);
    gateDrive.write(Setpoint);

    ctrlTimer.start();
    control.attach(&cb, TI);
    
    rightCount = 0;
    leftCount = 0; 

    navRt.fall(&incR);
    navLft.fall(&incL);
    
    while(1) {
        myled = !myled;
        //bt.printf("%f ",data[0]);
//        bt.printf("%f ", data[1]);
//        bt.printf("%f ", data[2]);
//        bt.printf("%f ", data[3]);
//        bt.printf("%f ", data[4]);
//        bt.printf("%f\r\n", data[5]);
    }
}