Bチーム
足上げセンサ八個版仮
Dependencies: mbed
Fork of
4LegUpDown_8sense
by 
Bチーム
Diff: main.cpp
- Revision:
- 3:cd7d3613e8e4
- Parent:
- 2:89cf1003e119
- Child:
- 4:04f2479b83c1
--- a/main.cpp Mon Aug 25 05:29:35 2014 +0000
+++ b/main.cpp Wed Sep 10 11:07:44 2014 +0000
@@ -1,44 +1,38 @@
#include "mbed.h"
-#define MOVE_TIME 0.11
-#define WAIT_TIME 0.02
-#define PWM_LEVEL 1.0// 11.1 / 7.4
-
-/*
-センサの数を八個で足一つに付き二つのセンサで足を上下させるプログラム．
-
-はじめのリミットスイッチが押されてもう一度押されるまでの時間を計り現在の速さを概算してタイマー制御に役立てる
-
-pin rise/fall attach use
-*//**/
-
+#define MOTOR_NUM 8
+#define MOVE_TIME 0.060
+#define MOVE_DOWN_TIME 0.070
+#define TEMPRA_TIME 1.0
+#define PWM_LEVEL 1.0
Serial pc(USBTX,USBRX);
-Timeout _toMotor[8];
-Timeout _toWait[4];
+InterruptIn Photo[8] = {p5,p6,p7,p8,p9,p10,p11,p12};
+InterruptIn Sw(p28);
+
-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 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(p29,p30);
+PwmOut Pwm[4] = {p21, p22, p23, p24 };
+PwmOut TempraPwm(p22);
-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
-PwmOut _poPwm[4] = {p21, p22, p23, p24 };
+Timeout StopTimer[8];
+Timeout StopTempra;
-/**more use binary number
-0xA0 = 10100000
-0x50 = 01010000
-0x0A = 00001010
-0x05 = 00000101
-**/
+
+volatile bool MotorState[4] = {0},Flag[4] = {0},FlagAdd[4] = {0},And[4] = {0};
+volatile uint8_t Way = 0,UpCt = 0;
-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}[]
-
+/*motorstate 上がっているか下がっている
+ flag 割込みに入ったか入ってないか←もしかしたらmotorstateと同じ可能性あり
+ flag_add入った回数の追加
+ 足が上がった時向き決定
+ 四回目の足下げで回数リセット
-enum {LFU = 0,LFD,RFU,RFD,LBU,LBD,RBU,RBD};
+*/
+
/*
bMotor = {
LeftFrontUp, LeftFrontDown,
@@ -48,676 +42,359 @@
};
*/
-void Motor_state(){
-/*
-uint8_t GetBit(uint8_t n, uint8_t bit)
- return (n >>(bit-1))%2;
-}by takamastu
-*/
- uint8_t i;
+
+
+void MotorStop0(){
+
+ Motor = Motor & 0x7F;
+
+}
+void MotorStop1(){
+
+ Motor = Motor & 0xBF;
+
+}
+
+void MotorStop2(){
+
+ Motor = Motor & 0xF7;
+
+}
+void MotorStop3(){
+
+ Motor = Motor & 0xFB;
+
+}
+void MotorStop4(){
+
+ Motor = Motor & 0xDF;
+
+}
+void MotorStop5(){
+
+ Motor = Motor & 0xEF;
+
+}
+void MotorStop6(){
+
+ Motor = Motor & 0xFD;
+
+}
+
+void MotorStop7(){
+
+ Motor = Motor & 0xFE;
+
+}
+void TempraStop(){
+ Motor = Tempra & 0x02;
+
+}
+void FallPhoto0(){//zero
+ //Wayの決定
+ if(Way == 0){
+ Way = 1;
+ }
+
+ if(Way == 1){
+ if(!MotorState[0]){
+
+ StopTimer[0].attach(&MotorStop0,MOVE_TIME);
+ Motor = Motor | 0x80;//10000000
+ MotorState[0] = 1;
+ Led[0]= 1;
+ }else{
+ Flag[0] = 1;
+ }
+ }else if(Way == 2){
+
+ if(Flag[0]&&!FlagAdd[0]){
+ FlagAdd[0] = 1;
+ }else{
+ 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 if(Way == 1){
+
+ if(Flag[0]&&!FlagAdd[0]){
+ FlagAdd[0] = 1;
+ }else{
+ 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 FallPhoto2(){//two
+ //Wayの決定
+
+ if(Way == 1){
+ if(!MotorState[1]){
+
+ StopTimer[2].attach(&MotorStop2,MOVE_TIME);
+ Motor = Motor | 0x08;
+ MotorState[1] = 1;
+ Led[1] = 1;
+ }else{
+ Flag[1] = 1;
+ }
+ }else if(Way == 2){
+
+ if(Flag[1]&&!FlagAdd[1]){
+ FlagAdd[1] = 1;
+ }else{
+ 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
- for (i = 0; i < 8 ; i++){
- bMotor[i] = ( _boMotor >> 7-i )%2;
+ 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 if(Way == 1){
+
+ if(Flag[1]&&!FlagAdd[1]){
+ FlagAdd[1] = 1;
+ }else{
+ StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
+ Motor = Motor | 0x04;
+ MotorState[1] = 0;
+ Flag[1] = 0;
+ Led[1] = 0;
+ And[1] = 1;
+
+ }
+ }
+}
+void FallPhoto4(){//four
+
+ if(Way ==0){
+ Way = 1;
+ }
+ if(Way == 1){
+ if(!MotorState[2]){
+ StopTimer[4].attach(&MotorStop4,MOVE_TIME);
+ Motor = Motor | 0x20;//00100000
+ MotorState[2] = 1;
+ Led[2] = 1;
+ }else{
+ Flag[2] = 1;
+ }
+ }else if(Way == 2){
+
+ if(Flag[2]&&!FlagAdd[2]){
+ FlagAdd[2] = 1;
+ }else{
+ 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 if(Way == 1){
+
+ if(Flag[2]&&!FlagAdd[2]){
+ FlagAdd[2] = 1;
+ }else{
+ StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
+ Motor = Motor | 0x10;
+ MotorState[2] = 0;
+ Flag[2] = 0;
+ Led[2] = 0;
+ And[2] = 1;
+ }
+ }
+}
+void FallPhoto6(){//six
+
+
+ if(Way == 1){
+ 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 == 2){
+
+ if(Flag[3]&&!FlagAdd[3]){
+ FlagAdd[3] = 1;
+ }else{
+ 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
+
+
+
+ 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]){
+ FlagAdd[3] = 1;
+ }else{
+ StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
+ Motor = Motor | 0x01;
+ MotorState[3] = 0;
+ Flag[3] = 0;
+ Led[3] = 0;
+ And[3] = 1;
+ }
+ }
+}
+
+void Reset(){
+
+ int i;
+
+ if(MotorState[0]){
+ StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
+ Motor = Motor | 0x40;//
+ }
+ 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++){
+ Flag[i] = 0;
+ FlagAdd[i] = 0;
+ MotorState[i] = 0;
}
}
-void _atWaitStop0(){//atatch Time Stop0
-//make delay time
- bWait[0] = 0;
-
-}
-
-void _atWaitStop1(){//atatch Time Stop0
-//make delay time
- bWait[1] = 0;
-
-}
-
-void _atWaitStop2(){//atatch Time Stop0
-//make delay time
- bWait[2] = 0;
-
-}
-
-void _atWaitStop3(){//atatch Time Stop0
-//make delay time
- bWait[3] = 0;
-
-}
-
-
+void NumCount(){//足を上げた回数を記録
-void _atMotorStop0(){//atatch Motor Stop0
-
- _boMotor = _boMotor & 0x7F;
- //01111111 指定ピンのみ変更
- //left front leg up
- _doLed[0] = 0;
- Motor_state();
-
-}
-
-
-void _atMotorStop1(){
- _toWait[0].attach(&_atWaitStop0,WAIT_TIME);
- _boMotor = _boMotor & 0xBF;
- //10111111 指定ピンのみ変更
- //left front leg down
- _doLed[0] = 0;
- Motor_state();
-
-}
-
-void _atMotorStop2(){
-
- _boMotor = _boMotor & 0xF7;
- //11110111 指定ピンのみ変更
- //left back leg up
- _doLed[1] = 0;
- Motor_state();
-
-}
-
-void _atMotorStop3(){
- _toWait[1].attach(&_atWaitStop1,WAIT_TIME);
- _boMotor = _boMotor & 0xFB;
- //11111011 指定ピンのみ変更
- //left back leg down
- _doLed[1] = 0;
- Motor_state();
+ bool AndCk = And[0]&And[1]&And[2]&And[3];
-}
-void _atMotorStop4(){//atatch Motor Stop0
-
- _boMotor = _boMotor & 0xDF;
- //11011111 指定ピンのみ変更
- //right front leg up
- _doLed[2] = 0;
- Motor_state();
-
-}
-
-
-void _atMotorStop5(){
- _toWait[2].attach(&_atWaitStop2,WAIT_TIME);
- _boMotor = _boMotor & 0xEF;
- //11101111 指定ピンのみ変更
- //right front leg down
- _doLed[2] = 0;
- Motor_state();
-
-}
-
-void _atMotorStop6(){
-
- _boMotor = _boMotor & 0xFD;
- //11111101 指定ピンのみ変更
- //right back leg up
- _doLed[3] = 0;
- Motor_state();
-
-}
-
-void _atMotorStop7(){
- _toWait[3].attach(&_atWaitStop3,WAIT_TIME);
- _boMotor = _boMotor & 0xFE;
- //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])){
- 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])){
- 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;
- Motor_state();
-
- }else{
- u8Ct2[0] = 2;
-
+ if(AndCk){
+ UpCt ++;
+ And[0] = And[1] = And[2] = And[3] = 0 ;
+ if(UpCt > 4){
+ UpCt = 0;
+ }
}
}
-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;
- Motor_state();
-
- }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;
- Motor_state();
-
- }else{
- u8Ct2[1] = 2;
-
+void SetUp(){
+ int i;
+ for(i = 0 ; i > 4 ; i++ ){
+ Pwm[i] = PWM_LEVEL ;
+ }
+ for(i = 0 ; i > MOTOR_NUM ;i++ ){
+ Photo[i].mode(PullUp);
}
-
-}
-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;
- Motor_state();
+ 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);
- }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;
- Motor_state();
-
- }else{
- u8Ct2[2] = 2;
- }
+ Sw.mode(PullUp);
+ Sw.fall(Reset);
+ TempraPwm = PWM_LEVEL;
+
}
-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 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])){
- u8Ct1[2]=0;
- u8Ct2[3]=1;
- //leg down
- _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
- _boMotor = _boMotor | 0x04;//00000100
- _doLed[1] = 1;
- bWait[1] = 1;
- Motor_state();
-
- }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;
- Motor_state();
-
- }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;
- Motor_state();
- 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;
- Motor_state();
-
- }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
- Motor_state();
- _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
- 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])){
- 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;
-
- }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
- 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])){
- 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;
-
- }else{
- u8Ct2[7] = 2;
- }
-}
-
-
-
int main() {
- int m;
-
- 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);
- limit[1].mode(PullUp);
- limit[2].mode(PullUp);
- limit[3].mode(PullUp);
- limit[4].mode(PullUp);
- 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);
-
-
- //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 */
- _poPwm[0] = PWM_LEVEL ;
- _poPwm[1] = PWM_LEVEL ;
- _poPwm[2] = PWM_LEVEL ;
- _poPwm[3] = PWM_LEVEL ;
- /* */
-
-
- wait(0.5);
- Motor_state();
-
- while(1){
- Motor_state();
- 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]);
-
-
-
- }
+ SetUp();
+ wait(0.5);
while(1){
- /*左の方-リミットスイッチ0~3*/
- if(!limit[0] && !bClim[0]){
-
- /* 1個目*/
- if(!bWait[0] && !u8Ct1[0] && !u8Ct1[1]){
- 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])){
- 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;
- }
- bClim[0] = 1;
-
- }else if(limit[0]){
- bClim[0] = 0;
- }
-
-
- if(!limit[1] && !bClim[1]){
-
- /* 2個目*/
- if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]){
- 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])){
- 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;
-
- }
- bClim[1] = 1;
-
- }else if(limit[1]){
- bClim[1] = 0;
- }
-
-
- if(!limit[2] && !bClim[2]){
-
- /* 3個目*/
- if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]){
- 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])){
- 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;
- }
-
- bClim[2] = 1;
- }else if(limit[2]){
- bClim[2] = 0;
- }
-
-
-
- if(!limit[3] && !bClim[3]){
- /* 4個目*/
- if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]){
- 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])){
- 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;
- }
-
- bClim[3] = 1;
- }else if(limit[3]){
- bClim[3] = 0;
- }
-
-
- /*右の方 リミットスイッチ4~7 */
+ NumCount();
- if(!limit[4] && !bClim[4]){
-
- /* 5個目*/
- if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]){
- 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])){
- 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;
- }
- bClim[4] = 1;
-
- }else if(limit[4]){
- bClim[4] = 0;
- }
-
-
- if(!limit[5] && !bClim[5]){
-
- /* 6個目*/
- if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]){
- 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])){
- 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;
-
- }
- bClim[5] = 1;
-
- }else if(limit[5]){
- bClim[5] = 0;
- }
-
-
- if(!limit[6] && !bClim[6]){
-
- /* 7個目*/
- if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]){
- 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])){
- 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;
- }
-
- bClim[6] = 1;
- }else if(limit[6]){
- bClim[6] = 0;
- }
-
-
-
- if(!limit[7] && !bClim[7]){
- /* 8個目*/
- if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]){
- 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])){
- 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;
- }
-
- bClim[7] = 1;
- }else if(limit[7]){
- bClim[7] = 0;
- }
-
}
}