Bチーム
足上げセンサ八個版仮
Dependencies: mbed
Fork of
4LegUpDown_8sense
by 
Bチーム
Diff: main.cpp
- Revision:
- 8:36a85b66796f
- Parent:
- 7:583ad8798bf1
- Child:
- 9:4b15d9a3e468
--- a/main.cpp Fri Sep 12 00:42:47 2014 +0000
+++ b/main.cpp Wed Sep 17 08:30:11 2014 +0000
@@ -1,16 +1,30 @@
+/*
+ *覚え書き
+ *
+ *mbedのピン配置に対するこのプログラムのピンの利用
+ *p5~p12
+ *
+ *
+ *
+ *
+ */
+
#include "mbed.h"
#define MOTOR_NUM 8
#define MOVE_TIME 0.080
#define MOVE_DOWN_TIME 0.090
-#define TEMPRA_TIME 5.0
+#define TEMPRA_TIME 4.0 //ワンショットタイマー用時間設定
+#define CHECK_TIME 5.0 //時間経過でストップするタイマー用時間設定
+#define WAIT_TIME 0.010
#define PWM_LEVEL 1.0
Serial pc(USBTX,USBRX);
-InterruptIn Photo[8] = {p5,p6,p7,p8,p9,p10,p11,p12};
-//InterruptIn Sw(NC);
+BusIn Photo(p5,p6,p7,p8,p9,p10,p11,p12);
+DigitalIn Sw[2] = {p29,p30};
+DigitalIn PhotoCk[8] = {p5,p6,p7,p8,p9,p10,p11,p12};
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 }
@@ -19,15 +33,17 @@
Timeout StopTimer[8];
Timeout StopTempra;
+Timeout StopWait[4];
+Timer TempraTimer;
+
-volatile bool MotorState[4] = {0},Flag[4] = {0},FlagAdd[4] = {0},And[4] = {0};
+volatile bool MotorState[4] = {0},Flag[4] = {0},FlagAdd[4] = {0},And[4] = {0},Wait[4] = {0};;
volatile uint8_t Way = 0,UpCt = 0;
-volatile bool TempraFlag = 0;
-
+volatile bool TempraFlag = 1;
/*motorstate 上がっているか下がっているか
-
++
*/
/*
@@ -40,6 +56,30 @@
*/
+void WaitStop0(){//atatch Time Stop0
+//make delay time
+ Wait[0] = 0;
+
+}
+
+void WaitStop1(){//atatch Time Stop0
+//make delay time
+ Wait[1] = 0;
+
+}
+
+void WaitStop2(){//atatch Time Stop0
+//make delay time
+ Wait[2] = 0;
+
+}
+
+void WaitStop3(){//atatch Time Stop0
+//make delay time
+ Wait[3] = 0;
+
+}
+
void MotorStop0(){
@@ -48,7 +88,7 @@
}
void MotorStop1(){
- Motor = Motor & 0xBF;
+ Motor = Motor & 0xBF;
}
@@ -60,6 +100,7 @@
void MotorStop3(){
Motor = Motor & 0xFB;
+ StopWait[1].attach(&WaitStop1,WAIT_TIME);
}
void MotorStop4(){
@@ -70,6 +111,7 @@
void MotorStop5(){
Motor = Motor & 0xEF;
+ StopWait[2].attach(&WaitStop2,WAIT_TIME);
}
void MotorStop6(){
@@ -80,21 +122,22 @@
void MotorStop7(){
Motor = Motor & 0xFE;
+ StopWait[3].attach(&WaitStop3,WAIT_TIME);
}
void TempraStop(){
Tempra = 0x00;
}
-void FallPhoto0(){//zero
+void Photo0(){//zero
//Wayの決定
if(Way == 0){
Way = 1;
}
- if(Way == 1){
- if(!MotorState[0]){
-
+ if((Way == 1)&&!Wait[0]){
+
+ if(!MotorState[0]){
StopTimer[0].attach(&MotorStop0,MOVE_TIME);
Motor = Motor | 0x80;//10000000
MotorState[0] = 1;
@@ -102,39 +145,11 @@
}else{
Flag[0] = 1;
}
- }/*else if(Way == 2){
-
- 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;
- Led[0] = 0;
- And[0] = 1;
-
- }
- }*/
+ }
}
-void FallPhoto1(){//one
- //Wayの決定
- /*if(Way ==0){
- Way = 2;
- }*/
-
- /*if(Way == 2){
- if(!MotorState[0]){
-
- StopTimer[0].attach(&MotorStop0,MOVE_TIME);
- Motor = Motor | 0x80;//10000000
- MotorState[0] = 1;
- Led[0] = 1;
- }else{
- Flag[0] = 1;
- }
- }else */
+void Photo1(){//one
+
if(Way == 1){
if(Flag[0]&&!FlagAdd[0]){
@@ -147,15 +162,15 @@
FlagAdd[0] = 0;
Led[0] = 0;
And[0] = 1;
+ Wait[0] = 1;
}
}
}
-void FallPhoto2(){//two
- //Wayの決定
+void Photo2(){//two
- if(Way == 1){
+ if((Way == 1)&&!Wait[1]){
if(!MotorState[1]){
StopTimer[2].attach(&MotorStop2,MOVE_TIME);
@@ -165,35 +180,11 @@
}else{
Flag[1] = 1;
}
- }/*else if(Way == 2){
-
- 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;
- Led[1] = 0;
- And[1] = 1;
- }
- } */
+ }
}
-void FallPhoto3(){ //three
- //Way
-
- /*if(Way == 2){
- if(!MotorState[1]){
-
- StopTimer[2].attach(&MotorStop2,MOVE_TIME);
- Motor = Motor | 0x08;
- MotorState[1] = 1;
- Led[1] = 1;
- }else{
- Flag[1] = 1;
- }
- }else */
+void Photo3(){ //three
+
if(Way == 1){
if(Flag[1]&&!FlagAdd[1]){
@@ -206,16 +197,16 @@
FlagAdd[1] = 0;
Led[1] = 0;
And[1] = 1;
-
+ Wait[1] = 1;
}
}
}
-void FallPhoto4(){//four
-
- if(Way ==0){
+void Photo4(){//four
+ if(Way == 0){
Way = 1;
- }
- if(Way == 1){
+ }
+
+ if((Way == 1)&&!Wait[2]){
if(!MotorState[2]){
StopTimer[4].attach(&MotorStop4,MOVE_TIME);
Motor = Motor | 0x20;//00100000
@@ -224,37 +215,11 @@
}else{
Flag[2] = 1;
}
- }/*else if(Way == 2){
-
- 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;
- Led[2] = 0;
- And[2] = 1;
-
- }
- } */
+ }
}
-void FallPhoto5(){//five
- /*if(Way ==0){
- Way = 2;
- }*/
-
- /*if(Way == 2){
- if(!MotorState[2]){
- StopTimer[4].attach(&MotorStop4,MOVE_TIME);
- Motor = Motor | 0x20;//00100000
- MotorState[2] = 1;
- Led[2] = 1;
- }else{
- Flag[2] = 1;
- }
- }else */
+void Photo5(){//five
+
if(Way == 1){
if(Flag[2]&&!FlagAdd[2]){
@@ -267,13 +232,14 @@
FlagAdd[2] = 0;
Led[2] = 0;
And[2] = 1;
+ Wait[2] = 1;
}
}
}
-void FallPhoto6(){//six
+void Photo6(){//six
- if(Way == 1){
+ if((Way == 1)&&!Wait[3]){
if(!MotorState[3]){
StopTimer[6].attach(&MotorStop6,MOVE_TIME);
Motor = Motor | 0x02;
@@ -282,34 +248,13 @@
}else{
Flag[3] = 1;
}
- }/*else if(Way == 2){
-
- 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;
- Led[3] = 0;
- And[3] = 1;
- }
- } */
+ }
+
}
-void FallPhoto7(){//seven
+void Photo7(){//seven
-
- /*if(Way == 2){
- if(!MotorState[3]){
- StopTimer[6].attach(&MotorStop6,MOVE_TIME);
- Motor = Motor | 0x02;
- MotorState[3] = 1;
- Led[3] = 1;
- }else{
- Flag[3] = 1;
- }
- }else */
+
if(Way == 1){
if(Flag[3]&&!FlagAdd[3]){
@@ -322,6 +267,7 @@
FlagAdd[3] = 0;
Led[3] = 0;
And[3] = 1;
+ Wait[3] = 1;
}
}
}
@@ -353,18 +299,108 @@
MotorState[i] = 0;
And[i] = 0;
}
+
Way = 0;
UpCt = 0;
+ TempraFlag = 1;
}
+
+void CheckPhoto(){
+
+ int i;
+ bool CkFlag[8] = {1};
+
+ Reset();
+ Tempra = 0;
+ TempraFlag = 0;
+
+ for (i = 0; i < 8;i++)
+ PhotoCk[i].mode(PullUp);
+ while(1){
+ if(!Sw[0]){
+ while(1){
+
+ if(!PhotoCk[0]&&CkFlag[0]){
+ Led[0] = 1;
+ CkFlag[0] = 0;
+ }else if(PhotoCk[0]){
+ CkFlag[0] = 1;
+ Led[0] = 0;
+ }
+
+ if(!PhotoCk[1]&&CkFlag[1]){
+ Led[1] = 1;
+ CkFlag[1] = 0;
+ }else if(PhotoCk[1]){
+ CkFlag[1] = 1;
+ Led[1] = 0;
+ }
+ if(!PhotoCk[2]&&CkFlag[2]){
+ Led[2] = 1;
+ CkFlag[2] = 0;
+ }else if(PhotoCk[2]){
+ CkFlag[2] = 1;
+ Led[2] = 0;
+ }
+ if(!PhotoCk[3]&&CkFlag[3]){
+ Led[3] = 1;
+ CkFlag[3] = 0;
+ }else if(PhotoCk[3]){
+ CkFlag[3] = 1;
+ Led[3] = 0;
+ }
+ if(!Sw[1]){
+ break;
+ }
+ }
+ }
+ if(!PhotoCk[4]&&CkFlag[4]){
+ Led[0] = 1;
+ CkFlag[4] = 0;
+ }else if(PhotoCk[4]){
+ CkFlag[4] = 1;
+ Led[0] = 0;
+ }
+
+ if(!PhotoCk[5]&&CkFlag[5]){
+ Led[1] = 1;
+ CkFlag[5] = 0;
+ }else if(PhotoCk[5]){
+ CkFlag[5] = 1;
+ Led[1] = 0;
+ }
+ if(!PhotoCk[6]&&CkFlag[6]){
+ Led[2] = 1;
+ CkFlag[6] = 0;
+ }else if(PhotoCk[6]){
+ CkFlag[6] = 1;
+ Led[2] = 0;
+ }
+ if(!PhotoCk[7]&&CkFlag[7]){
+ Led[3] = 1;
+ CkFlag[7] = 0;
+ }else if(PhotoCk[7]){
+ CkFlag[7] = 1;
+ Led[3] = 0;
+ }
+
+
+
+ }
+
+
+
+
+}
+
void NumCount(){//足を上げた回数を記録
UpCt ++;
And[0] = 0 ;
And[1] = 0 ;
And[2] = 0 ;
And[3] = 0 ;
- TempraFlag = 0;
if(UpCt > 3){
UpCt = 0;
Way = 0;
@@ -379,27 +415,13 @@
Pwm[2] = PWM_LEVEL ;
Pwm[3] = PWM_LEVEL ;
//for(i = 0 ; i < MOTOR_NUM ; i++ ){
- Photo[0].mode(PullUp);
- Photo[1].mode(PullUp);
- Photo[2].mode(PullUp);
- Photo[3].mode(PullUp);
- Photo[4].mode(PullUp);
- Photo[5].mode(PullUp);
- Photo[6].mode(PullUp);
- Photo[7].mode(PullUp);
+
+ Photo.mode(PullUp);
- //}
- Photo[0].fall(FallPhoto0);
- Photo[1].fall(FallPhoto1);
- Photo[2].fall(FallPhoto2);
- Photo[3].fall(FallPhoto3);
- Photo[4].fall(FallPhoto4);
- Photo[5].fall(FallPhoto5);
- Photo[6].fall(FallPhoto6);
- Photo[7].fall(FallPhoto7);
-
- Sw.mode(PullUp);
- Sw.fall(Reset);
+ Sw[0].mode(PullUp);
+ Sw[1].mode(PullUp);
+ //Sw.fall(Reset);
+ TempraTimer.reset();
}
@@ -408,20 +430,115 @@
SetUp();
wait(0.5);
+ uint8_t Ct = 0,nanika = 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;
while(1){
- if(MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3]){
- TempraFlag = 1;
+
+ //sensorの位置と足の位置は一致するとする．
+ //順番 右足前，後ろ 左足前，後ろ の順
+ nanika = ~Photo;
+ CkPhoto[0] = nanika & 0x03;
+ CkPhoto[1] = (nanika & 0x0C) >> 2;
+ CkPhoto[2] = (nanika & 0x30) >> 4;
+ CkPhoto[3] = (nanika & 0xC0) >> 6 ;
+
+ if(!CkPhoto[0]){
+ CkFlag[0] = 1;
+ CkFlag[1] = 1;
+
+ }else{
+ CkPhoto[0] -= 1;
+ if(CkFlag[CkPhoto[0]]){
+ (*MotorMove[CkPhoto[0]])();//要確認
+ CkFlag[CkPhoto[0]] = 0;
+ }
+
+ }
+ if(!CkPhoto[1]){
+ CkFlag[2] = 1;
+ CkFlag[3] = 1;
+
+ }else{
+ CkPhoto[1] += 1;
+ if(CkFlag[CkPhoto[1]]){
+ (*MotorMove[CkPhoto[1]])();//要確認
+ CkFlag[CkPhoto[1]] = 0;
+ }
+
+ }
+ if(!CkPhoto[2]){
+ CkFlag[4] = 1;
+ CkFlag[5] = 1;
+
+ }else{
+ CkPhoto[2] += 3;
+ if(CkFlag[CkPhoto[2]]){
+ (*MotorMove[CkPhoto[2]])();//要確認
+ CkFlag[CkPhoto[2]] = 0;
+ }
+
+ }
+ if(!CkPhoto[3]){
+ CkFlag[6] = 1;
+ CkFlag[7] = 1;
+
+ }else{
+ CkPhoto[3] += 5;
+ if(CkFlag[CkPhoto[3]]){
+ (*MotorMove[CkPhoto[3]])();//要確認
+ CkFlag[CkPhoto[3]] = 0;
+
+ }
+
+ }
+ //Ct = Photo;
+ //pc.printf("%d\n",Ct);
+
+ if((MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3])&&TempraFlag){
Tempra = 0x0A;
- }else if(TempraFlag){
StopTempra.attach(&TempraStop,TEMPRA_TIME);
- TempraFlag = 0;
}
-
+
+ if(!Sw[0]&&!Sw[1]){
+ Ct = 0;
+ while(!Sw[0]&&!Sw[1]&&(Ct <= 100)){
+ Ct++;
+ }
+ if(Ct >= 100){
+ CheckPhoto();
+ }
+ }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;
+ }
+ }
if(And[0]&&And[1]&&And[2]&&And[3]){
NumCount();
}
-
- //printf("%d\t%d\n",UpCt,Way);
}
-}
+}
\ No newline at end of file