File content as of revision 20:e05fdab66e68:

#define pi 3.141593
#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "QEI.h"
#include "Servo.h"
#define RATE 0.05
#define shoottime 0.5
BusOut air(p5,p6);
DigitalOut out(p7);
Serial conn(p9,p10);
Serial pc(USBTX,USBRX);
Serial slave(p28,p27);
PwmOut Ll(LED1);
PwmOut Rl(LED2);
Servo L(p25);
Servo R(p26);
PID Tp(50,4000000,0,0.001);
//PID Tp(4.,40000,0,0.001);
BusOut led(p8,p13,p14,p24);
Motor ot(p23,p19,p20);
QEI sensort(p29,p30,NC,624);
//Motor sup1(p21,p15,p16);
//Motor sup2(p22,p17,p18);
SoftPWM sup1p(p21);
BusOut sup1d(p15,p16);
SoftPWM sup2p(p22);
BusOut sup2d(p17,p18);
DigitalIn limit1(p11,PullUp);
DigitalIn limit2(p12,PullUp);
Timeout ai;
char read;
int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
int i = 0;
void sup1_speed(float speed)
{
    if(speed>0){
        sup1d = 1;
    }else if(speed < 0){
        sup1d =2;
    }
    sup1p = fabs(speed);
}
void sup2_speed(float speed)
{
    if(speed>0){
        sup2d = 1;
    }else if(speed < 0){
        sup2d =2;
    }
    sup2p = fabs(speed);
}
void supply()
{
    if(d%2){
        sup1_speed(0.9);
    }else{
        sup2_speed(0.9);
    }
    d++;
}
void zero()
{
    air = 0;
    i = 0;
    out = 1;
    //supply();
}
void rev()
{
    air = 2;
    ai.attach(&zero,shoottime);
    out = 0;
}
int main()
{
    led = 1;
    double tilt = 0,lo = 0,ro = 0;
    int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
    char tro = 0,tlo = 0;
    Tp.setInputLimits(-45,45);
    Tp.setOutputLimits(-0.9,0.9);
    Tp.setMode(1);
    Tp.setBias(0.0);
    L = 0.5;
    R = 0.5;
    wait(3);
    led= 15;
    conn.baud(38400);
    while(1) {
        //ps3コントローラーシリアル受信
        if(conn.getc() == 255) {
            read = conn.getc();
            ttilt = conn.getc()-60;
            ro = (conn.getc()-127)/127.0*0.9;
            lo = (conn.getc()-127)/127.0*0.9;
        }
        //nara シリアル受信
        if(slave.avaiable(){
            narasup = slave.getc();
        }
        if(narasup == 1){
            sup1_speed(0.9);
            sup2_speed(0);
        }else if (narasup == 2){
            sup1_speed(-0.9);
            sup2_speed(0);
        }else if (narasup == 4){
            sup2_speed(0.9);
            sup1_speed(0);
        }else if (narasup ==8){
            sup2_speed(-0.9);
            sup1_speed(0);
        }else {
            sup1_speed(0);
            sup2_speed(0);
        }
            
        if(read>4){
            read = 0;
        }
        led = read;
        if((read>>1)%2 && i == 0) {
            air = 1;
            ai.attach(&rev,shoottime);
            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/2.0);
        tilt = double(sensort.getPulses());
        tilt = tilt*49.0/1745.0;
        Tp.setProcessValue(tilt);
        /*足の出力が小さい場合はゼロとする*/
        if (abs(lo) < 0.1) {
            lo = 0;
        }
        if (abs(ro) < 0.1) {
            ro = 0;
        }
        /*スレーブにreadを送信*/
        slave.putc(read);
        /*各モーターに出力*/
        L = (ro+1.0)/2.0;
        Ll = (ro+1.0)/2.0;
        R = 1.0-(lo+1.0)/2.0;
        Rl = 1.0-(lo+1.0)/2.0;
        ot.speed(Tp.compute());
        //pc.printf("%f\n\r",tilt);
        //pc.printf("%d\n\r",sensort.getPulses());
        //pc.printf("%f \n\r",Tp.compute());
    }
}