File content as of revision 10:fefff533c442:

#define pi 3.141593
#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "QEI.h"
#include "Servo.h"
#define RATE 0.05
DigitalOut led(LED1);
BusOut air(PC_10,PC_11);
DigitalOut out(PC_12);
Serial conn(NC,PA_12);
Serial pc(SERIAL_TX, SERIAL_RX);
PID Tp(50,40000,0,0.001);
Motor ot(PB_13,PB_3,PA_10);
QEI sensort(PC_3,PC_0,NC,624);
Servo L(PC_9);
Servo R(PC_8); 
Timeout ai;
char read;
int Rs = 0,Ls = 0;
int i = 0;
void zero(){ 
    air = 0;
    i = 0;
    out = 1;
}
void rev(){
    air = 2;
    ai.attach(&zero,1.0);
    out = 0;
}
int main()
{
    pc.printf("connected\n\r");
    //conn.baud(115200);
    //ot.setfrequency(60000);
    double tilt = 0,lo = 0,ro = 0;
    int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
    char tro = 0,tlo = 0;
    Tp.setInputLimits(-45,45);
    Tp.setOutputLimits(-0.9,0.9);
    Tp.setMode(1);
    led=1;
    Tp.setBias(0.0);
    while(1) {
        if(conn.getc() == 0xFF) {
            led = !led;
            tmpread = conn.getc();
            tmpttilt = conn.getc();
            tro = conn.getc();
            tlo = conn.getc();
            if(tmpread^tmpttilt^tro^tlo == conn.getc()){
                ttilt = tmpttilt;
                read = tmpread;
                ro = tro;
                lo = tlo;
            }
        }
        if((read>>2)%2 && i == 0){
            air = 1;
            ai.attach(&rev,1.0);
            i = 1;
        }
        Tp.setSetPoint(ttilt);
        tilt = double(sensort.getPulses());
        tilt = tilt*61/5128.0;
        Tp.setProcessValue(tilt);
        if (abs(lo) < 0.1){
            lo = 0;
        }
        if (abs(ro) < 0.1){
            ro = 0;
        }
        L= (lo-127)/127.0;
        R=(ro-127)/127.0;
        ot.speed(Tp.compute());
        //pc.printf("%f %d\n\r",Tp.compute(),read);
        pc.printf("%d-%f\r\n",read,lo);
    }
}