File content as of revision 0:76ba33d3b4a6:

#include "mbed.h"
#define MOVE_TIME 0.11
#define WAIT_TIME 0.02
#define PWM_LEVEL 1.0// 11.1 / 7.4 


/*
センサの数を八個で足一つに付き二つのセンサで足を上下させるプログラム．

はじめのリミットスイッチが押されてもう一度押されるまでの時間を計り現在の速さを概算してタイマー制御に役立てる


*//**/


Serial pc(USBTX,USBRX);

Timeout _toMotor[8];
Timeout _toWait[4];
DigitalIn  limit[8] = {p5,p6,p7,p8,p9,p10,p11,p12 };
DigitalOut _doLed[4] = {LED1, LED2, LED3, LED4 };

BusOut _boMotor(p13, p14, p15, p16, p17, p18, p19, p20 );//four motors 
//{left front, right front, left back ，right back } in sequence?
PwmOut _poPwm[4] = {p21, p22, p23, p24 };

/**more use binary number
0xA0 = 10100000
0x50 = 01010000
0x0A = 00001010
0x05 = 00000101
**/

volatile bool bWait[4] = {0};

void _atWaitStop0(){//atatch Time Stop0
//make delay time
    bWait[0] =  0;
    
}

void _atWaitStop1(){//atatch Time Stop0
//make delay time
    bWait[1] =  0;
    
}

void _atWaitStop2(){//atatch Time Stop0
//make delay time
    bWait[2] =  0;
    
}

void _atWaitStop3(){//atatch Time Stop0
//make delay time
    bWait[3] =  0;
    
}



void _atMotorStop0(){//atatch Motor Stop0
    
    _boMotor = _boMotor & 0x7F;
    //01111111 指定ピンのみ変更
    //left front leg up
    _doLed[0] = 0;
}


void _atMotorStop1(){
    _toWait[0].attach(&_atWaitStop0,WAIT_TIME);
    _boMotor = _boMotor & 0xBF; 
    //10111111 　指定ピンのみ変更
    //left front leg down
    _doLed[1] = 0;
    
}

void _atMotorStop2(){
    
    _boMotor = _boMotor & 0xF7;
    //11110111 指定ピンのみ変更
    //left back leg up
    _doLed[2] = 0;
    
}

void _atMotorStop3(){
    _toWait[1].attach(&_atWaitStop1,WAIT_TIME);
    _boMotor = _boMotor & 0xFB;
    //11111011 指定ピンのみ変更
    //left back leg down
    _doLed[3] = 0;
    
}
void _atMotorStop4(){//atatch Motor Stop0
    
    _boMotor = _boMotor & 0xDF;
    //11011111 指定ピンのみ変更
    //right front leg up
    
    
}


void _atMotorStop5(){
    _toWait[2].attach(&_atWaitStop2,WAIT_TIME);
    _boMotor = _boMotor & 0xEF; 
    //11101111 　指定ピンのみ変更
    //right front leg down
    
    
}

void _atMotorStop6(){
    
    _boMotor = _boMotor & 0xFD;
    //11111101 指定ピンのみ変更
    //right back leg up
        
}

void _atMotorStop7(){
    _toWait[3].attach(&_atWaitStop3,WAIT_TIME);
    _boMotor = _boMotor & 0xFE;
    //11111110 指定ピンのみ変更
    //right back leg down
    
    
}



int main() {
    
    static bool bClim[8] = {1,1,1,1,1,1,1,1};//bool Check limit[]
    static uint8_t u8Ct1[8] = {0},u8Ct2[8] = {1,1,1,1,1,1,1,1};//uint8_t Count{1,2}[]
    
    limit[0].mode(PullUp);
    limit[1].mode(PullUp);
    limit[2].mode(PullUp);
    limit[3].mode(PullUp);
    limit[4].mode(PullUp);
    limit[5].mode(PullUp);
    limit[6].mode(PullUp);
    limit[7].mode(PullUp);


    wait(1);


    /* pwm level setting */
    _poPwm[0] = PWM_LEVEL ;
    _poPwm[1] = PWM_LEVEL ;
    _poPwm[2] = PWM_LEVEL ;
    _poPwm[3] = PWM_LEVEL ;
    /*                  */

    
    while(1){
        /*左の方-リミットスイッチ0~3*/
        if(!limit[0] && !bClim[0]){ 
            
            /* 1個目*/
            if(!bWait[0] && !u8Ct1[0] && !u8Ct1[1]){
                u8Ct1[0] = 1;
                //legs up
                _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
                _boMotor = _boMotor | 0x80;//10000000
                _doLed[0] = 1;
            
            }else if(u8Ct1[0]){
                u8Ct1[0] = 2;
        
            }else if((u8Ct1[1] == u8Ct2[0])){
                u8Ct1[1]=0;
                u8Ct2[0]=1;
                //legs down
                _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
                _boMotor = _boMotor | 0x40;//0x40 = 01000000
                _doLed[1] = 1;
                bWait[0] = 1; 
                
            }else{
                u8Ct2[0] = 2;
            }
            bClim[0] = 1;
        
        }else if(limit[0]){
            bClim[0] = 0;
        }
    
    
        if(!limit[1] && !bClim[1]){
            
            /* 2個目*/
            if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]){
                u8Ct1[1] = 1;
                //legs up
                _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
                _boMotor = _boMotor | 0x80;// 10000000
                _doLed[0] = 1;
                
            }else if(u8Ct1[1]){
                u8Ct1[1] = 2;
                
            }else if((u8Ct1[0] == u8Ct2[1])){
                u8Ct1[0]=0;
                u8Ct2[1]=1;
                //legs down
                _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
                _boMotor = _boMotor | 0x40;// 01000000
                _doLed[1] = 1;
                bWait[0] = 1; 
            
            }else{
                u8Ct2[1] = 2;
            
            }
            bClim[1] = 1;
    
        }else if(limit[1]){
            bClim[1] = 0;    
        }
    
    
        if(!limit[2] && !bClim[2]){
            
            /* 3個目*/
            if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]){
                u8Ct1[2] = 1;
                //legs up
                _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
                _boMotor = _boMotor | 0x08;//00001000
                _doLed[2] = 1;
            }else if(u8Ct1[2]){
                u8Ct1[2] = 2;
        
            }else if((u8Ct1[3] == u8Ct2[2])){
                u8Ct1[3]=0;
                u8Ct2[2]=1;
                //legs down
                _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
                _boMotor = _boMotor | 0x04;//00000100
                _doLed[3] = 1;   
                bWait[1] = 1;
            }else{
                u8Ct2[2] = 2;
            }

            bClim[2] = 1;
        }else if(limit[2]){
            bClim[2] = 0;
        }



        if(!limit[3] && !bClim[3]){
            /* 4個目*/
            if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]){
                u8Ct1[3] = 1;
                //leg up
            _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
            _boMotor = _boMotor | 0x08;// 00001000
            _doLed[2] = 1;
            
            }else if(u8Ct1[3]){
                u8Ct1[3] = 2;
        
            }else if((u8Ct1[2] == u8Ct2[3])){
                u8Ct1[2]=0;
                u8Ct2[3]=1;
                //leg down
                _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
                _boMotor = _boMotor | 0x04;//00000100
                _doLed[3] = 1;   
                bWait[1] = 1;
            }else{
                u8Ct2[3] = 2;
            }
 
            bClim[3] = 1;
        }else if(limit[3]){
            bClim[3] = 0;    
        }
    
    
        /*右の方 リミットスイッチ4~7 */
    
        if(!limit[4] && !bClim[4]){ 
            
            /* 5個目*/
            if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]){
                u8Ct1[4] = 1;
                //leg up
                _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
                _boMotor = _boMotor | 0x20;//00100000
                
            }else if(u8Ct1[4]){
                u8Ct1[4] = 2;
        
            }else if((u8Ct1[5] == u8Ct2[4])){
                u8Ct1[5]=0;
                u8Ct2[4]=1;
                //leg down
                _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
                _boMotor = _boMotor | 0x10;//0x10 = 00010000
                bWait[2] = 1; 
                
            }else{
                u8Ct2[4] = 2;
            }
            bClim[4] = 1;
        
        }else if(limit[4]){
            bClim[4] = 0;
        }
    
    
        if(!limit[5] && !bClim[5]){
            
            /* 6個目*/
            if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]){
                u8Ct1[5] = 1;
                //leg up
                _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
                _boMotor = _boMotor | 0x20;// 00100000
                
            }else if(u8Ct1[5]){
                u8Ct1[5] = 2;
                
            }else if((u8Ct1[4] == u8Ct2[5])){
                u8Ct1[4]=0;
                u8Ct2[5]=1;
                //leg down
                _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
                _boMotor = _boMotor | 0x10;// 00010000
                bWait[2] = 1; 
            
            }else{
                u8Ct2[5] = 2;
            
            }
            bClim[5] = 1;
    
        }else if(limit[5]){
            bClim[5] = 0;    
        }
    
    
        if(!limit[6] && !bClim[6]){
            
            /* 7個目*/
            if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]){
                u8Ct1[6] = 1;
                //legs up
                _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
                _boMotor = _boMotor | 0x02;//00000010
            
            }else if(u8Ct1[6]){
                u8Ct1[6] = 2;
        
            }else if((u8Ct1[7] == u8Ct2[6])){
                u8Ct1[7]=0;
                u8Ct2[6]=1;
                //leg down
                _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
                _boMotor = _boMotor | 0x01;//00000001
                bWait[3] = 1;
            }else{
                u8Ct2[6] = 2;
            }

            bClim[6] = 1;
        }else if(limit[6]){
            bClim[6] = 0;
        }



        if(!limit[7] && !bClim[7]){
            /* 8個目*/
            if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]){
                u8Ct1[7] = 1;
                //leg up
            _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
            _boMotor = _boMotor | 0x02;// 00000010
            //_doLed[6] = 1;
            
            }else if(u8Ct1[7]){
                u8Ct1[7] = 2;
        
            }else if((u8Ct1[6] == u8Ct2[7])){
                u8Ct1[6]=0;
                u8Ct2[7]=1;
                //leg down
                _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
                _boMotor = _boMotor | 0x01;//00000001   
                bWait[3] = 1;
                
            }else{
                u8Ct2[7] = 2;
            }
 
            bClim[7] = 1;
        }else if(limit[7]){
            bClim[7] = 0;    
        }
        
    }
}