Bチーム
足上げセンサ八個版仮
Dependencies: mbed
Fork of
4LegUpDown_8sense
by 
Bチーム
Diff: main.cpp
- Revision:
- 9:4b15d9a3e468
- Parent:
- 8:36a85b66796f
- Child:
- 10:87c6b540b3b3
--- a/main.cpp Wed Sep 17 08:30:11 2014 +0000
+++ b/main.cpp Fri Sep 19 05:01:36 2014 +0000
@@ -2,7 +2,17 @@
*覚え書き
*
*mbedのピン配置に対するこのプログラムのピンの利用
- *p5~p12
+ *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 :Swich Multi Use
+ *LED1 ~ LED4 Led ...DigitalOut :Led Brinked
+ *
+ *
+ *@file
+ *
+ *My program.
*
*
*
@@ -11,25 +21,24 @@
#include "mbed.h"
-#define MOTOR_NUM 8
-#define MOVE_TIME 0.080
-#define MOVE_DOWN_TIME 0.090
+
+#define MOTOR_NUM 8//モーター，センサーの数
+#define MOVE_TIME 0.090//足を上げる時間
+#define MOVE_DOWN_TIME 0.10//足を下げる時間
#define TEMPRA_TIME 4.0 //ワンショットタイマー用時間設定
-#define CHECK_TIME 5.0 //時間経過でストップするタイマー用時間設定
-#define WAIT_TIME 0.010
-#define PWM_LEVEL 1.0
+#define WAIT_TIME 0.010//上げ下げの待ち時間
+#define PWM_LEVEL 1.0//モーターの回転速度のPWM
+
Serial pc(USBTX,USBRX);
-
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 }
BusOut Tempra(p25,p26,p27,p28);
-PwmOut Pwm[4] = {p21, p22, p23, p24 };
+PwmOut Pwm[4] = {p21, p22, p23, p24 };
Timeout StopTimer[8];
Timeout StopTempra;
@@ -39,122 +48,103 @@
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 uint8_t Way = 0, DownCount = 0;
volatile bool TempraFlag = 1;
-/*motorstate 上がっているか下がっているか
-
-+
-*/
/*
Motor = {
- LeftFrontUp, LeftFrontDown,
+ LeftFrontUp, LeftFrontDown,
RightFrontUp, RightFrontDown,
- LeftBackUp, LeftBackDown,
- RightBackUp, RightBackDown
+ LeftBackUp, LeftBackDown,
+ RightBackUp, RightBackDown
};
*/
-void WaitStop0(){//atatch Time Stop0
-//make delay time
+void WaitStop0()
+{
Wait[0] = 0;
-
}
-
-void WaitStop1(){//atatch Time Stop0
-//make delay time
+void WaitStop1()
+{
Wait[1] = 0;
-
}
-
-void WaitStop2(){//atatch Time Stop0
-//make delay time
+void WaitStop2()
+{
Wait[2] = 0;
-
}
-
-void WaitStop3(){//atatch Time Stop0
-//make delay time
+void WaitStop3()
+{
Wait[3] = 0;
-
+}
+void MotorStop0()
+{
+ Motor = Motor & 0x7F;
}
-void MotorStop0(){
-
- Motor = Motor & 0x7F;
-
-}
-void MotorStop1(){
+void MotorStop1()
+{
Motor = Motor & 0xBF;
-
+ StopWait[0].attach(&WaitStop0,WAIT_TIME);
}
-
-void MotorStop2(){
-
+void MotorStop2()
+{
Motor = Motor & 0xF7;
-
}
-void MotorStop3(){
-
+void MotorStop3()
+{
Motor = Motor & 0xFB;
StopWait[1].attach(&WaitStop1,WAIT_TIME);
-
}
-void MotorStop4(){
-
+void MotorStop4()
+{
Motor = Motor & 0xDF;
-
}
-void MotorStop5(){
-
- Motor = Motor & 0xEF;
+void MotorStop5()
+{
+ Motor = Motor & 0xEF;
StopWait[2].attach(&WaitStop2,WAIT_TIME);
-
}
-void MotorStop6(){
-
+void MotorStop6()
+{
Motor = Motor & 0xFD;
-
}
-void MotorStop7(){
-
+void MotorStop7()
+{
Motor = Motor & 0xFE;
StopWait[3].attach(&WaitStop3,WAIT_TIME);
-
}
-void TempraStop(){
- Tempra = 0x00;
-
+
+
+void TempraStop()
+{
+ Tempra = 0x00;
}
-void Photo0(){//zero
- //Wayの決定
- if(Way == 0){
+
+void Photo0() //zero Up
+{
+ if(Way == 0) {
Way = 1;
}
-
- if((Way == 1)&&!Wait[0]){
-
- if(!MotorState[0]){
+ if((Way == 1)&&!Wait[0]) {
+ if(!MotorState[0]) {
StopTimer[0].attach(&MotorStop0,MOVE_TIME);
- Motor = Motor | 0x80;//10000000
- MotorState[0] = 1;
+ Motor = Motor | 0x80;//10000000
+ MotorState[0] = 1;
Led[0]= 1;
- }else{
+ } else {
Flag[0] = 1;
}
}
}
-
-void Photo1(){//one
-
- if(Way == 1){
-
- if(Flag[0]&&!FlagAdd[0]){
+void Photo1() //one Down
+{
+ if(Way == 1) {
+ if(Flag[0]&&!FlagAdd[0]) {
FlagAdd[0] = 1;
- }else if(MotorState[0]) {
+ } else if(MotorState[0]) {
StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
Motor = Motor | 0x40;//0x40 = 01000000
MotorState[0] = 0;
@@ -163,33 +153,28 @@
Led[0] = 0;
And[0] = 1;
Wait[0] = 1;
-
}
}
-
}
-void Photo2(){//two
-
- if((Way == 1)&&!Wait[1]){
- if(!MotorState[1]){
-
+void Photo2() //two Up
+{
+ if((Way == 1)&&!Wait[1]) {
+ if(!MotorState[1]) {
StopTimer[2].attach(&MotorStop2,MOVE_TIME);
- Motor = Motor | 0x08;
- MotorState[1] = 1;
+ Motor = Motor | 0x08;
+ MotorState[1] = 1;
Led[1] = 1;
- }else{
+ } else {
Flag[1] = 1;
}
}
-
}
-void Photo3(){ //three
-
- if(Way == 1){
-
- if(Flag[1]&&!FlagAdd[1]){
+void Photo3() //three Down
+{
+ if(Way == 1) {
+ if(Flag[1]&&!FlagAdd[1]) {
FlagAdd[1] = 1;
- }else if(MotorState[1]){
+ } else if(MotorState[1]) {
StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
Motor = Motor | 0x04;
MotorState[1] = 0;
@@ -199,32 +184,30 @@
And[1] = 1;
Wait[1] = 1;
}
- }
+ }
}
-void Photo4(){//four
- if(Way == 0){
+void Photo4() //four Up
+{
+ if(Way == 0) {
Way = 1;
- }
-
- if((Way == 1)&&!Wait[2]){
- if(!MotorState[2]){
+ }
+ if((Way == 1)&&!Wait[2]) {
+ if(!MotorState[2]) {
StopTimer[4].attach(&MotorStop4,MOVE_TIME);
- Motor = Motor | 0x20;//00100000
- MotorState[2] = 1;
+ Motor = Motor | 0x20;//00100000
+ MotorState[2] = 1;
Led[2] = 1;
- }else{
+ } else {
Flag[2] = 1;
}
}
-
}
-void Photo5(){//five
-
- if(Way == 1){
-
- if(Flag[2]&&!FlagAdd[2]){
+void Photo5() //five Down
+{
+ if(Way == 1) {
+ if(Flag[2]&&!FlagAdd[2]) {
FlagAdd[2] = 1;
- }else if(MotorState[2]){
+ } else if(MotorState[2]) {
StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
Motor = Motor | 0x10;
MotorState[2] = 0;
@@ -234,32 +217,27 @@
And[2] = 1;
Wait[2] = 1;
}
- }
+ }
}
-void Photo6(){//six
-
-
- if((Way == 1)&&!Wait[3]){
- if(!MotorState[3]){
+void Photo6() //six Up
+{
+ if((Way == 1)&&!Wait[3]) {
+ if(!MotorState[3]) {
StopTimer[6].attach(&MotorStop6,MOVE_TIME);
- Motor = Motor | 0x02;
+ Motor = Motor | 0x02;
MotorState[3] = 1;
- Led[3] = 1;
- }else{
+ Led[3] = 1;
+ } else {
Flag[3] = 1;
}
}
-
}
-void Photo7(){//seven
-
-
-
- if(Way == 1){
-
- if(Flag[3]&&!FlagAdd[3]){
+void Photo7() //seven Down
+{
+ if(Way == 1) {
+ if(Flag[3]&&!FlagAdd[3]) {
FlagAdd[3] = 1;
- }else if(MotorState[3]){
+ } else if(MotorState[3]) {
StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
Motor = Motor | 0x01;
MotorState[3] = 0;
@@ -269,170 +247,169 @@
And[3] = 1;
Wait[3] = 1;
}
- }
+ }
}
-void Reset(){
-
+
+void Reset()
+{
int i;
- if(MotorState[0]){
+ if(MotorState[0]) {
StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
Motor = Motor | 0x40;//
}
- if(MotorState[1]){
+ if(MotorState[1]) {
StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
Motor = Motor | 0x04;//00000100
}
- if(MotorState[2]){
+ if(MotorState[2]) {
StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
Motor = Motor | 0x10;//00010000
}
- if(MotorState[3]){
+ if(MotorState[3]) {
StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
- Motor = Motor | 0x01;//00000001
+ Motor = Motor | 0x01;//00000001
}
- for(i = 0; i < 4; i++){
+ for(i = 0; i < 4; i++) {
Led[i] = 0;
- Flag[i] = 0;
+ Flag[i] = 0;
FlagAdd[i] = 0;
MotorState[i] = 0;
And[i] = 0;
}
-
+
Way = 0;
- UpCt = 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 CheckPhoto(){
-
- int i;
- bool CkFlag[8] = {1};
-
- Reset();
- Tempra = 0;
- TempraFlag = 0;
+void AndCount() //"Legs" Up and Down counting
+{
- 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 ++;
+ DownCount ++;
And[0] = 0 ;
And[1] = 0 ;
And[2] = 0 ;
And[3] = 0 ;
- if(UpCt > 3){
- UpCt = 0;
+ if(DownCount > 3) {
+ DownCount = 0;
Way = 0;
}
}
-void SetUp(){
- //int i;
-
+void SetUp()
+{
+
Pwm[0] = PWM_LEVEL ;
Pwm[1] = PWM_LEVEL ;
Pwm[2] = PWM_LEVEL ;
Pwm[3] = PWM_LEVEL ;
- //for(i = 0 ; i < MOTOR_NUM ; i++ ){
-
Photo.mode(PullUp);
-
- Sw[0].mode(PullUp);
+ Sw[0].mode(PullUp);
Sw[1].mode(PullUp);
- //Sw.fall(Reset);
+
TempraTimer.reset();
-
-
}
-int main() {
-
- SetUp();
- wait(0.5);
- uint8_t Ct = 0,nanika = 0,CkPhoto[4] = {0};
+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;
@@ -441,104 +418,99 @@
MotorMove[5] = Photo5;
MotorMove[6] = Photo6;
MotorMove[7] = Photo7;
-
- while(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]){
+
+ 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) { //@remarks 右前足
CkFlag[0] = 1;
CkFlag[1] = 1;
-
- }else{
+ } else if(CkPhoto[0]<3) {
CkPhoto[0] -= 1;
- if(CkFlag[CkPhoto[0]]){
- (*MotorMove[CkPhoto[0]])();//要確認
- CkFlag[CkPhoto[0]] = 0;
+ if(CkFlag[CkPhoto[0]]) {
+ (*MotorMove[CkPhoto[0]])();
+ CkFlag[CkPhoto[0]] = 0;
}
-
}
- if(!CkPhoto[1]){
+
+ if(CkPhoto[1]==0) { //@remarks 右後足
CkFlag[2] = 1;
CkFlag[3] = 1;
-
- }else{
+ } else if(CkPhoto[1]<3) {
CkPhoto[1] += 1;
- if(CkFlag[CkPhoto[1]]){
- (*MotorMove[CkPhoto[1]])();//要確認
- CkFlag[CkPhoto[1]] = 0;
+ if(CkFlag[CkPhoto[1]]) {
+ (*MotorMove[CkPhoto[1]])();
+ CkFlag[CkPhoto[1]] = 0;
}
-
}
- if(!CkPhoto[2]){
+
+ if(CkPhoto[2]==0) { //@remarks 左前足
CkFlag[4] = 1;
CkFlag[5] = 1;
-
- }else{
+ } else if(CkPhoto[2]<3) {
CkPhoto[2] += 3;
- if(CkFlag[CkPhoto[2]]){
- (*MotorMove[CkPhoto[2]])();//要確認
- CkFlag[CkPhoto[2]] = 0;
+ if(CkFlag[CkPhoto[2]]) {
+ (*MotorMove[CkPhoto[2]])();
+ CkFlag[CkPhoto[2]] = 0;
}
-
}
- if(!CkPhoto[3]){
+
+ if(CkPhoto[3]==0) { //@remarks 左後足
CkFlag[6] = 1;
CkFlag[7] = 1;
-
- }else{
+ } else if(CkPhoto[3]<3) {
CkPhoto[3] += 5;
- if(CkFlag[CkPhoto[3]]){
- (*MotorMove[CkPhoto[3]])();//要確認
- CkFlag[CkPhoto[3]] = 0;
-
+ 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){
+ 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]){
+
+ if(!Sw[0]&&!Sw[1]) {
Ct = 0;
- while(!Sw[0]&&!Sw[1]&&(Ct <= 100)){
+ while(!Sw[0]&&!Sw[1]&&(Ct <= 100)) {
Ct++;
}
- if(Ct >= 100){
- CheckPhoto();
+ if(Ct >= 100) {
+ GainSetting();
}
- }else if(!Sw[0]){
+
+ } else if(!Sw[0]) {
Ct = 0;
- while(!Sw[0]&&(Ct <= 100)){
+ while(!Sw[0]&&(Ct <= 100)) {
Ct ++;
//printf("%d\n",Ct);
}
- if(Ct >= 100){
- Reset();
+ if(Ct >= 100) {
+ Reset();
}
- }else if(!Sw[1]){
+
+ } else if(!Sw[1]) {
Ct = 0;
- while(!Sw[1]&&(Ct <= 100)){
+ while(!Sw[1]&&(Ct <= 100)) {
Ct ++;
//printf("%d\n",Ct);
}
- if(Ct >= 100){
+ if(Ct >= 100) {
Tempra = 0;
- TempraFlag = 0;
+ TempraFlag = 0;
}
}
- if(And[0]&&And[1]&&And[2]&&And[3]){
- NumCount();
- }
}
}
\ No newline at end of file