File content as of revision 2:9dae549ae1b4:

#include "mbed.h"
#include "main.h"
#include "moterdrive.h"
#include "DualShockMod.h"

#define PERIOD 40

#define hosei1 1.070    //left front
#define hosei2 1.000    //right front
#define hosei3 0.960    //back

#define LIFTPOWER_UP 0.2
#define LIFTPOWER_DOWN 0.2

DigitalOut  my_led(LED1);
PwmOut      md1_pwm1(PB_2);
DigitalOut  md1_cw1(PB_12);
DigitalOut  md1_ccw1(PB_1);
DigitalOut  md1_dis1(PA_11);

PwmOut      md1_pwm2(PB_15);
DigitalOut  md1_cw2(PA_12);
DigitalOut  md1_ccw2(PB_14);
DigitalOut  md1_dis2(PB_13);

PwmOut      md2_pwm1(PC_7);
DigitalOut  md2_cw1(PA_8);
DigitalOut  md2_ccw1(PC_4);//ここ
DigitalOut  md2_dis1(PB_10);

PwmOut      md2_pwm2(PB_3);
DigitalOut  md2_cw2(PB_4);
DigitalOut  md2_ccw2(PA_10);
DigitalOut  md2_dis2(PB_5);

DigitalIn   Limit1(PC_9);  //back
DigitalIn   Limit2(PC_8);  //right
DigitalIn   Limit3(PC_6);
DigitalIn   Limit4(PC_5);

DigitalIn   photo1(PA_13);
DigitalIn   photo2(PA_14);
DigitalIn   photo3(PA_15); 

DigitalIn button1(USER_BUTTON);
DigitalOut led1(D14);
DigitalOut led2(D15);

//serial通信
Serial pc(SERIAL_TX, SERIAL_RX);
Serial tsuushin(PC_10,PC_11);  


/*プロトタイプ宣言*/
void moter(int num, char dir, float power);


Timer timer;

int main(){
    
    typedef enum{
        WAIT,
        INIT1,
        INIT2,
        WAIT2,
        HARI1,
        HARI2,
        HARI3,
        HARI4,
        HARI_END,
        SYM1,
        SYM2,
        SYM3,
        SYM4,
        SYM5,
        SYM6,
        SYM7,
        SYM8,
        END,
        DEBUG   
    }SEQENCE;
    SEQENCE seq = WAIT;

    typedef enum{
        STOP,
        FFAST,
        FSLOW,
        BFAST,
        BSLOW,
        RIGHT,
        LEFT,
        RROLL,
        LROLL,
        UP,
        DOWN 
    }MOVEDIR;
    MOVEDIR move = STOP;

    //エンコーダーの値
    tsuushin.baud(115200);   
    uint8_t InitDS(Serial* f_serial);
    void getPOSdata(void);   
    InitDS(&tsuushin);
    tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始           

    
    md1_pwm1.period_us(100);
    md1_pwm2.period_us(100);
    md2_pwm1.period_us(100);
    md2_pwm2.period_us(100);
    
    int pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1,k = {0};
    
    while(1){
        
        pho1 = !photo1;
        pho2 = !photo2;
        pho3 = !photo3;
        lim1 = Limit1;
        lim2 = Limit2;
        lim3 = Limit3;
        lim4 = !Limit4;                        
        btn1 = !button1;

        
//        pc.printf("ENC = %d \n\r", qei.getPulses());
    
//代入部
//        posX = ;
//        posY = ;     

        if(move == STOP) led1 = 1;
            else led1 = 0;          
            
        if(lim4 == 1) seq = DEBUG;

        switch(seq){
            case WAIT:
                seq = INIT1;
             
             break;
            
            case INIT1:
              move = DOWN;
               
               if(pho1 == 1){
                    move = STOP;
                    seq = INIT2;
                    }

             break;
             
             case INIT2:
              timer.start();
              move = UP;
              if(timer.read_ms() > 2500){
                    move = STOP;
                    timer.reset();
                    timer.stop();                    
                seq = WAIT2;
             }
              break;
             
            case WAIT2:
                move = STOP;
                
                 if(btn1 == 1){
                    seq = HARI1;
                    }
                 if(lim2 == 1){
                    seq = SYM1;
                     }   
                break;
            
            case HARI1:
                move = FFAST;
            
                if(posY > 1000 ){
                    move = FSLOW;
                    if(posY > 1080)
                    seq = HARI2;
                    }
              break;
              
            case HARI2:
                move = DOWN;
                
                if(pho1 == 1){
                    move = STOP;
                    seq = HARI3;
                    }    
              break;
              
            case HARI3:
               move = BFAST; 
               
               if(posY < 800){ 
                    seq = HARI4;
                    timer.reset();
                }  
             break;
             
            case HARI4:
               timer.start();
               move = UP;
               
               if(timer.read_ms() > 2000){
                    move = STOP;
                    timer.reset();
                    seq = HARI_END;
               }
                    
             break;
             
            case HARI_END:
               move = STOP;

             break;
             
            case SYM1:
                move = FFAST;
                
                if(posY > 750 ){
                    move = FSLOW;
                    if(posY > 800)
                    seq = SYM2;
                    }
              break;
              
            case SYM2:
                move = RIGHT;
                
                if(posX > 850){
                    seq = SYM3;
                    }   
              break;
              
            case SYM3:
                move = BSLOW;
                
                if(lim1 == 1){
                    move = FFAST;
                    seq = SYM4;
                    k = posY;
                    }   
              break; 
            
            case SYM4:
                move = FFAST;
                
                if( posY - k >360){
                    seq = SYM5;
                    }
              break; 
              
            case SYM5:
                move = LEFT;
                
                if(posX < 300){
                    seq = SYM6;
                    } 
              break;   
              
            case SYM6:
                move = LROLL;
                
                timer.start();
                if(timer.read_ms() > 5000){
                    move = STOP;
                    timer.reset();
                    timer.stop(); 
                    seq = SYM7;
                    }   
              break; 
              
            case SYM7:
                 move = DOWN;
                
                if(pho1 == 1){
                    move = STOP;
                    seq = SYM8;
                    }   
              break; 
              
            case SYM8:
                move = BFAST;
                timer.start();
                if(timer.read_ms() > 3000){
                    move = STOP;
                    timer.reset();
                    timer.stop(); 
                
                    seq = END;  
                    }   
              break; 
              
            case DEBUG:
                if(lim1 == 1) move = UP;
                else if (lim2 == 1) move = DOWN;
                else move = STOP;
            break;
              
            }
            
            
            
            
        switch(move){
            case STOP:
                moter(1,STOP,0);
                moter(2,STOP,0);
                moter(3,STOP,0);
                moter(4,STOP,0);
             break;
            
            case FFAST:
                moter(1,cw,1);
                moter(2,ccw,1);
                moter(3,STOP,0);
                moter(4,STOP,0);
             break;
            
            case FSLOW:
                moter(1,cw,0.5);
                moter(2,ccw,0.5);
                moter(3,STOP,0);
                moter(4,STOP,0);
             break;
            
            case BFAST:
                moter(1,ccw,1);
                moter(2,cw,1);
                moter(3,STOP,0);
                moter(4,STOP,0);
             break;
             
            case BSLOW:
                moter(1,ccw,0.5);
                moter(2,cw,0.5);
                moter(3,STOP,0);
                moter(4,STOP,0);
             break;
            
            case RROLL:
                moter(1,cw,0.3);
                moter(2,cw,0.3);
                moter(3,ccw,0.3);
                moter(4,STOP,0);
             break;
            
            case LROLL:
                moter(1,ccw,0.3);
                moter(2,ccw,0.3);
                moter(3,cw,0.3);
                moter(4,STOP,0);
             break;
            
            case RIGHT:
                moter(1,cw,0.5);
                moter(2,cw,0.5);
                moter(3,cw,1);
                moter(4,STOP,0);
             break;
            
            case LEFT:
                moter(1,ccw,0.5);
                moter(2,ccw,0.5);
                moter(3,ccw,1);
                moter(4,STOP,0);
             break;
            
            case UP:
                moter(1,STOP,0);
                moter(2,STOP,0);
                moter(3,STOP,0);
                moter(4,cw,LIFTPOWER_UP);
             break;
            
            case DOWN:
                moter(1,STOP,0);
                moter(2,STOP,0);
                moter(3,STOP,0);
                moter(4,ccw,LIFTPOWER_DOWN);
             break;
            
        }

        pc.printf("posX=%d,posY=%d  ", posX, posY);
        pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\t BTN=%d  ", pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1);
        pc.printf("seq = %d\n\r", seq);
         }    
}



void moter(int num, char dir, float power){
    int pho1 = !photo1;
    int output;
    if(num == 1){
        if(dir == cw){
            md1_cw1 = 1;
            md1_ccw1 = 0;
            }
            else if(dir == ccw){
            md1_cw1 = 0;
            md1_ccw1 = 1;
            }
            else if(dir == STOP){
            md1_cw1 = 0;
            md1_ccw1 = 0;
            }
        output = PERIOD * power * (float)hosei1;
        md1_pwm1.pulsewidth_us(output);
        } 
        if(num == 2){
            if(dir == cw){
            md1_cw2 = 1;
            md1_ccw2 = 0;
            }
            else if(dir == ccw){
            md1_cw2 = 0;
            md1_ccw2 = 1;
            }
            else if(dir == STOP){
            md1_cw2 = 0;
            md1_ccw2 = 0;
            }
        output = PERIOD * power * (float)hosei2;
        md1_pwm2.pulsewidth_us(output);
        } 
        if(num == 3){
            if(dir == cw){
            md2_cw1 = 1;
            md2_ccw1 = 0;
            }
            else if(dir == ccw){
            md2_cw1 = 0;
            md2_ccw1 = 1;
            }
            else if(dir == STOP){
            md2_cw1 = 0;
            md2_ccw1 = 0;
            }
        output = PERIOD * power * (float)hosei3;
        md2_pwm1.pulsewidth_us(output);
        }
        if(num == 4){
            if(dir == cw){
            md2_cw2 = 1;
            md2_ccw2 = 0;
            }
            else if(dir == ccw){
            md2_cw2 = 0;
            
                if(pho1 == 1){
                md2_ccw2 = 0;
                }
                else{
                md2_ccw2 = 1;
                }
            }
            else if(dir == STOP){
            md2_cw2 = 0;
            md2_ccw2 = 0;
            }
        output = 100 * power ;
        md2_pwm2.pulsewidth_us(output);
        }    
    }