File content as of revision 0:996353c69d55:

#include "mbed.h"
#include "QEI.h"
#include "ps3CTRLmbed.h"
/*
上　下
R1　L1　０．５
R2　L2　遅く
立ち上がりで停止 pwm=0
〇　4段階　　2個　　０にする
×  停止
□   0 1
△  1 0
*/
void RL_ONE(void);
void RL_TWO(void);

QEI wheel1(p30, p29, NC, 624);//p24,p19のモーター
QEI wheel2(p7, p8, NC, 624);//p25,p20のモーター
Serial pc(USBTX, USBRX); // tx, rx

void QEI(void);

InterruptIn PH1(p12);//L1とR1のアレ
InterruptIn PH2(p13);//L2とR2のアレ

/*100行目あたり。□かつS1＝０で動く。△かつRS2=0で動く。*/
DigitalIn RS1(p5);
DigitalIn RS2(p6);

DigitalOut ST1(p14);
DigitalOut ST2(p15);
DigitalOut UD(p16);
DigitalOut RL1(p17);
DigitalOut RL2(p18);
DigitalOut C1(p19);
DigitalOut C2(p20);

PwmOut UDP(p21);
PwmOut RLP1(p22);
PwmOut RLP2(p23);
PwmOut CP1(p24);
PwmOut CP2(p25);
PwmOut STP(p26);

I2C i2c(p9,p10);

int addr=0x00;//アドレスをいじるならここ
int ack=0;
double t1=0;
double t2=0;
double t3=0;
double t4=0;
double t5=0;
double t6=0;
int flag=0;
int p=0;
int q=0;
int k=0;
int CC=0;
int M=0;      //目標パルス値
int rs1=0;
int rs2=0;

int main() {
    char cmd[4];
    PS3DefaultSet();
    while(1) {
        CheckInputData();
        for(int i=0;i<4;i++){
            cmd[i]=(char)psdata[i+3];
        }
        i2c.write(addr,cmd,4);
        
        /*上下*/
        if(BTTRUE(UP)){/*上*/
            t1+=0.2;
            if(t1>1.0){
                t1=1.0;
            }
            UD=0;
            UDP=t1;
        }else if(BTTRUE(DOWN)){/*下*/
            t1+=0.2;
            if(t1>1.0){
                t1=1.0;
            }
            UD=1;
            UDP=t1;
        }else{
            t1-=0.2;
            if(t1<0){
                t1=0.0;
            }
            UDP=t1;
        }
        
        rs1=RS1;
        rs2=RS2;
        
        /*□ △*/
        if(BTTRUE(SQUARE)&&rs1==0){
            t6+=0.2;
            if(t6>1.0){
                t6=1.0;
            }
            ST1=0;
            STP=t6;
            ST2=1;
            STP=t6;
        }else if(BTTRUE(TRIANGLE)&&rs2==0){
            t6+=0.2;
            if(t6>1.0){
                t6=1.0;
            }
            ST1=1;
            STP=t6;
            ST2=0;
            STP=t6;
        }else{
            t6-=0.2;
            if(t6<0.0){
                t6=0.0;
            }
            STP=t6;
        }
        
        //PH1.rise(&RL_ONE);
        //PH2.rise(&RL_TWO);
        
        /*R1 L1*/
        if(BTTRUE(R1)){/*R1*/
            PH1.rise(&RL_ONE);
            if(p<2){
                t2+=0.1;
                if(t2>0.5){
                    t2=0.5;
                }
                RL1=0;
                RLP1=t2;
            }else{
                t2=0;
                RLP1=0;
            }
        }else if(BTTRUE(L1)){//L1
            PH1.rise(&RL_ONE);
            if(p<2){
                t2+=0.1;
                if(t2>0.5){
                    t2=0.5;
                }
                RL1=1;
                RLP1=t2;
            }else{
                t2=0;
                RLP1=0;
            }
        }else{
            t2-=0.15;
            if(t2<0){
                t2=0;
            }
            RLP1=t2;
        }
        
        /*R2 L2*/
        if(BTTRUE(R2)){/*R2*/
            PH2.rise(&RL_TWO);
            if(q<2){
                t3+=0.1;
                if(t3>0.5){
                    t3=0.5;
                }
                RL2=0;
                RLP2=t3;
            }else{
                t3=0;
                RLP2=0;
            }
        }else if(BTTRUE(L2)){/*L2*/
            PH2.rise(&RL_TWO);
            if(q<2){
                t3+=0.1;
                if(t3>0.5){
                    t3=0.5;
                }
                RL2=1;
                RLP2=t3;
            }else{
                t3=0;
                RLP2=0;
            }
        }else{
            t3-=0.15;
            if(t3<0){
                t3=0;
            }
            RLP2=t3;
        }
        
        if(p>=2 && BTFALSE(R1) && BTFALSE(L1)){
            p=0;
        }
        
        if(q>=2 && BTFALSE(R2) && BTFALSE(L2)){
            q=0;
        }
        
        /*
        //〇
        while(BTTRUE(CIRCLE)){
            if(BTFALSE(CIRCLE)){
                flag=flag+1;
                if(flag>4){
                    flag=1;
                }
                break;
            }
        }
        */
        
        //〇
        if(BTTRUE(CIRCLE)){
            CC=1;
        }

        if(BTFALSE(CIRCLE)){
            if(CC==1){
                flag=flag+1;
                if(flag>4){
                    flag=1;
                }
            }
            CC=0;
        }
        
        //×
        if(BTTRUE(CROSS)){
            CC=0;
            flag=0;
        }
        
        //〇  ×
        if(flag>0){//CC>0
            //if(flag==0){
            t4+=0.1;
            t5+=0.1;
            //}
            if(t4>(flag*0.2)){
                t4=flag*0.2;
            }
            if(t5>(flag*0.2)){
                t5=flag*0.2;
            }
            
            if(t4>0.8){
                t4=0.8;
                //flag=1;
            }
            if(t5>0.8){
                t5=0.8;
                //flag=1;
            }
            C1=0;
            C2=0;
            CP1=t4;
            CP2=t5;
            QEI();
        }else{
            t4-=0.15;
            t5-=0.15;
            if(t4<0){
                t4=0;
            }
            if(t5<0){
                t5=0;
            }
            CP1=0;
            CP2=0;
        }
    }
}

void RL_ONE(void){
    p++;
    if(p>=2){
        t2=0;
        RLP1=0;
    }
}

void RL_TWO(void){
    q++;
    if(q>=2){
        t3=0;
        RLP2=t3;
    }
}

void QEI(){
    int a,b/*,A,B*/;
    //double c,R3,R4;
    //R3=wheel1.getAngle(); 
    //R4=wheel2.getAngle(); 
    a=wheel1.getPulses();
    b=wheel2.getPulses();
    //pc.printf("a:%d    b:%d    ",a,b);
    if(a<0){
        a=a*(-1);
    }
    if(b<0){
        b=b*(-1);
    }
    /*
    c=(a+b)/2;
    A=a-(int)c;
    B=b-(int)c;
    */
    
    M=4+flag*4;
    
    if(k>20){
        if(M>a){
            t4+=0.02;
        }
        else if(M<a){
            t4-=0.02;
        }
        if(t4>1.0){
            t4=1.0;
        }
        if(t4<0.0){
            t4=0.0;
        }
        
        if(M>b){
            t5+=0.02;
        }
        else if(M<b){
            t5-=0.02;
        }
        if(t5>1.0){
            t5=1.0;
        }
        if(t5<0.0){
            t5=0.0;
        }
        //pc.printf("pwm1:%f    pwm2:%f    ",t4,t5);
        CP1=t4;
        CP2=t5;
        wheel1.reset();
        wheel2.reset();
        k=0;
    }else{
        k++;
        //flag=0;
    }
    
}