Bチーム
足上げセンサ八個版仮
Dependencies: mbed
Fork of
4LegUpDown_8sense
by 
Bチーム
Diff: main.cpp
- Revision:
- 1:807091115672
- Parent:
- 0:76ba33d3b4a6
- Child:
- 2:89cf1003e119
--- a/main.cpp Wed Aug 20 08:59:12 2014 +0000
+++ b/main.cpp Thu Aug 21 09:45:46 2014 +0000
@@ -9,7 +9,7 @@
はじめのリミットスイッチが押されてもう一度押されるまでの時間を計り現在の速さを概算してタイマー制御に役立てる
-
+pin rise/fall attach use
*//**/
@@ -17,11 +17,14 @@
Timeout _toMotor[8];
Timeout _toWait[4];
-DigitalIn limit[8] = {p5,p6,p7,p8,p9,p10,p11,p12 };
+
+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?
+//{left front, right front, left back ，right back } in sequeNCe
PwmOut _poPwm[4] = {p21, p22, p23, p24 };
/**more use binary number
@@ -32,6 +35,33 @@
**/
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
@@ -73,7 +103,7 @@
_boMotor = _boMotor & 0xBF;
//10111111 指定ピンのみ変更
//left front leg down
- _doLed[1] = 0;
+ _doLed[0] = 0;
}
@@ -82,7 +112,7 @@
_boMotor = _boMotor & 0xF7;
//11110111 指定ピンのみ変更
//left back leg up
- _doLed[2] = 0;
+ _doLed[1] = 0;
}
@@ -91,7 +121,7 @@
_boMotor = _boMotor & 0xFB;
//11111011 指定ピンのみ変更
//left back leg down
- _doLed[3] = 0;
+ _doLed[1] = 0;
}
void _atMotorStop4(){//atatch Motor Stop0
@@ -99,7 +129,7 @@
_boMotor = _boMotor & 0xDF;
//11011111 指定ピンのみ変更
//right front leg up
-
+ _doLed[2] = 0;
}
@@ -109,7 +139,7 @@
_boMotor = _boMotor & 0xEF;
//11101111 指定ピンのみ変更
//right front leg down
-
+ _doLed[2] = 0;
}
@@ -118,7 +148,8 @@
_boMotor = _boMotor & 0xFD;
//11111101 指定ピンのみ変更
//right back leg up
-
+ _doLed[3] = 0;
+
}
void _atMotorStop7(){
@@ -126,17 +157,229 @@
_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}[]
-
+ /*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);
@@ -145,9 +388,29 @@
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);
- wait(1);
+
+ //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 */
@@ -157,8 +420,19 @@
_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]){
@@ -179,7 +453,7 @@
//legs down
_toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
_boMotor = _boMotor | 0x40;//0x40 = 01000000
- _doLed[1] = 1;
+ _doLed[0] = 1;
bWait[0] = 1;
}else{
@@ -211,7 +485,7 @@
//legs down
_toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
_boMotor = _boMotor | 0x40;// 01000000
- _doLed[1] = 1;
+ _doLed[0] = 1;
bWait[0] = 1;
}else{
@@ -233,7 +507,7 @@
//legs up
_toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
_boMotor = _boMotor | 0x08;//00001000
- _doLed[2] = 1;
+ _doLed[1] = 1;
}else if(u8Ct1[2]){
u8Ct1[2] = 2;
@@ -243,7 +517,7 @@
//legs down
_toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
_boMotor = _boMotor | 0x04;//00000100
- _doLed[3] = 1;
+ _doLed[1] = 1;
bWait[1] = 1;
}else{
u8Ct2[2] = 2;
@@ -263,7 +537,7 @@
//leg up
_toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
_boMotor = _boMotor | 0x08;// 00001000
- _doLed[2] = 1;
+ _doLed[1] = 1;
}else if(u8Ct1[3]){
u8Ct1[3] = 2;
@@ -274,7 +548,7 @@
//leg down
_toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
_boMotor = _boMotor | 0x04;//00000100
- _doLed[3] = 1;
+ _doLed[1] = 1;
bWait[1] = 1;
}else{
u8Ct2[3] = 2;
@@ -296,6 +570,7 @@
//leg up
_toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
_boMotor = _boMotor | 0x20;//00100000
+ _doLed[2] = 0;
}else if(u8Ct1[4]){
u8Ct1[4] = 2;
@@ -306,6 +581,7 @@
//leg down
_toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
_boMotor = _boMotor | 0x10;//0x10 = 00010000
+ _doLed[2] = 0;
bWait[2] = 1;
}else{
@@ -326,6 +602,7 @@
//leg up
_toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
_boMotor = _boMotor | 0x20;// 00100000
+ _doLed[2] = 0;
}else if(u8Ct1[5]){
u8Ct1[5] = 2;
@@ -336,6 +613,7 @@
//leg down
_toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
_boMotor = _boMotor | 0x10;// 00010000
+ _doLed[2] = 0;
bWait[2] = 1;
}else{
@@ -357,7 +635,8 @@
//legs up
_toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
_boMotor = _boMotor | 0x02;//00000010
-
+ _doLed[3] = 0;
+
}else if(u8Ct1[6]){
u8Ct1[6] = 2;
@@ -368,6 +647,8 @@
_toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
_boMotor = _boMotor | 0x01;//00000001
bWait[3] = 1;
+ _doLed[3] = 0;
+
}else{
u8Ct2[6] = 2;
}
@@ -387,6 +668,7 @@
_toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
_boMotor = _boMotor | 0x02;// 00000010
//_doLed[6] = 1;
+ _doLed[3] = 0;
}else if(u8Ct1[7]){
u8Ct1[7] = 2;
@@ -398,6 +680,7 @@
_toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
_boMotor = _boMotor | 0x01;//00000001
bWait[3] = 1;
+ _doLed[3] = 0;
}else{
u8Ct2[7] = 2;