File content as of revision 24:7e93db7d1c32:

#define pi 3.141593
#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "QEI.h"
#include "Servo.h"
#include "DebounceIn.h"
#define RATE 0.05
#define shoottime 0.5
#define DEADTIME 500
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);
BusOut mled(LED3,LED4);
Servo L(p25);
Servo R(p26);
PID Tp(50,4000000,0,0.001);
BusOut led(p8,p13,p14,p24);
Motor ot(p23,p20,p19);
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(p18,p17);
DebounceIn limit1(p11);
DebounceIn limit2(p12);
Timeout ai;
Timer timer1;
Timer timer2;
char read;
int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
int i = 0;
int old_limit1 = 0,old_limit2 = 0,new_limit1 = 0,new_limit2 = 0,count1 = 0,count2 = 0,target_count1 = 0,target_count2 = 0,supply_seq1 = 0,supply_seq2 = 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 sup1_brake()
{
    sup1d = 0;
    sup1p = 1;
}
void sup2_brake()
{
    sup2d = 0;
    sup2p = 1;
}
void supply_1()
{
    sup1_speed(0.4);
    target_count1 +=3;
    supply_seq1 = 1;
}
void supply_2()
{
    sup2_speed(0.4);
    target_count2 +=3;
    supply_seq2 = 1;
}
void supply()
{
    if (supply_seq1 ==0 && supply_seq2 == 0) {
        if(d%2) {
            supply_1();
        } else {
            supply_2();
        }
        d++;
    }
}

void count_check()
{
    if((target_count1 == count1)&&(supply_seq1 == 1)) {
        sup1_speed(-0.4);
        target_count1 += 2;
        supply_seq1 = 2;
    } else if((target_count1 == count1) && (supply_seq1 == 2)) {
        sup1_brake();
        supply_seq1 = 0;
        mled = 0;
    }
    if((target_count2 == count2)&&(supply_seq2 == 1)) {
        sup2_speed(-0.4);
        target_count2 += 2;
        supply_seq2 = 2;
    } else if((target_count2 == count2) && (supply_seq2 == 2)) {
        sup2_brake();
        supply_seq2 = 0;
        mled = 0;
    }
}
void limit1_count()
{
    new_limit1 = limit1;
    if ((new_limit1==0) && (old_limit1==1)) count1++;
    old_limit1 = new_limit1;
}
void limit2_count()
{
    new_limit2 = limit2;
    if ((new_limit2==0) && (old_limit2==1)) count2++;
    old_limit2 = new_limit2;
}
void zero()
{
    air = 0;
    i = 0;
    out = 1;
}
void rev()
{
    air = 2;
    ai.attach(&zero,shoottime);
    out = 0;
}
int main()
{
    L = 0.5;
    R = 0.5;
    limit1.mode(PullUp);
    limit2.mode(PullUp);
    led = 1;
    int dead1 = 0,dead2 = 0;
    double tilt = 0,lo = 0,ro = 0;
    int8_t ttilt = 60,tmpread = 0,tmpttilt = 0,ajst = 0;
    char tro = 0,tlo = 0,narasup = 60;
    Tp.setInputLimits(-45,45);
    Tp.setOutputLimits(-0.9,0.9);
    Tp.setMode(1);
    Tp.setBias(0.0);
    wait(3);
    led= 15;
    conn.baud(38400);
    while(1) {
        //ps3コントローラーシリアル受信
        if(conn.getc() == 255) {
            read = conn.getc();
            ttilt = conn.getc();
            tro =conn.getc();
            tlo = conn.getc();
            if (tro == 255){
                ro =ro;
            }else{
                ro = (tro-127)/127.0*0.9;
            }
            if(tlo ==255){
                lo = lo;
            }else {
                lo = (tlo-127)/127.0*0.9;
            }
        }
        //nara シリアル受信
        if(slave.readable()) {
            narasup = slave.getc();
            if((narasup-60) == 1 && supply_seq1 == 0) {
                mled = 1;
                supply_1();
            } else if((narasup-60) == 2 && supply_seq2 == 0) {
                supply_2();
                mled =2;
            }
        }
        if (read > 4){ //通信不良対策
            read = 0;
        }
        led = read;
        if(read==2 && i == 0) {
            air = 1;
            ai.attach(&rev,shoottime);
            i = 1;
            supply();
        }
        limit1_count();
        limit2_count();
        count_check();
        /*射角制御*/
        Tp.setSetPoint((ttilt-60)/2.0);
        tilt = double(-sensort.getPulses());
        tilt = tilt*49.0/1745.0;
        Tp.setProcessValue(tilt);
        /*足の出力が小さい場合はゼロとする*/
        /*if(timer1.read_ms() > DEADTIME) {
            dead1 = 0;
            timer1.stop();
            timer1.reset();
        }
        if(timer2.read_ms() > DEADTIME) {
            dead2 = 0;
            timer2.stop();
            timer2.reset();
        }
        if ((L >0.5 && (lo+1.0)/2.0<=0.5) || (L <0.5 && (lo+1.0)/2.0>=0.5) || dead1 ==1) {
            L = 0.5;
            Ll = 0.5;
            dead1 =1;
            timer1.start();
        } else {
            L = (lo+1.0)/2.0;
            Ll = (lo+1.0)/2.0;
        }
        if ( (R >0.5 && (ro+1.0)/2.0<=0.5) || (R <0.5 && (ro+1.0)/2.0>=0.5)  || dead2 ==1) {
            R = 0.5;
            Rl = 0.5;
            dead2 = 1;
            timer2.start();
        } else {
            R = (ro+1.0)/2.0;
            Rl = (ro+1.0)/2.0;
        }*/
        R = (ro+1.0)/2.0;
        Rl = (ro+1.0)/2.0;
        L = (lo+1.0)/2.0;
        Ll = (lo+1.0)/2.0;
        /*スレーブにreadを送信*/
        slave.putc(read);
        /*各モーターに出力*/
        //pc.printf("%d\n\r",narasup);
        ot.speed(Tp.compute());
        pc.printf("%f\n\r",R);
        //pc.printf("%f \n\r",Tp.compute());
    }
}