File content as of revision 9:4b15d9a3e468:

/*
 *覚え書き
 *
 *mbedのピン配置に対するこのプログラムのピンの利用
 *p5  ~ p12 Motor ...BusOut     :Motor moved
 *p13 ~ p20 Photo ...BusIn      :Photo Sensor
 *p21 ~ p24 Pwm   ...PwmOut     :Motor Pwm Signal
 *p25 ~ p28 Tempra...BusOut     :Motor Tempra Moved
 *p29,p30   Sw[2] ...DigitalIn  :Swich Multi Use
 *LED1 ~ LED4 Led ...DigitalOut :Led Brinked
 *
 *
 *@file
 *
 *My program.
 *
 *
 *
 *
 */

#include "mbed.h"


#define MOTOR_NUM 8//モーター，センサーの数
#define MOVE_TIME 0.090//足を上げる時間
#define MOVE_DOWN_TIME 0.10//足を下げる時間
#define TEMPRA_TIME 4.0 //ワンショットタイマー用時間設定
#define WAIT_TIME 0.010//上げ下げの待ち時間
#define PWM_LEVEL 1.0//モーターの回転速度のPWM


Serial pc(USBTX,USBRX);

BusIn Photo(p5,p6,p7,p8,p9,p10,p11,p12);
DigitalIn Sw[2] = {p29,p30};

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 StopWait[4];
Timer TempraTimer;



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

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


void WaitStop0()
{
    Wait[0] =  0;
}
void WaitStop1()
{
    Wait[1] =  0;
}
void WaitStop2()
{
    Wait[2] =  0;
}
void WaitStop3()
{
    Wait[3] =  0;
}
void MotorStop0()
{
    Motor = Motor & 0x7F;
}



void MotorStop1()
{
    Motor = Motor & 0xBF;
    StopWait[0].attach(&WaitStop0,WAIT_TIME);
}
void MotorStop2()
{
    Motor = Motor & 0xF7;
}
void MotorStop3()
{
    Motor = Motor & 0xFB;
    StopWait[1].attach(&WaitStop1,WAIT_TIME);
}
void MotorStop4()
{
    Motor = Motor & 0xDF;
}
void MotorStop5()
{
    Motor = Motor & 0xEF;
    StopWait[2].attach(&WaitStop2,WAIT_TIME);
}
void MotorStop6()
{
    Motor = Motor & 0xFD;
}
void MotorStop7()
{
    Motor = Motor & 0xFE;
    StopWait[3].attach(&WaitStop3,WAIT_TIME);
}


void TempraStop()
{
    Tempra = 0x00;
}

void Photo0() //zero Up
{
    if(Way == 0) {
        Way = 1;
    }
    if((Way == 1)&&!Wait[0]) {
        if(!MotorState[0]) {
            StopTimer[0].attach(&MotorStop0,MOVE_TIME);
            Motor = Motor | 0x80;//10000000
            MotorState[0] = 1;
            Led[0]= 1;
        } else {
            Flag[0] = 1;
        }
    }
}
void Photo1() //one Down
{
    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;
            Wait[0] = 1;
        }
    }
}
void Photo2() //two Up
{
    if((Way == 1)&&!Wait[1]) {
        if(!MotorState[1]) {
            StopTimer[2].attach(&MotorStop2,MOVE_TIME);
            Motor = Motor | 0x08;
            MotorState[1] = 1;
            Led[1] = 1;
        } else {
            Flag[1] = 1;
        }
    }
}
void Photo3()  //three Down
{
    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;
            Wait[1] = 1;
        }
    }
}
void Photo4() //four Up
{
    if(Way == 0) {
        Way = 1;
    }
    if((Way == 1)&&!Wait[2]) {
        if(!MotorState[2]) {
            StopTimer[4].attach(&MotorStop4,MOVE_TIME);
            Motor = Motor | 0x20;//00100000
            MotorState[2] = 1;
            Led[2] = 1;
        } else {
            Flag[2] = 1;
        }
    }
}
void Photo5() //five Down
{
    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;
            Wait[2] = 1;
        }
    }
}
void Photo6() //six Up
{
    if((Way == 1)&&!Wait[3]) {
        if(!MotorState[3]) {
            StopTimer[6].attach(&MotorStop6,MOVE_TIME);
            Motor = Motor | 0x02;
            MotorState[3] = 1;
            Led[3] = 1;
        } else {
            Flag[3] = 1;
        }
    }
}
void Photo7() //seven Down
{
    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;
            Wait[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;
    DownCount = 0;
    TempraFlag = 1;

}

void GainSetting ()
{

    bool CkFlag[8] = {1,1,1,1,1,1,1,1},PhotoCk[8] = {0};

    Reset();
    Tempra = 0;
    TempraFlag = 0;
    Led[0] = Led[1] = Led[2] = Led[3] = 1;

    while(1) {
        if(!Sw[0]) {
            while(1) {
                PhotoCk[0] = ((~Photo)&0x01);
                PhotoCk[1] = ((~Photo)&0x02) >> 1;
                PhotoCk[2] = ((~Photo)&0x04) >> 2;
                PhotoCk[3] = ((~Photo)&0x08) >> 3;
                if(PhotoCk[0]&&CkFlag[0]) {
                    Led[0] = 0;
                    CkFlag[0] = 0;
                } else if(!PhotoCk[0]) {
                    CkFlag[0] = 1;
                    Led[0] = 1;
                }

                if(PhotoCk[1]&&CkFlag[1]) {
                    Led[1] = 0;
                    CkFlag[1] = 0;
                } else if(!PhotoCk[1]) {
                    CkFlag[1] = 1;
                    Led[1] = 1;
                }
                if(PhotoCk[2]&&CkFlag[2]) {
                    Led[2] = 0;
                    CkFlag[2] = 0;
                } else if(!PhotoCk[2]) {
                    CkFlag[2] = 1;
                    Led[2] = 1;
                }
                if(PhotoCk[3]&&CkFlag[3]) {
                    Led[3] = 0;
                    CkFlag[3] = 0;
                } else if(!PhotoCk[3]) {
                    CkFlag[3] = 1;
                    Led[3] = 1;
                }
                if(!Sw[1]) break;
            }
        }

        PhotoCk[4] = ((~Photo)&0x10) >> 4;
        PhotoCk[5] = ((~Photo)&0x20) >> 5;
        PhotoCk[6] = ((~Photo)&0x40) >> 6;
        PhotoCk[7] = ((~Photo)&0x80) >> 7;

        if(PhotoCk[4]&&CkFlag[4]) {
            Led[0] = 0;
            CkFlag[4] = 0;
        } else if(!PhotoCk[4]) {
            CkFlag[4] = 1;
            Led[0] = 1;
        }
        if(PhotoCk[5]&&CkFlag[5]) {
            Led[1] = 0;
            CkFlag[5] = 0;
        } else if(!PhotoCk[5]) {
            CkFlag[5] = 1;
            Led[1] = 1;
        }
        if(PhotoCk[6]&&CkFlag[6]) {
            Led[2] = 0;
            CkFlag[6] = 0;
        } else if(!PhotoCk[6]) {
            CkFlag[6] = 1;
            Led[2] = 1;
        }
        if(PhotoCk[7]&&CkFlag[7]) {
            Led[3] = 0;
            CkFlag[7] = 0;
        } else if(!PhotoCk[7]) {
            CkFlag[7] = 1;
            Led[3] = 1;
        }

    }

}


void AndCount() //"Legs" Up and Down counting
{

    DownCount ++;
    And[0] = 0 ;
    And[1] = 0 ;
    And[2] = 0 ;
    And[3] = 0 ;
    if(DownCount > 3) {
        DownCount = 0;
        Way = 0;
    }
}

void SetUp()
{

    Pwm[0] = PWM_LEVEL ;
    Pwm[1] = PWM_LEVEL ;
    Pwm[2] = PWM_LEVEL ;
    Pwm[3] = PWM_LEVEL ;
    Photo.mode(PullUp);
    Sw[0].mode(PullUp);
    Sw[1].mode(PullUp);

    TempraTimer.reset();
}

int main()
{
    SetUp();

    uint8_t Ct = 0,PhotoState = 0,CkPhoto[4] = {0};
    bool CkFlag[8] = {1,1,1,1,1,1,1,1};
    void (*MotorMove[8])();

    MotorMove[0] = Photo0;
    MotorMove[1] = Photo1;
    MotorMove[2] = Photo2;
    MotorMove[3] = Photo3;
    MotorMove[4] = Photo4;
    MotorMove[5] = Photo5;
    MotorMove[6] = Photo6;
    MotorMove[7] = Photo7;

    wait(0.5);


    while(1) {

        PhotoState = ~Photo;
        CkPhoto[0] = PhotoState & 0x03;
        CkPhoto[1] = (PhotoState & 0x0C) >> 2;
        CkPhoto[2] = (PhotoState & 0x30) >> 4;
        CkPhoto[3] = (PhotoState & 0xC0) >> 6 ;

        if(CkPhoto[0]==0) { //@remarks 右前足
            CkFlag[0] = 1;
            CkFlag[1] = 1;
        } else if(CkPhoto[0]<3) {
            CkPhoto[0] -= 1;
            if(CkFlag[CkPhoto[0]]) {
                (*MotorMove[CkPhoto[0]])();
                CkFlag[CkPhoto[0]] = 0;
            }
        }

        if(CkPhoto[1]==0) { //@remarks 右後足
            CkFlag[2] = 1;
            CkFlag[3] = 1;
        } else if(CkPhoto[1]<3) {
            CkPhoto[1] += 1;
            if(CkFlag[CkPhoto[1]]) {
                (*MotorMove[CkPhoto[1]])();
                CkFlag[CkPhoto[1]] = 0;
            }
        }

        if(CkPhoto[2]==0) { //@remarks 左前足
            CkFlag[4] = 1;
            CkFlag[5] = 1;
        } else if(CkPhoto[2]<3) {
            CkPhoto[2] += 3;
            if(CkFlag[CkPhoto[2]]) {
                (*MotorMove[CkPhoto[2]])();
                CkFlag[CkPhoto[2]] = 0;
            }
        }

        if(CkPhoto[3]==0) { //@remarks 左後足
            CkFlag[6] = 1;
            CkFlag[7] = 1;
        } else if(CkPhoto[3]<3) {
            CkPhoto[3] += 5;
            if(CkFlag[CkPhoto[3]]) {
                (*MotorMove[CkPhoto[3]])();
                CkFlag[CkPhoto[3]] = 0;
            }
        }
        if(And[0]&&And[1]&&And[2]&&And[3]) {
            AndCount();
        }

        if((MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3])&&TempraFlag) {
            Tempra = 0x0A;
            StopTempra.attach(&TempraStop,TEMPRA_TIME);
        }

        if(!Sw[0]&&!Sw[1]) {
            Ct = 0;
            while(!Sw[0]&&!Sw[1]&&(Ct <= 100)) {
                Ct++;
            }
            if(Ct >= 100) {
                GainSetting();
            }

        } else if(!Sw[0]) {
            Ct = 0;
            while(!Sw[0]&&(Ct <= 100)) {
                Ct ++;
                //printf("%d\n",Ct);
            }
            if(Ct >= 100) {
                Reset();
            }

        } else if(!Sw[1]) {
            Ct = 0;
            while(!Sw[1]&&(Ct <= 100)) {
                Ct ++;
                //printf("%d\n",Ct);
            }
            if(Ct >= 100) {
                Tempra = 0;
                TempraFlag = 0;
            }
        }
    }
}