Bチーム
足上げセンサ八個版仮
Dependencies: mbed
Fork of
4LegUpDown_8sense
by 
Bチーム
Diff: main.cpp
- Revision:
- 2:89cf1003e119
- Parent:
- 1:807091115672
- Child:
- 3:cd7d3613e8e4
--- a/main.cpp Thu Aug 21 09:45:46 2014 +0000
+++ b/main.cpp Mon Aug 25 05:29:35 2014 +0000
@@ -22,9 +22,8 @@
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
@@ -58,7 +57,7 @@
uint8_t i;
for (i = 0; i < 8 ; i++){
- bMotor[i] = ( _boMotor >> i )%2;
+ bMotor[i] = ( _boMotor >> 7-i )%2;
}
}
@@ -95,6 +94,8 @@
//01111111 指定ピンのみ変更
//left front leg up
_doLed[0] = 0;
+ Motor_state();
+
}
@@ -104,6 +105,7 @@
//10111111 指定ピンのみ変更
//left front leg down
_doLed[0] = 0;
+ Motor_state();
}
@@ -113,6 +115,7 @@
//11110111 指定ピンのみ変更
//left back leg up
_doLed[1] = 0;
+ Motor_state();
}
@@ -122,6 +125,7 @@
//11111011 指定ピンのみ変更
//left back leg down
_doLed[1] = 0;
+ Motor_state();
}
void _atMotorStop4(){//atatch Motor Stop0
@@ -130,6 +134,7 @@
//11011111 指定ピンのみ変更
//right front leg up
_doLed[2] = 0;
+ Motor_state();
}
@@ -140,6 +145,7 @@
//11101111 指定ピンのみ変更
//right front leg down
_doLed[2] = 0;
+ Motor_state();
}
@@ -149,6 +155,7 @@
//11111101 指定ピンのみ変更
//right back leg up
_doLed[3] = 0;
+ Motor_state();
}
@@ -158,22 +165,24 @@
//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])){
+ 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])){
+ }else if((u8Ct1[1] == u8Ct2[0])&&((_boMotor == 0x00)||bMotor[3]||bMotor[4]||bMotor[6])){
u8Ct1[1]=0;
u8Ct2[0]=1;
//legs down
@@ -181,6 +190,7 @@
_boMotor = _boMotor | 0x40;//0x40 = 01000000
_doLed[0] = 1;
bWait[0] = 1;
+ Motor_state();
}else{
u8Ct2[0] = 2;
@@ -190,17 +200,18 @@
void _aiiFLsw1(){ //attach interruptIn fall Limit switch
/* 2個目*/
- if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]&&((_boMotor = 0x00)||bMotor[2]||bMotor[5]||bMotor[7])){
+ 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])){
+ }else if((u8Ct1[0] == u8Ct2[1])&&((_boMotor == 0x00)||bMotor[3]||bMotor[4]||bMotor[6])){
u8Ct1[0]=0;
u8Ct2[1]=1;
//legs down
@@ -208,6 +219,7 @@
_boMotor = _boMotor | 0x40;// 01000000
_doLed[0] = 1;
bWait[0] = 1;
+ Motor_state();
}else{
u8Ct2[1] = 2;
@@ -217,17 +229,18 @@
}
void _aiiFLsw2(){ //attach interruptIn fall Limit switch
/* 3個目*/
- if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
+ 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])){
+ }else if((u8Ct1[3] == u8Ct2[2])&&((_boMotor == 0x00)||bMotor[0]||bMotor[2]||bMotor[7])){
u8Ct1[3]=0;
u8Ct2[2]=1;
//legs down
@@ -235,6 +248,7 @@
_boMotor = _boMotor | 0x04;//00000100
_doLed[1] = 1;
bWait[1] = 1;
+ Motor_state();
}else{
u8Ct2[2] = 2;
@@ -243,17 +257,18 @@
}
void _aiiFLsw3(){ //attach interruptIn fall Limit switch
/* 4個目*/
- if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
+ if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]&&((_boMotor == 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
u8Ct1[3] = 1;
- //leg up
+ //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])){
+ }else if((u8Ct1[2] == u8Ct2[3])&&((_boMotor == 0x00)||bMotor[0]||bMotor[2]||bMotor[7])){
u8Ct1[2]=0;
u8Ct2[3]=1;
//leg down
@@ -261,6 +276,7 @@
_boMotor = _boMotor | 0x04;//00000100
_doLed[1] = 1;
bWait[1] = 1;
+ Motor_state();
}else{
u8Ct2[3] = 2;
@@ -268,23 +284,25 @@
}
void _aiiFLsw4(){ //attach interruptIn fall Limit switch
/* 5個目*/
- if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]&&((_boMotor = 0x00)||bMotor[0]||bMotor[5]||bMotor[7])){
+ 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])){
+ }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{
@@ -293,22 +311,24 @@
}
void _aiiFLsw5(){ //attach interruptIn fall Limit switch
/* 6個目*/
- if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]&&((_boMotor = 0x00)||bMotor[0]||bMotor[5]||bMotor[7])){
+ 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])){
+ }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;
@@ -320,22 +340,24 @@
void _aiiFLsw6(){ //attach interruptIn fall Limit switch
/* 7個目*/
- if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[4])){
+ 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])){
+ }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;
@@ -345,23 +367,25 @@
}
void _aiiFLsw7(){ //attach interruptIn fall Limit switch
/* 8個目*/
- if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[4])){
+ 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])){
+ }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;
@@ -374,10 +398,11 @@
int main() {
-
+
+ int m;
- 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}[]
+ 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);
@@ -426,12 +451,15 @@
while(1){
Motor_state();
- wait(0.15);
- _doLed[0] = !_doLed[0];
+ 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]){