Bチーム
足の上げ下げを行う 一つの足につき2つのセンサを使用、 RTOS使用、 NHKの足上げプログラム予定
Diff: main.cpp
- Revision:
- 0:0773a5499020
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Oct 12 00:43:25 2014 +0000
@@ -0,0 +1,629 @@
+/*
+ *覚え書き
+ *
+ ***mbedのピン配置に対するこのプログラムのピンの設定
+ *p5 ~ p12 Motor ...BusOut :Motor moved
+ *p13 ~ p20 Photo ...BusIn :Photo Sensor
+ *p21 ~ p24 Pwm ...PwmOut :Motor Pwm Signal
+ *p21 ~ p24 Sp ...DigitalOut :Check sensor state,do ditto led
+ *p25 ~ p28 Tempra...BusOut :Motor Tempra Moved
+ *p29,p30 Sw[2] ...DigitalIn :Switch Multi Use
+ *LED1 ~ LED4 Led ...DigitalOut :Led Brinked
+ *
+ **********************
+ ***関数の内容
+ *void WaitStop() :use wait
+ *void MotorStop() :erase motor pulses
+ *void MotorStart() :motor start timing timer
+ *void TempraStop() :stop motor of crawler
+ * //void Photo() :main processing ,wheel(lift or lower)motor move start
+ *void Reset() :wheels lower
+ *void GainSetting():sensor check mode,blinking Led
+ *void AndCount() :and count
+ *void SetUp() :first in function
+ *int main() :main function
+ *
+ *スレッド
+ *void Photo01_Thread(void const *argument)
+ *void Photo23_Thread(void const *argument)
+ *void Photo45_Thread(void const *argument)
+ *void Photo67_Thread(void const *argument) : main processing ,wheel(lift or lower)motor move start thread
+ *void ParallelThread(void const *argument) : Jump reset or GainSetting function
+ *
+ ***********************
+ ***処理内容
+ *photo系
+ *前のセンサで検知したら足をあげる、その後後ろのセンサが検知したら足を下げる
+ *前センサ→前センサ→後ろセンサ→後ろセンサ この場合足の上げ下げは一回しか行われない
+ *
+ *
+ *
+ **追記
+ *Photo {右前々、右前後、右後前、右後々、 左前々、左前後、左後前、左後々}
+ *
+ *
+ */
+
+
+#include "mbed.h"
+#include "rtos.h"
+
+#define MOTOR_NUM 8 //motor & sensor num
+#define START_TIME_B 0.237 //motor start wait time of Back //a lot
+#define START_TIME_F 0.04
+#define MOVE_TIME 0.10 //lift a wheel for 0.090 seconds
+#define MOVE_DOWN_TIME 0.115 //lower a wheel for 0.10 seconds
+#define TEMPRA_TIME 3.0 //move a crawler for 4.0 seconds
+#define WAIT_TIME 0.010 //make wait time
+#define PWM_LEVEL 1.0 //motors pwm setting level
+
+
+BusIn Photo(p5, p6, p7, p8, p9, p10, p11, p12 );
+DigitalIn Sw[2] = {p29, p30};
+
+DigitalOut Led[4] = {LED1, LED2, LED3, LED4 };
+DigitalOut Sp[4] = {p21, p22, p23, p24 };
+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 };
+
+Timeout StartTimer[4];
+Timeout StopTimer[8];
+Timeout StopTempra;
+Timeout StopWait[4];
+
+
+volatile bool MotorState[4] = {0} ,Flag[4] = {0} ,FlagAdd[4] = {0} ,And[4] = {0} ,Wait[4] = {0};
+volatile uint8_t Way = 0, DownCount = 0;
+volatile bool TempraFlag = 1;
+
+
+/*
+Motor = {
+ LeftFrontUp, LeftFrontDown,
+ RightFrontUp, RightFrontDown,
+ LeftBackUp, LeftBackDown,
+ RightBackUp, RightBackDown
+ };
+*/
+
+/***********************WaitStop*/
+void WaitStop0()
+{
+ Wait[0] = 0;
+}
+void WaitStop1()
+{
+ Wait[1] = 0;
+}
+void WaitStop2()
+{
+ Wait[2] = 0;
+}
+void WaitStop3()
+{
+ Wait[3] = 0;
+}
+/*
+********************************MotorStop
+*/
+void MotorStop0()
+{
+ Motor = Motor & 0x7F;
+}
+void MotorStop1()
+{
+ Motor = Motor & 0xBF;
+ StopWait[0].attach(&WaitStop0,WAIT_TIME);
+}
+void MotorStop2()
+{
+ Motor = Motor & 0xF7;
+}
+void MotorStop3()
+{
+ Motor = Motor & 0xFB;
+ StopWait[1].attach(&WaitStop1,WAIT_TIME);
+}
+void MotorStop4()
+{
+ Motor = Motor & 0xDF;
+}
+void MotorStop5()
+{
+ Motor = Motor & 0xEF;
+ StopWait[2].attach(&WaitStop2,WAIT_TIME);
+}
+void MotorStop6()
+{
+ Motor = Motor & 0xFD;
+}
+void MotorStop7()
+{
+ Motor = Motor & 0xFE;
+ StopWait[3].attach(&WaitStop3,WAIT_TIME);
+}
+
+/*
+***********************MotorStart
+*/
+void MotorStart0()
+{
+ StopTimer[0].attach(&MotorStop0,MOVE_TIME);
+ Motor = Motor & 0xBF;
+ Motor = Motor | 0x80;//10000000
+
+}
+void MotorStart1()
+{
+ StopTimer[2].attach(&MotorStop2,MOVE_TIME);
+ Motor = Motor & 0xFB;
+ Motor = Motor | 0x08;//00001000
+}
+void MotorStart2()
+{
+ StopTimer[4].attach(&MotorStop4,MOVE_TIME);
+ Motor = Motor & 0xEF;
+ Motor = Motor | 0x20;//00100000
+
+}
+void MotorStart3()
+{
+ StopTimer[6].attach(&MotorStop6,MOVE_TIME);
+ Motor = Motor & 0xFE;
+ Motor = Motor | 0x02;//00000010
+}
+
+
+void TempraStop()
+{
+ Tempra = 0x00;
+}
+
+
+
+void Photo01_Thread(void const *argument)
+{
+ uint8_t CkPhoto = 0;
+ bool CkFlag[2] = {1,1};
+
+ while(1) {
+ CkPhoto = (~Photo) & 0x03;
+ if(CkPhoto==0) { // 右前足
+ CkFlag[0] = 1;
+ CkFlag[1] = 1;
+ } else if(CkPhoto<3) {
+ CkPhoto -= 1;
+ if(CkFlag[CkPhoto]) {
+ if(CkPhoto == 0) {
+ if(Way ==0){
+ Way = 1;
+ }
+ if((Way == 1)&&!Wait[0]) {
+ if(!MotorState[0]) {
+ StartTimer[0].attach(&MotorStart0,START_TIME_F);
+ MotorState[0] = 1;
+ Led[0] = 1;
+ Sp [0] = 1;
+ } else {
+ Flag[0] = 1;
+ }
+ }
+
+ } else {
+ if(Way == 1) {
+ if(Flag[0]&&!FlagAdd[0]) {
+ FlagAdd[0] = 1;
+ } else if(MotorState[0]) {
+ StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
+ Motor = Motor & 0x7F;// 01111111
+ Motor = Motor | 0x40;//0x40 = 01000000
+ MotorState[0] = 0;
+ Flag[0] = 0;
+ FlagAdd[0] = 0;
+ Led[0] = 0;
+ Sp [0] = 0;
+ And[0] = 1;
+ Wait[0] = 1;
+ }
+ }
+
+ }
+ CkFlag[CkPhoto] = 0;
+ }
+ }
+ }
+
+}
+void Photo23_Thread(void const *argument)
+{
+ uint8_t CkPhoto = 0;
+ bool CkFlag[2] = {1,1};
+
+ while(1) {
+ CkPhoto = ((~Photo) & 0x0C) >> 2;
+ if(CkPhoto==0) { // 右前足
+ CkFlag[0] = 1;
+ CkFlag[1] = 1;
+ } else if(CkPhoto<3) {
+ CkPhoto -= 1;
+ if(CkFlag[CkPhoto]) {
+ if(CkPhoto == 0) {
+ if((Way == 1)&&!Wait[1]) {
+ if(!MotorState[1]) {
+ StartTimer[1].attach(&MotorStart1,START_TIME_B);
+ MotorState[1] = 1;
+ Led[1] = 1;
+ Sp [1] = 1;
+ } else {
+ Flag[1] = 1;
+ }
+ }
+
+ } else {
+ if(Way == 1) {
+ if(Flag[1]&&!FlagAdd[1]) {
+ FlagAdd[1] = 1;
+ } else if(MotorState[1]) {
+ StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
+ Motor = Motor & 0xF7;
+ Motor = Motor | 0x04;//00000100
+ MotorState[1] = 0;
+ Flag[1] = 0;
+ FlagAdd[1] = 0;
+ Led[1] = 0;
+ Sp [1] = 0;
+ And[1] = 1;
+ Wait[1] = 1;
+ }
+ }
+
+ }
+ CkFlag[CkPhoto] = 0;
+ }
+ }
+ }
+
+}
+void Photo45_Thread(void const *argument)
+{
+ uint8_t CkPhoto = 0;
+ bool CkFlag[2] = {1,1};
+
+ while(1) {
+ CkPhoto = ((~Photo) & 0x30) >> 4;
+ if(CkPhoto==0) { // 右前足
+ CkFlag[0] = 1;
+ CkFlag[1] = 1;
+ } else if(CkPhoto<3) {
+ CkPhoto -= 1;
+ if(CkFlag[CkPhoto]) {
+ if(CkPhoto == 0) {
+ if(Way == 0) {
+ Way = 1;
+ }
+ if((Way == 1)&&!Wait[2]) {
+ if(!MotorState[2]) {
+ StartTimer[2].attach(&MotorStart2,START_TIME_F);
+ MotorState[2] = 1;
+ Led[2] = 1;
+ Sp [2] = 1;
+ } else {
+ Flag[2] = 1;
+ }
+ }
+ } else {
+ if(Way == 1) {
+ if(Flag[2]&&!FlagAdd[2]) {
+ FlagAdd[2] = 1;
+ } else if(MotorState[2]) {
+ StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
+ Motor = Motor & 0xDF;
+ Motor = Motor | 0x10;//00010000
+ MotorState[2] = 0;
+ Flag[2] = 0;
+ FlagAdd[2] = 0;
+ Led[2] = 0;
+ Sp [2] = 0;
+ And[2] = 1;
+ Wait[2] = 1;
+ }
+ }
+
+ }
+ CkFlag[CkPhoto] = 0;
+ }
+ }
+ }
+
+}
+void Photo67_Thread(void const *argument)
+{
+ uint8_t CkPhoto = 0;
+ bool CkFlag[2] = {1,1};
+
+ while(1) {
+ CkPhoto = ((~Photo) & 0xC0) >> 6;
+ if(CkPhoto==0) { // 右前足
+ CkFlag[0] = 1;
+ CkFlag[1] = 1;
+ } else if(CkPhoto<3) {
+ CkPhoto -= 1;
+ if(CkFlag[CkPhoto]) {
+ if(CkPhoto == 0) {
+ if((Way == 1)&&!Wait[3]) {
+ if(!MotorState[3]) {
+ StartTimer[3].attach(&MotorStart3,START_TIME_B);
+ MotorState[3] = 1;
+ Led[3] = 1;
+ Sp [3] = 1;
+ } else {
+ Flag[3] = 1;
+ }
+ }
+
+ } else {
+ if(Way == 1) {
+ if(Flag[3]&&!FlagAdd[3]) {
+ FlagAdd[3] = 1;
+ } else if(MotorState[3]) {
+ StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
+ Motor = Motor & 0xFD;//11111101
+ Motor = Motor | 0x01;//00000001
+ MotorState[3] = 0;
+ Flag[3] = 0;
+ FlagAdd[3] = 0;
+ Led[3] = 0;
+ Sp [3] = 0;
+ And[3] = 1;
+ Wait[3] = 1;
+ }
+ }
+
+ }
+ CkFlag[CkPhoto] = 0;
+ }
+ }
+ }
+
+}
+
+
+void Reset()
+{
+ int i;
+
+ if(MotorState[0]) {
+ StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
+ Motor = Motor | 0x40;//01000000
+ }
+ if(MotorState[1]) {
+ StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
+ Motor = Motor | 0x04;//00000100
+ }
+ if(MotorState[2]) {
+ StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
+ Motor = Motor | 0x10;//00010000
+ }
+ if(MotorState[3]) {
+ StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
+ Motor = Motor | 0x01;//00000001
+ }
+ for(i = 0; i < 4; i++) {
+ Led[i] = 0;
+ Flag[i] = 0;
+ FlagAdd[i] = 0;
+ MotorState[i] = 0;
+ And[i] = 0;
+ }
+
+ Way = 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;
+ Sp[0] = Sp[1] = Sp[2] = Sp[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;
+ Sp [0] = 0;
+ CkFlag[0] = 0;
+ } else if(!PhotoCk[0]) {
+ CkFlag[0] = 1;
+ Led[0] = 1;
+ Sp [0] = 1;
+ }
+
+ if(PhotoCk[1]&&CkFlag[1]) {
+ Led[1] = 0;
+ Sp [1] = 0;
+ CkFlag[1] = 0;
+ } else if(!PhotoCk[1]) {
+ CkFlag[1] = 1;
+ Led[1] = 1;
+ Sp [1] = 1;
+ }
+ if(PhotoCk[2]&&CkFlag[2]) {
+ Led[2] = 0;
+ Sp [2] = 0;
+ CkFlag[2] = 0;
+ } else if(!PhotoCk[2]) {
+ CkFlag[2] = 1;
+ Led[2] = 1;
+ Sp [2] = 1;
+ }
+ if(PhotoCk[3]&&CkFlag[3]) {
+ Led[3] = 0;
+ Sp [3] = 0;
+ CkFlag[3] = 0;
+ } else if(!PhotoCk[3]) {
+ CkFlag[3] = 1;
+ Led[3] = 1;
+ Sp [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;
+ Sp [0] = 0;
+ CkFlag[4] = 0;
+ } else if(!PhotoCk[4]) {
+ CkFlag[4] = 1;
+ Led[0] = 1;
+ Sp [0] = 1;
+ }
+ if(PhotoCk[5]&&CkFlag[5]) {
+ Led[1] = 0;
+ Sp [1] = 0;
+ CkFlag[5] = 0;
+ } else if(!PhotoCk[5]) {
+ CkFlag[5] = 1;
+ Led[1] = 1;
+ Sp [1] = 1;
+ }
+ if(PhotoCk[6]&&CkFlag[6]) {
+ Led[2] = 0;
+ Sp [2] = 0;
+ CkFlag[6] = 0;
+ } else if(!PhotoCk[6]) {
+ CkFlag[6] = 1;
+ Led[2] = 1;
+ Sp [2] = 1;
+ }
+ if(PhotoCk[7]&&CkFlag[7]) {
+ Led[3] = 0;
+ Sp [3] = 0;
+ CkFlag[7] = 0;
+ } else if(!PhotoCk[7]) {
+ CkFlag[7] = 1;
+ Led[3] = 1;
+ Sp [3] = 1;
+ }
+
+ }
+
+}
+
+
+void AndCount() //"Wheels" Up and Down counting
+{
+
+ DownCount ++;
+ And[0] = 0 ;
+ And[1] = 0 ;
+ And[2] = 0 ;
+ And[3] = 0 ;
+ if(DownCount > 3) {
+ DownCount = 0;
+ Way = 0;
+ }
+}
+
+/****************************Reset or Crawler_stop or CheckMode_Start Thread
+*/
+void ParallelThread(void const *argument)
+{
+ uint8_t Ct;
+ while (1) {
+
+
+ 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]) {
+ Ct = 0;
+ while(!Sw[0]&&!Sw[1]&&(Ct <= 100)) {
+ Ct++;
+ }
+ if(Ct >= 100) {
+ GainSetting();
+ }
+
+ } else if(!Sw[0]) {
+ Ct = 0;
+ while(!Sw[0]&&(Ct <= 100)) {
+ Ct ++;
+ }
+ if(Ct >= 100) {
+ Reset();
+ }
+
+ } else if(!Sw[1]) {
+ Ct = 0;
+ while(!Sw[1]&&(Ct <= 100)) {
+ Ct ++;
+ }
+ if(Ct >= 100) {
+ Tempra = 0;
+ TempraFlag = 0;
+ }
+
+
+ }
+ }
+}
+
+
+void SetUp()
+
+{
+/*
+ Pwm[0] = PWM_LEVEL ;
+ Pwm[1] = PWM_LEVEL ;
+ Pwm[2] = PWM_LEVEL ;
+ Pwm[3] = PWM_LEVEL ;
+*/
+ Sp[0] = Sp[1] = Sp[2] = Sp[3] = 0;
+ Photo.mode(PullUp);
+ Sw[0].mode(PullUp);
+ Sw[1].mode(PullUp);
+
+}
+int main()
+{
+ SetUp();
+
+ Thread thread(ParallelThread);
+ Thread thread0(Photo01_Thread);
+ Thread thread1(Photo23_Thread);
+ Thread thread2(Photo45_Thread);
+ Thread thread3(Photo67_Thread);
+ wait(0.5);
+
+
+ while(1) {
+
+ }
+}
+