File content as of revision 1:807091115672:

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


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

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

pin rise/fall attach use
*//**/


Serial pc(USBTX,USBRX);

Timeout _toMotor[8];
Timeout _toWait[4];

DigitalIn  limit[8] = {NC,NC,NC,NC,NC,NC,NC,NC };
InterruptIn _iiLsw[8] = {p5,p6,p7,p8,p9,p10,p11,p12 };/*swicth is a-contact*/

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};
volatile bool bMotor[8] = {0};//名前が似ているものがあるので注意
volatile uint8_t u8Ct1[8] = {0},u8Ct2[8] = {1,1,1,1,1,1,1,1};//uint8_t Count{1,2}[]
    

enum {LFU = 0,LFD,RFU,RFD,LBU,LBD,RBU,RBD};
/*
bMotor = {
          LeftFrontUp, LeftFrontDown,  
          RightFrontUp, RightFrontDown,
          LeftBackUp, LeftBackDown,   
          RightBackUp, RightBackDown 
          };
*/

void Motor_state(){
/*
uint8_t GetBit(uint8_t n, uint8_t bit)
     return (n >>(bit-1))%2;
}by takamastu
*/
    uint8_t i;
    
    for (i = 0; i < 8 ; i++){
        bMotor[i] = ( _boMotor >> i )%2;
    }
    
}

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[0] = 0;
    
}

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

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


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

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

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

void _aiiFLsw0(){  //attach interruptIn fall Limit switch
    /* 1個目*/
    if(!bWait[0] && !u8Ct1[0] && !u8Ct1[1]&&((_boMotor = 0x00)||bMotor[2]||bMotor[5]||bMotor[7])){
        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])&&((_boMotor = 0x00)||bMotor[3]||bMotor[4]||bMotor[6])){
        u8Ct1[1]=0;
        u8Ct2[0]=1;
        //legs down
        _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
        _boMotor = _boMotor | 0x40;//0x40 = 01000000
        _doLed[0] = 1;
        bWait[0] = 1; 
                
    }else{
        u8Ct2[0] = 2;
    
    }
}

void _aiiFLsw1(){  //attach interruptIn fall Limit switch
    /* 2個目*/
    if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]&&((_boMotor = 0x00)||bMotor[2]||bMotor[5]||bMotor[7])){
        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])&&((_boMotor = 0x00)||bMotor[3]||bMotor[4]||bMotor[6])){
        u8Ct1[0]=0;
        u8Ct2[1]=1;
        //legs down
        _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
        _boMotor = _boMotor | 0x40;// 01000000
        _doLed[0] = 1;
        bWait[0] = 1; 
            
    }else{
        u8Ct2[1] = 2;
            
    }

}
void _aiiFLsw2(){  //attach interruptIn fall Limit switch
    /* 3個目*/
    if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
        u8Ct1[2] = 1;
        //legs up
        _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
        _boMotor = _boMotor | 0x08;//00001000
        _doLed[1] = 1;
    
    }else if(u8Ct1[2]){
        u8Ct1[2] = 2;
        
    }else if((u8Ct1[3] == u8Ct2[2])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[7])){
        u8Ct1[3]=0;
        u8Ct2[2]=1;
        //legs down
        _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
        _boMotor = _boMotor | 0x04;//00000100
        _doLed[1] = 1;   
        bWait[1] = 1;
    
    }else{
        u8Ct2[2] = 2;
    }

}
void _aiiFLsw3(){  //attach interruptIn fall Limit switch
    /* 4個目*/
    if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
        u8Ct1[3] = 1;
        //leg up
        _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
        _boMotor = _boMotor | 0x08;// 00001000
        _doLed[1] = 1;
            
    }else if(u8Ct1[3]){
        u8Ct1[3] = 2;
        
    }else if((u8Ct1[2] == u8Ct2[3])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[7])){
        u8Ct1[2]=0;
        u8Ct2[3]=1;
        //leg down
        _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
        _boMotor = _boMotor | 0x04;//00000100
        _doLed[1] = 1;   
        bWait[1] = 1;
        
    }else{
        u8Ct2[3] = 2;
    }
}
void _aiiFLsw4(){  //attach interruptIn fall Limit switch
    /* 5個目*/
    if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]&&((_boMotor = 0x00)||bMotor[0]||bMotor[5]||bMotor[7])){
        u8Ct1[4] = 1;
        //leg up
        _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
        _boMotor = _boMotor | 0x20;//00100000
        _doLed[2] = 0;
                
    }else if(u8Ct1[4]){
        u8Ct1[4] = 2;
        
    }else if((u8Ct1[5] == u8Ct2[4])&&((_boMotor = 0x00)||bMotor[1]||bMotor[4]||bMotor[6])){
        u8Ct1[5]=0;
        u8Ct2[4]=1;
        //leg down
        _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
        _boMotor = _boMotor | 0x10;//0x10 = 00010000
        _doLed[2] = 0;
        bWait[2] = 1; 
                
    }else{
        u8Ct2[4] = 2;
    }
}
void _aiiFLsw5(){  //attach interruptIn fall Limit switch
    /* 6個目*/
    if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]&&((_boMotor = 0x00)||bMotor[0]||bMotor[5]||bMotor[7])){
        u8Ct1[5] = 1;
        //leg up
        _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
        _boMotor = _boMotor | 0x20;// 00100000
        _doLed[2] = 0;
                
    }else if(u8Ct1[5]){
        u8Ct1[5] = 2;
                
    }else if((u8Ct1[4] == u8Ct2[5])&&((_boMotor = 0x00)||bMotor[1]||bMotor[4]||bMotor[6])){
        u8Ct1[4]=0;
        u8Ct2[5]=1;
        //leg down
        _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
        _boMotor = _boMotor | 0x10;// 00010000
        _doLed[2] = 0;
        bWait[2] = 1; 
            
    }else{
        u8Ct2[5] = 2;
            
    }
}
void _aiiFLsw6(){  //attach interruptIn fall Limit switch
            
    /* 7個目*/
    if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[4])){
        u8Ct1[6] = 1;
        //legs up
        _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
        _boMotor = _boMotor | 0x02;//00000010
        _doLed[3] = 0;
                
    }else if(u8Ct1[6]){
        u8Ct1[6] = 2;
        
    }else if((u8Ct1[7] == u8Ct2[6])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[5])){
        u8Ct1[7]=0;
        u8Ct2[6]=1;
        //leg down
        _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
        _boMotor = _boMotor | 0x01;//00000001
        bWait[3] = 1;
        _doLed[3] = 0;
                
    }else{
        u8Ct2[6] = 2;
    }
}
void _aiiFLsw7(){  //attach interruptIn fall Limit switch
    /* 8個目*/
    if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[4])){
        u8Ct1[7] = 1;
        //leg up
        _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
        _boMotor = _boMotor | 0x02;// 00000010
        //_doLed[6] = 1;
        _doLed[3] = 0;
        
    }else if(u8Ct1[7]){
        u8Ct1[7] = 2;
        
    }else if((u8Ct1[6] == u8Ct2[7])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[5])){
        u8Ct1[6]=0;
        u8Ct2[7]=1;
        //leg down
        _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
        _boMotor = _boMotor | 0x01;//00000001   
        bWait[3] = 1;
        _doLed[3] = 0;
                
    }else{
        u8Ct2[7] = 2;
        }
}




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);
    */
    
    //Limit switch mode pullup   
    _iiLsw[0].mode(PullUp);
    _iiLsw[1].mode(PullUp);
    _iiLsw[2].mode(PullUp);
    _iiLsw[3].mode(PullUp);
    _iiLsw[4].mode(PullUp);
    _iiLsw[5].mode(PullUp);
    _iiLsw[6].mode(PullUp);
    _iiLsw[7].mode(PullUp);

                                      
    //interrupt attach fall set use 
    _iiLsw[0].fall(_aiiFLsw0);
    _iiLsw[1].fall(_aiiFLsw1);
    _iiLsw[2].fall(_aiiFLsw2);
    _iiLsw[3].fall(_aiiFLsw3);
    _iiLsw[4].fall(_aiiFLsw4);
    _iiLsw[5].fall(_aiiFLsw5);
    _iiLsw[6].fall(_aiiFLsw6);
    _iiLsw[7].fall(_aiiFLsw7);



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


    wait(0.5);
    Motor_state();
    
    while(1){
        Motor_state();
        wait(0.15);
        _doLed[0] = !_doLed[0];
        
        
    
    }
    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[0] = 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[0] = 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[1] = 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[1] = 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[1] = 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[1] = 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
                _doLed[2] = 0;
                
            }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
                _doLed[2] = 0;
                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
                _doLed[2] = 0;
                
            }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
                _doLed[2] = 0;
                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
                _doLed[3] = 0;
                
            }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;
                _doLed[3] = 0;
                
            }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;
            _doLed[3] = 0;
            
            }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;
                _doLed[3] = 0;
                
            }else{
                u8Ct2[7] = 2;
            }
 
            bClim[7] = 1;
        }else if(limit[7]){
            bClim[7] = 0;    
        }
        
    }
}