File content as of revision 6:6664c6aa67d3:

#include "mbed.h"

#define MOTOR_NUM 8
#define MOVE_TIME 0.080
#define MOVE_DOWN_TIME 0.090
#define TEMPRA_TIME 5.0
#define PWM_LEVEL 1.0

Serial pc(USBTX,USBRX);

InterruptIn Photo[8] = {p5,p6,p7,p8,p9,p10,p11,p12};
//InterruptIn Sw(NC);


DigitalOut Led[4] = {LED1, LED2, LED3, LED4 };
BusOut Motor(p13, p14, p15, p16, p17, p18, p19, p20 );//{left front, right front, left back ，right back }
BusOut Tempra(p25,p26,p27,p28);
PwmOut Pwm[4] = {p21, p22, p23, p24 }; 

Timeout StopTimer[8];
Timeout StopTempra;
//Timeout StopTempra;


volatile bool MotorState[4] = {0},Flag[4] = {0},FlagAdd[4] = {0},And[4] = {0};
volatile uint8_t Way = 0,UpCt = 0;
volatile bool TempraFlag = 0;

/*motorstate 上がっているか下がっているか


*/

/*
bMotor = {
          LeftFrontUp, LeftFrontDown,  
          RightFrontUp, RightFrontDown,
          LeftBackUp, LeftBackDown,   
          RightBackUp, RightBackDown 
          };
*/



void MotorStop0(){
    
    Motor = Motor & 0x7F;
    
}
void MotorStop1(){

    Motor = Motor & 0xBF; 
    
}

void MotorStop2(){
    
    Motor = Motor & 0xF7;
    
}
void MotorStop3(){
    
    Motor = Motor & 0xFB;

}
void MotorStop4(){
    
    Motor = Motor & 0xDF;

}
void MotorStop5(){

    Motor = Motor & 0xEF; 

}
void MotorStop6(){
    
    Motor = Motor & 0xFD;

}
void MotorStop7(){
    
    Motor = Motor & 0xFE;
    
}
void TempraStop(){
    Tempra = 0x00;   
    
}
void FallPhoto0(){//zero
    //Wayの決定
    if(Way == 0){
        Way = 1;
    }
    
    if(Way == 1){
        if(!MotorState[0]){
            
            StopTimer[0].attach(&MotorStop0,MOVE_TIME);
            Motor = Motor | 0x80;//10000000       
            MotorState[0] = 1;     
            Led[0]= 1;
        }else{
            Flag[0] = 1;
        }
    }/*else if(Way == 2){
        
        if(Flag[0]&&!FlagAdd[0]){
            FlagAdd[0] = 1;
        }else if(MotorState[0]){
            StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
            Motor = Motor | 0x40;//0x40 = 01000000
            MotorState[0] = 0;
            Flag[0] = 0;
            Led[0] = 0;
            And[0] = 1;
            
        }
    }*/
}

void FallPhoto1(){//one
    //Wayの決定
    /*if(Way ==0){
        Way = 2;
    }*/
    
    /*if(Way == 2){
        if(!MotorState[0]){
            
            StopTimer[0].attach(&MotorStop0,MOVE_TIME);
            Motor = Motor | 0x80;//10000000       
            MotorState[0] = 1;     
            Led[0] = 1;
        }else{
            Flag[0] = 1;
        }
    }else */
    if(Way == 1){
        
        if(Flag[0]&&!FlagAdd[0]){
            FlagAdd[0] = 1;
        }else if(MotorState[0]) {
            StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
            Motor = Motor | 0x40;//0x40 = 01000000
            MotorState[0] = 0;
            Flag[0] = 0;
            FlagAdd[0] = 0;
            Led[0] = 0;
            And[0] = 1;
            
        }
    }

}
void FallPhoto2(){//two
        //Wayの決定
    
    if(Way == 1){
        if(!MotorState[1]){
            
            StopTimer[2].attach(&MotorStop2,MOVE_TIME);
            Motor = Motor | 0x08;       
            MotorState[1] = 1;     
            Led[1] = 1;
        }else{
            Flag[1] = 1;
        }
    }/*else if(Way == 2){
        
        if(Flag[1]&&!FlagAdd[1]){
            FlagAdd[1] = 1;
        }else if(MotorState[1]){
            StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
            Motor = Motor | 0x04;
            MotorState[1] = 0;
            Flag[1] = 0;
            Led[1] = 0;
            And[1] = 1;
        }
   } */

}
void FallPhoto3(){ //three
    //Way
    
    /*if(Way == 2){
        if(!MotorState[1]){
            
            StopTimer[2].attach(&MotorStop2,MOVE_TIME);
            Motor = Motor | 0x08;       
            MotorState[1] = 1;
            Led[1] = 1;     
        }else{
            Flag[1] = 1;
        }
    }else */
    if(Way == 1){
        
        if(Flag[1]&&!FlagAdd[1]){
            FlagAdd[1] = 1;
        }else if(MotorState[1]){
            StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
            Motor = Motor | 0x04;
            MotorState[1] = 0;
            Flag[1] = 0;
            FlagAdd[1] = 0;
            Led[1] = 0;
            And[1] = 1;
            
        }
   } 
}
void FallPhoto4(){//four
    
    if(Way ==0){
        Way = 1;
    }
    if(Way == 1){
        if(!MotorState[2]){
            StopTimer[4].attach(&MotorStop4,MOVE_TIME);
            Motor = Motor | 0x20;//00100000  
            MotorState[2] = 1;     
            Led[2] = 1;
        }else{
            Flag[2] = 1;
        }
    }/*else if(Way == 2){
        
        if(Flag[2]&&!FlagAdd[2]){
            FlagAdd[2] = 1;
        }else if(MotorState[2]){
            StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
            Motor = Motor | 0x10;
            MotorState[2] = 0;
            Flag[2] = 0;
            Led[2] = 0;
            And[2] = 1;
            
        }
   } */

}
void FallPhoto5(){//five
    /*if(Way ==0){
        Way = 2;
    }*/
    
    /*if(Way == 2){
        if(!MotorState[2]){
            StopTimer[4].attach(&MotorStop4,MOVE_TIME);
            Motor = Motor | 0x20;//00100000  
            MotorState[2] = 1;
            Led[2] = 1;     
        }else{
            Flag[2] = 1;
        }
    }else */
    if(Way == 1){
        
        if(Flag[2]&&!FlagAdd[2]){
            FlagAdd[2] = 1;
        }else if(MotorState[2]){
            StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
            Motor = Motor | 0x10;
            MotorState[2] = 0;
            Flag[2] = 0;
            FlagAdd[2] = 0;
            Led[2] = 0;
            And[2] = 1;
        }
   } 
}
void FallPhoto6(){//six
    
    
    if(Way == 1){
        if(!MotorState[3]){
            StopTimer[6].attach(&MotorStop6,MOVE_TIME);
            Motor = Motor | 0x02;  
            MotorState[3] = 1;
            Led[3] = 1;     
        }else{
            Flag[3] = 1;
        }
    }/*else if(Way == 2){
        
        if(Flag[3]&&!FlagAdd[3]){
            FlagAdd[3] = 1;
        }else if(MotorState[3]){
            StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
            Motor = Motor | 0x01;
            MotorState[3] = 0;
            Flag[3] = 0;
            Led[3] = 0;
            And[3] = 1;
        }
   }   */          
}
void FallPhoto7(){//seven
    
    
    
    /*if(Way == 2){
        if(!MotorState[3]){
            StopTimer[6].attach(&MotorStop6,MOVE_TIME);
            Motor = Motor | 0x02;  
            MotorState[3] = 1;
            Led[3] = 1;     
        }else{
            Flag[3] = 1;
        }
    }else */
    if(Way == 1){
        
        if(Flag[3]&&!FlagAdd[3]){
            FlagAdd[3] = 1;
        }else if(MotorState[3]){
            StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
            Motor = Motor | 0x01;
            MotorState[3] = 0;
            Flag[3] = 0;
            FlagAdd[3] = 0;
            Led[3] = 0;
            And[3] = 1;
        }
   }             
}

void Reset(){
    
    int i;

    if(MotorState[0]){
        StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
        Motor = Motor | 0x40;//
    }
    if(MotorState[1]){
        StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
        Motor = Motor | 0x04;//00000100
    }
    if(MotorState[2]){
        StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
        Motor = Motor | 0x10;//00010000
    }
    if(MotorState[3]){
        StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
        Motor = Motor | 0x01;//00000001   
    }
    for(i = 0; i > 4; i++){
        Led[i] = 0;
        Flag[i] = 0; 
        FlagAdd[i] = 0;
        MotorState[i] = 0;
        And[i] = 0;
    }
    Way = 0;
    UpCt = 0;
    
}

void NumCount(){//足を上げた回数を記録
    UpCt ++;
    And[0] = 0 ;
    And[1] = 0 ;
    And[2] = 0 ;
    And[3] = 0 ;
    TempraFlag = 0;
    if(UpCt > 3){
        UpCt = 0;
        Way = 0;
    }
}

void SetUp(){
    //int i;   
    
    Pwm[0] = PWM_LEVEL ;
    Pwm[1] = PWM_LEVEL ;
    Pwm[2] = PWM_LEVEL ;
    Pwm[3] = PWM_LEVEL ;
    //for(i = 0 ; i > MOTOR_NUM ; i++ ){
        Photo[0].mode(PullUp);
        Photo[1].mode(PullUp);
        Photo[2].mode(PullUp);
        Photo[3].mode(PullUp);
        Photo[4].mode(PullUp);
        Photo[5].mode(PullUp);
        Photo[6].mode(PullUp);
        Photo[7].mode(PullUp);
    
    //}
    Photo[0].fall(FallPhoto0);
    Photo[1].fall(FallPhoto1);
    Photo[2].fall(FallPhoto2);
    Photo[3].fall(FallPhoto3);
    Photo[4].fall(FallPhoto4);
    Photo[5].fall(FallPhoto5);
    Photo[6].fall(FallPhoto6);
    Photo[7].fall(FallPhoto7);
    
  //  Sw.mode(PullUp);                                   
  //  Sw.fall(Reset);
 

}

int main() {
    
    SetUp();    
    wait(0.5);  
    
    while(1){
        if(MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3]){
            TempraFlag = 1;
            Tempra = 0x0A;
        }else if(TempraFlag){
            StopTempra.attach(&TempraStop,TEMPRA_TIME);
            TempraFlag = 0;
        }
            
        if(And[0]&&And[1]&&And[2]&&And[3]){
            NumCount();
        }
        
        //printf("%d\t%d\n",UpCt,Way);
    }
}