File content as of revision 3:d2c733b52600:

#define pi 3.141593
#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "QEI.h"
#define RATE 0.01
BusOut air(p13,p14);
DigitalOut out(p12);
Serial conn(p9,p10);
Serial pc(USBTX,USBRX);
Motor L(p22,p15,p16);
Motor R(p23,p17,p18);
PID Lp(1.0,0,0,RATE);
PID Rp(1.0,0,0,RATE);
PID Tp(1.0,0,0,RATE);
BusOut led(LED1,LED2,LED3,LED4);
Motor ot(p21,p19,p20);
QEI sensort(p29,p30,NC,624);
QEI ls(p27,p28,NC,624);
QEI rs(p25,p26,NC,624);
Timeout ai;
Ticker getsp;
char read;
int Rs = 0,Ls = 0;
int i = 0;
void getspeed()
{
    Rs =rs.getPulses();
    Ls =ls.getPulses();
    rs.reset();
    ls.reset();
}
void zero(){ 
    air = 0;
    i = 0;
    out = 1;
}
void rev(){
    air = 2;
    ai.attach(&zero,1.0);
    out = 0;
}
int main()
{
    double tilt = 0;
    int8_t ttilt = 0;
    Lp.setInputLimits(-3000,3000);
    Rp.setInputLimits(-3000,3000);
    Tp.setInputLimits(-10000,10000);
    Lp.setOutputLimits(-0.9,0.9);
    Rp.setOutputLimits(-0.9,0.9);
    Tp.setOutputLimits(-0.9,0.9);
    Lp.setMode(1);
    Rp.setMode(1);
    Tp.setMode(1);
    Lp.setBias(0);
    Rp.setBias(0);
    Tp.setBias(0);
    getsp.attach(&getspeed,0.1);
    while(1) {
        if(conn.getc() == 255) {
            read = conn.getc();
            ttilt = conn.getc();
        }
        if((read>>2)%2 && i == 0){
            air = 1;
            ai.attach(&rev,1.0);
            i = 1;
            led = 1;
        }
        if((read>>3)%2){
            Lp.setSetPoint(100);
            Rp.setSetPoint(100);
        }else
        if((read>>4)%2){
            Lp.setSetPoint(-100);
            Rp.setSetPoint(-100);
            led = 2;
        }else
        if((read>>5)%2){
            Lp.setSetPoint(-100);
            Rp.setSetPoint(100);
            led = 4;
        }else
        if((read>>6)%2){
            Lp.setSetPoint(100);
            Rp.setSetPoint(-100);
            led = 8;
        }else{
            Lp.setSetPoint(0);
            Rp.setSetPoint(0);
            led = 0;
        }
        Tp.setSetPoint(ttilt);
        tilt = sensort.getPulses();
        //tilt = tilt*24.0/500.0*pi/360.0;
        conn.putc(int8_t(tilt));
        Lp.setProcessValue(Ls);
        Rp.setProcessValue(Rs);
        Tp.setProcessValue(tilt);
        L.speed(Lp.compute());
        R.speed(Rp.compute());
        ot.speed(Tp.compute());
        pc.printf("%f\n\r",Lp.compute());
    }
}