足上げセンサ八個版仮
Dependencies: mbed
Fork of 4LegUpDown_8sense by
main.cpp
- Committer:
- ryuna
- Date:
- 2014-08-25
- Revision:
- 2:89cf1003e119
- Parent:
- 1:807091115672
- Child:
- 3:cd7d3613e8e4
File content as of revision 2:89cf1003e119:
#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 >> 7-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; Motor_state(); } void _atMotorStop1(){ _toWait[0].attach(&_atWaitStop0,WAIT_TIME); _boMotor = _boMotor & 0xBF; //10111111 指定ピンのみ変更 //left front leg down _doLed[0] = 0; Motor_state(); } void _atMotorStop2(){ _boMotor = _boMotor & 0xF7; //11110111 指定ピンのみ変更 //left back leg up _doLed[1] = 0; Motor_state(); } void _atMotorStop3(){ _toWait[1].attach(&_atWaitStop1,WAIT_TIME); _boMotor = _boMotor & 0xFB; //11111011 指定ピンのみ変更 //left back leg down _doLed[1] = 0; Motor_state(); } void _atMotorStop4(){//atatch Motor Stop0 _boMotor = _boMotor & 0xDF; //11011111 指定ピンのみ変更 //right front leg up _doLed[2] = 0; Motor_state(); } void _atMotorStop5(){ _toWait[2].attach(&_atWaitStop2,WAIT_TIME); _boMotor = _boMotor & 0xEF; //11101111 指定ピンのみ変更 //right front leg down _doLed[2] = 0; Motor_state(); } void _atMotorStop6(){ _boMotor = _boMotor & 0xFD; //11111101 指定ピンのみ変更 //right back leg up _doLed[3] = 0; Motor_state(); } void _atMotorStop7(){ _toWait[3].attach(&_atWaitStop3,WAIT_TIME); _boMotor = _boMotor & 0xFE; //11111110 指定ピンのみ変更 //right back leg down _doLed[3] = 0; Motor_state(); } 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; Motor_state(); }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; Motor_state(); }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; Motor_state(); }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; Motor_state(); }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; Motor_state(); }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; Motor_state(); }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 u0p _toMotor[2].attach(&_atMotorStop2,MOVE_TIME); _boMotor = _boMotor | 0x08;// 00001000 _doLed[1] = 1; Motor_state(); }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; Motor_state(); }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; Motor_state(); }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; Motor_state(); 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; Motor_state(); }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 Motor_state(); _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 Motor_state(); _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 Motor_state(); 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 Motor_state(); //_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 Motor_state(); bWait[3] = 1; _doLed[3] = 0; }else{ u8Ct2[7] = 2; } } int main() { int m; bool bClim[8] = {1,1,1,1,1,1,1,1};//bool Check limit[] /* 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(); m = _boMotor; pc.printf("_boMotor = %d bMotor = %d%d%d%d%d%d%d%d\n",m, bMotor[0],bMotor[1],bMotor[2],bMotor[3],bMotor[4],bMotor[5], bMotor[6],bMotor[7]); } 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; } } }