足上げセンサ八個版仮
Dependencies: mbed
Fork of 4LegUpDown_8sense by
main.cpp
- Committer:
- ryuna
- Date:
- 2014-09-27
- Revision:
- 11:748134b48d24
- Parent:
- 10:87c6b540b3b3
- Child:
- 12:8ff721b67e49
File content as of revision 11:748134b48d24:
/* *覚え書き * ***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 :Switch Multi Use *LED1 ~ LED4 Led ...DigitalOut :Led Brinked * ********************** ***関数の内容 *void WaitStop() :use wait *void MotorStop() :erase motor pulses *void TempraStop() :stop motor of crawler *void Photo() :main processing ,wheel(lift or lower)motor move start *void Reset() :wheels lower *void GainSetting():sensor check mode,blinking Led *void AndCount() :and count *void SetUp() :first in function *int main() :main function * *********************** ***追記 * * * */ #include "mbed.h" #define MOTOR_NUM 8 //motor & sensor num #define MOVE_TIME 0.10 //lift a wheel for 0.090 seconds #define MOVE_DOWN_TIME 0.115 //lower a wheel for 0.10 seconds #define TEMPRA_TIME 4.0 //move a crawler for 4.0 seconds #define WAIT_TIME 0.010 //make wait time #define PWM_LEVEL 1.0 //motors pwm setting level 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]; 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 & 0xBF; 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 & 0x7F;// 01111111 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 & 0xFB; Motor = Motor | 0x08;//00001000 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 & 0xF7; Motor = Motor | 0x04;//00000100 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 & 0xEF; 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 & 0xDF; Motor = Motor | 0x10;//00010000 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 & 0xFE; Motor = Motor | 0x02;//00000010 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 & 0xFD; Motor = Motor | 0x01;//00000001 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;//01000000 } 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() //"Wheels" 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); } 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; /* *use function pointer for go photo0~7 */ 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) { // 右前足 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) { // 右後足 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) { // 左前足 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) { // 左後足 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 ++; } if(Ct >= 100) { Reset(); } } else if(!Sw[1]) { Ct = 0; while(!Sw[1]&&(Ct <= 100)) { Ct ++; } if(Ct >= 100) { Tempra = 0; TempraFlag = 0; } } } }