File content as of revision 11:427bb7a43d7a:

#define pi 3.141593
#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "QEI.h"
#include "Servo.h"
#define RATE 0.05
BusOut air(p5,p6);
DigitalOut out(p7);
Serial conn(NC,p27);
Serial pc(USBTX,USBRX);
Serial slave(p9,NC);
Servo L(p24);
Servo R(p25);
PID Tp(50,40000,0,0.001);
BusOut led(LED1,LED2,LED3,LED4);
Motor ot(p23,p13,p14);
QEI sensort(p29,p30,NC,624);
Motor sup1(p21,p15,p17);
Motor sup2(p22,p19,p20);
BusIn limit1(p10);
BusIn limit2(p11);
Timeout ai;
char read;
int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
int i = 0;
void supply()
{
    if(d%2){
        sup1.speed(1);
    }else{
        sup2.speed(1);
    }

}
void zero()
{
    air = 0;
    i = 0;
    out = 1;
    supply();
}
void rev()
{
    air = 2;
    ai.attach(&zero,1.0);
    out = 0;
}
int main()
{
    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);
    Tp.setBias(0.0);
    while(1) {
        if(conn.getc() == 255) {
            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-127)/127.0*0.9;
                lo = (tlo-127)/127.0*0.9;
            }
        }
        if((read>>2)%2 && i == 0) {
            air = 1;
            ai.attach(&rev,1.0);
            i = 1;
        }
        /*補給制御*/
        if(limit1 == 0) {
            su1 = 1;
        }
        if(limit2 == 0){
            su2 = 1;
        }
        if(limit1 == 1 && su1 == 1) {
            su1 = 0;
            sup1.speed(0);
        }
        if(limit2 == 1 && su2 == 1) {
            su2 = 0;
            sup2.speed(0);
        }
        /*射角制御*/
        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;
        }
        /*スレーブにreadを送信*/
        slave.putc(read);
        /*各モーターに出力*/
        L = (lo+1.0)/2.0;
        R = (ro+1.0)/2.0;;
        ot.speed(Tp.compute());
        //pc.printf("%f %d\n\r",Tp.compute(),read);
        //pc.printf("%d-%d\r\n",tlo,tro);
        wait_ms(1);
    }
}