Bチーム / Mbed 2 deprecated 4LegUpDown_8sense_ver2

足上げセンサ八個版仮

Dependencies:   mbed

Fork of 4LegUpDown_8sense by Bチーム

main.cpp@1:807091115672, 2014-08-21 (annotated)

Committer:
ryuna
Date:
Thu Aug 21 09:45:46 2014 +0000
Revision:
1:807091115672
Parent:
0:76ba33d3b4a6
Child:
2:89cf1003e119
pin fall attach use version & noise countermeasure enhancement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:76ba33d3b4a6 1 #include "mbed.h"
ryuna 0:76ba33d3b4a6 2 #define MOVE_TIME 0.11
ryuna 0:76ba33d3b4a6 3 #define WAIT_TIME 0.02
ryuna 0:76ba33d3b4a6 4 #define PWM_LEVEL 1.0// 11.1 / 7.4
ryuna 0:76ba33d3b4a6 5
ryuna 0:76ba33d3b4a6 6
ryuna 0:76ba33d3b4a6 7 /*
ryuna 0:76ba33d3b4a6 8 センサの数を八個で足一つに付き二つのセンサで足を上下させるプログラム．
ryuna 0:76ba33d3b4a6 9
ryuna 0:76ba33d3b4a6 10 はじめのリミットスイッチが押されてもう一度押されるまでの時間を計り現在の速さを概算してタイマー制御に役立てる
ryuna 0:76ba33d3b4a6 11
ryuna 1:807091115672 12 pin rise/fall attach use
ryuna 0:76ba33d3b4a6 13 *//**/
ryuna 0:76ba33d3b4a6 14
ryuna 0:76ba33d3b4a6 15
ryuna 0:76ba33d3b4a6 16 Serial pc(USBTX,USBRX);
ryuna 0:76ba33d3b4a6 17
ryuna 0:76ba33d3b4a6 18 Timeout _toMotor[8];
ryuna 0:76ba33d3b4a6 19 Timeout _toWait[4];
ryuna 1:807091115672 20
ryuna 1:807091115672 21 DigitalIn limit[8] = {NC,NC,NC,NC,NC,NC,NC,NC };
ryuna 1:807091115672 22 InterruptIn _iiLsw[8] = {p5,p6,p7,p8,p9,p10,p11,p12 };/*swicth is a-contact*/
ryuna 1:807091115672 23
ryuna 0:76ba33d3b4a6 24 DigitalOut _doLed[4] = {LED1, LED2, LED3, LED4 };
ryuna 0:76ba33d3b4a6 25
ryuna 0:76ba33d3b4a6 26 BusOut _boMotor(p13, p14, p15, p16, p17, p18, p19, p20 );//four motors
ryuna 1:807091115672 27 //{left front, right front, left back ，right back } in sequeNCe
ryuna 0:76ba33d3b4a6 28 PwmOut _poPwm[4] = {p21, p22, p23, p24 };
ryuna 0:76ba33d3b4a6 29
ryuna 0:76ba33d3b4a6 30 /**more use binary number
ryuna 0:76ba33d3b4a6 31 0xA0 = 10100000
ryuna 0:76ba33d3b4a6 32 0x50 = 01010000
ryuna 0:76ba33d3b4a6 33 0x0A = 00001010
ryuna 0:76ba33d3b4a6 34 0x05 = 00000101
ryuna 0:76ba33d3b4a6 35 **/
ryuna 0:76ba33d3b4a6 36
ryuna 0:76ba33d3b4a6 37 volatile bool bWait[4] = {0};
ryuna 1:807091115672 38 volatile bool bMotor[8] = {0};//名前が似ているものがあるので注意
ryuna 1:807091115672 39 volatile uint8_t u8Ct1[8] = {0},u8Ct2[8] = {1,1,1,1,1,1,1,1};//uint8_t Count{1,2}[]
ryuna 1:807091115672 40
ryuna 1:807091115672 41
ryuna 1:807091115672 42 enum {LFU = 0,LFD,RFU,RFD,LBU,LBD,RBU,RBD};
ryuna 1:807091115672 43 /*
ryuna 1:807091115672 44 bMotor = {
ryuna 1:807091115672 45 LeftFrontUp, LeftFrontDown,
ryuna 1:807091115672 46 RightFrontUp, RightFrontDown,
ryuna 1:807091115672 47 LeftBackUp, LeftBackDown,
ryuna 1:807091115672 48 RightBackUp, RightBackDown
ryuna 1:807091115672 49 };
ryuna 1:807091115672 50 */
ryuna 1:807091115672 51
ryuna 1:807091115672 52 void Motor_state(){
ryuna 1:807091115672 53 /*
ryuna 1:807091115672 54 uint8_t GetBit(uint8_t n, uint8_t bit)
ryuna 1:807091115672 55 return (n >>(bit-1))%2;
ryuna 1:807091115672 56 }by takamastu
ryuna 1:807091115672 57 */
ryuna 1:807091115672 58 uint8_t i;
ryuna 1:807091115672 59
ryuna 1:807091115672 60 for (i = 0; i < 8 ; i++){
ryuna 1:807091115672 61 bMotor[i] = ( _boMotor >> i )%2;
ryuna 1:807091115672 62 }
ryuna 1:807091115672 63
ryuna 1:807091115672 64 }
ryuna 0:76ba33d3b4a6 65
ryuna 0:76ba33d3b4a6 66 void _atWaitStop0(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 67 //make delay time
ryuna 0:76ba33d3b4a6 68 bWait[0] = 0;
ryuna 0:76ba33d3b4a6 69
ryuna 0:76ba33d3b4a6 70 }
ryuna 0:76ba33d3b4a6 71
ryuna 0:76ba33d3b4a6 72 void _atWaitStop1(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 73 //make delay time
ryuna 0:76ba33d3b4a6 74 bWait[1] = 0;
ryuna 0:76ba33d3b4a6 75
ryuna 0:76ba33d3b4a6 76 }
ryuna 0:76ba33d3b4a6 77
ryuna 0:76ba33d3b4a6 78 void _atWaitStop2(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 79 //make delay time
ryuna 0:76ba33d3b4a6 80 bWait[2] = 0;
ryuna 0:76ba33d3b4a6 81
ryuna 0:76ba33d3b4a6 82 }
ryuna 0:76ba33d3b4a6 83
ryuna 0:76ba33d3b4a6 84 void _atWaitStop3(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 85 //make delay time
ryuna 0:76ba33d3b4a6 86 bWait[3] = 0;
ryuna 0:76ba33d3b4a6 87
ryuna 0:76ba33d3b4a6 88 }
ryuna 0:76ba33d3b4a6 89
ryuna 0:76ba33d3b4a6 90
ryuna 0:76ba33d3b4a6 91
ryuna 0:76ba33d3b4a6 92 void _atMotorStop0(){//atatch Motor Stop0
ryuna 0:76ba33d3b4a6 93
ryuna 0:76ba33d3b4a6 94 _boMotor = _boMotor & 0x7F;
ryuna 0:76ba33d3b4a6 95 //01111111 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 96 //left front leg up
ryuna 0:76ba33d3b4a6 97 _doLed[0] = 0;
ryuna 0:76ba33d3b4a6 98 }
ryuna 0:76ba33d3b4a6 99
ryuna 0:76ba33d3b4a6 100
ryuna 0:76ba33d3b4a6 101 void _atMotorStop1(){
ryuna 0:76ba33d3b4a6 102 _toWait[0].attach(&_atWaitStop0,WAIT_TIME);
ryuna 0:76ba33d3b4a6 103 _boMotor = _boMotor & 0xBF;
ryuna 0:76ba33d3b4a6 104 //10111111 　指定ピンのみ変更
ryuna 0:76ba33d3b4a6 105 //left front leg down
ryuna 1:807091115672 106 _doLed[0] = 0;
ryuna 0:76ba33d3b4a6 107
ryuna 0:76ba33d3b4a6 108 }
ryuna 0:76ba33d3b4a6 109
ryuna 0:76ba33d3b4a6 110 void _atMotorStop2(){
ryuna 0:76ba33d3b4a6 111
ryuna 0:76ba33d3b4a6 112 _boMotor = _boMotor & 0xF7;
ryuna 0:76ba33d3b4a6 113 //11110111 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 114 //left back leg up
ryuna 1:807091115672 115 _doLed[1] = 0;
ryuna 0:76ba33d3b4a6 116
ryuna 0:76ba33d3b4a6 117 }
ryuna 0:76ba33d3b4a6 118
ryuna 0:76ba33d3b4a6 119 void _atMotorStop3(){
ryuna 0:76ba33d3b4a6 120 _toWait[1].attach(&_atWaitStop1,WAIT_TIME);
ryuna 0:76ba33d3b4a6 121 _boMotor = _boMotor & 0xFB;
ryuna 0:76ba33d3b4a6 122 //11111011 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 123 //left back leg down
ryuna 1:807091115672 124 _doLed[1] = 0;
ryuna 0:76ba33d3b4a6 125
ryuna 0:76ba33d3b4a6 126 }
ryuna 0:76ba33d3b4a6 127 void _atMotorStop4(){//atatch Motor Stop0
ryuna 0:76ba33d3b4a6 128
ryuna 0:76ba33d3b4a6 129 _boMotor = _boMotor & 0xDF;
ryuna 0:76ba33d3b4a6 130 //11011111 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 131 //right front leg up
ryuna 1:807091115672 132 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 133
ryuna 0:76ba33d3b4a6 134 }
ryuna 0:76ba33d3b4a6 135
ryuna 0:76ba33d3b4a6 136
ryuna 0:76ba33d3b4a6 137 void _atMotorStop5(){
ryuna 0:76ba33d3b4a6 138 _toWait[2].attach(&_atWaitStop2,WAIT_TIME);
ryuna 0:76ba33d3b4a6 139 _boMotor = _boMotor & 0xEF;
ryuna 0:76ba33d3b4a6 140 //11101111 　指定ピンのみ変更
ryuna 0:76ba33d3b4a6 141 //right front leg down
ryuna 1:807091115672 142 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 143
ryuna 0:76ba33d3b4a6 144 }
ryuna 0:76ba33d3b4a6 145
ryuna 0:76ba33d3b4a6 146 void _atMotorStop6(){
ryuna 0:76ba33d3b4a6 147
ryuna 0:76ba33d3b4a6 148 _boMotor = _boMotor & 0xFD;
ryuna 0:76ba33d3b4a6 149 //11111101 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 150 //right back leg up
ryuna 1:807091115672 151 _doLed[3] = 0;
ryuna 1:807091115672 152
ryuna 0:76ba33d3b4a6 153 }
ryuna 0:76ba33d3b4a6 154
ryuna 0:76ba33d3b4a6 155 void _atMotorStop7(){
ryuna 0:76ba33d3b4a6 156 _toWait[3].attach(&_atWaitStop3,WAIT_TIME);
ryuna 0:76ba33d3b4a6 157 _boMotor = _boMotor & 0xFE;
ryuna 0:76ba33d3b4a6 158 //11111110 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 159 //right back leg down
ryuna 1:807091115672 160 _doLed[3] = 0;
ryuna 0:76ba33d3b4a6 161
ryuna 1:807091115672 162 }
ryuna 1:807091115672 163
ryuna 1:807091115672 164 void _aiiFLsw0(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 165 /* 1個目*/
ryuna 1:807091115672 166 if(!bWait[0] && !u8Ct1[0] && !u8Ct1[1]&&((_boMotor = 0x00)||bMotor[2]||bMotor[5]||bMotor[7])){
ryuna 1:807091115672 167 u8Ct1[0] = 1;
ryuna 1:807091115672 168 //legs up
ryuna 1:807091115672 169 _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
ryuna 1:807091115672 170 _boMotor = _boMotor | 0x80;//10000000
ryuna 1:807091115672 171 _doLed[0] = 1;
ryuna 1:807091115672 172
ryuna 1:807091115672 173 }else if(u8Ct1[0]){
ryuna 1:807091115672 174 u8Ct1[0] = 2;
ryuna 1:807091115672 175
ryuna 1:807091115672 176 }else if((u8Ct1[1] == u8Ct2[0])&&((_boMotor = 0x00)||bMotor[3]||bMotor[4]||bMotor[6])){
ryuna 1:807091115672 177 u8Ct1[1]=0;
ryuna 1:807091115672 178 u8Ct2[0]=1;
ryuna 1:807091115672 179 //legs down
ryuna 1:807091115672 180 _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
ryuna 1:807091115672 181 _boMotor = _boMotor | 0x40;//0x40 = 01000000
ryuna 1:807091115672 182 _doLed[0] = 1;
ryuna 1:807091115672 183 bWait[0] = 1;
ryuna 1:807091115672 184
ryuna 1:807091115672 185 }else{
ryuna 1:807091115672 186 u8Ct2[0] = 2;
ryuna 0:76ba33d3b4a6 187
ryuna 1:807091115672 188 }
ryuna 1:807091115672 189 }
ryuna 1:807091115672 190
ryuna 1:807091115672 191 void _aiiFLsw1(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 192 /* 2個目*/
ryuna 1:807091115672 193 if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]&&((_boMotor = 0x00)||bMotor[2]||bMotor[5]||bMotor[7])){
ryuna 1:807091115672 194 u8Ct1[1] = 1;
ryuna 1:807091115672 195 //legs up
ryuna 1:807091115672 196 _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
ryuna 1:807091115672 197 _boMotor = _boMotor | 0x80;// 10000000
ryuna 1:807091115672 198 _doLed[0] = 1;
ryuna 1:807091115672 199
ryuna 1:807091115672 200 }else if(u8Ct1[1]){
ryuna 1:807091115672 201 u8Ct1[1] = 2;
ryuna 1:807091115672 202
ryuna 1:807091115672 203 }else if((u8Ct1[0] == u8Ct2[1])&&((_boMotor = 0x00)||bMotor[3]||bMotor[4]||bMotor[6])){
ryuna 1:807091115672 204 u8Ct1[0]=0;
ryuna 1:807091115672 205 u8Ct2[1]=1;
ryuna 1:807091115672 206 //legs down
ryuna 1:807091115672 207 _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
ryuna 1:807091115672 208 _boMotor = _boMotor | 0x40;// 01000000
ryuna 1:807091115672 209 _doLed[0] = 1;
ryuna 1:807091115672 210 bWait[0] = 1;
ryuna 1:807091115672 211
ryuna 1:807091115672 212 }else{
ryuna 1:807091115672 213 u8Ct2[1] = 2;
ryuna 1:807091115672 214
ryuna 1:807091115672 215 }
ryuna 1:807091115672 216
ryuna 1:807091115672 217 }
ryuna 1:807091115672 218 void _aiiFLsw2(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 219 /* 3個目*/
ryuna 1:807091115672 220 if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
ryuna 1:807091115672 221 u8Ct1[2] = 1;
ryuna 1:807091115672 222 //legs up
ryuna 1:807091115672 223 _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
ryuna 1:807091115672 224 _boMotor = _boMotor | 0x08;//00001000
ryuna 1:807091115672 225 _doLed[1] = 1;
ryuna 1:807091115672 226
ryuna 1:807091115672 227 }else if(u8Ct1[2]){
ryuna 1:807091115672 228 u8Ct1[2] = 2;
ryuna 1:807091115672 229
ryuna 1:807091115672 230 }else if((u8Ct1[3] == u8Ct2[2])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[7])){
ryuna 1:807091115672 231 u8Ct1[3]=0;
ryuna 1:807091115672 232 u8Ct2[2]=1;
ryuna 1:807091115672 233 //legs down
ryuna 1:807091115672 234 _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
ryuna 1:807091115672 235 _boMotor = _boMotor | 0x04;//00000100
ryuna 1:807091115672 236 _doLed[1] = 1;
ryuna 1:807091115672 237 bWait[1] = 1;
ryuna 1:807091115672 238
ryuna 1:807091115672 239 }else{
ryuna 1:807091115672 240 u8Ct2[2] = 2;
ryuna 1:807091115672 241 }
ryuna 1:807091115672 242
ryuna 1:807091115672 243 }
ryuna 1:807091115672 244 void _aiiFLsw3(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 245 /* 4個目*/
ryuna 1:807091115672 246 if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[6])){
ryuna 1:807091115672 247 u8Ct1[3] = 1;
ryuna 1:807091115672 248 //leg up
ryuna 1:807091115672 249 _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
ryuna 1:807091115672 250 _boMotor = _boMotor | 0x08;// 00001000
ryuna 1:807091115672 251 _doLed[1] = 1;
ryuna 1:807091115672 252
ryuna 1:807091115672 253 }else if(u8Ct1[3]){
ryuna 1:807091115672 254 u8Ct1[3] = 2;
ryuna 1:807091115672 255
ryuna 1:807091115672 256 }else if((u8Ct1[2] == u8Ct2[3])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[7])){
ryuna 1:807091115672 257 u8Ct1[2]=0;
ryuna 1:807091115672 258 u8Ct2[3]=1;
ryuna 1:807091115672 259 //leg down
ryuna 1:807091115672 260 _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
ryuna 1:807091115672 261 _boMotor = _boMotor | 0x04;//00000100
ryuna 1:807091115672 262 _doLed[1] = 1;
ryuna 1:807091115672 263 bWait[1] = 1;
ryuna 1:807091115672 264
ryuna 1:807091115672 265 }else{
ryuna 1:807091115672 266 u8Ct2[3] = 2;
ryuna 1:807091115672 267 }
ryuna 1:807091115672 268 }
ryuna 1:807091115672 269 void _aiiFLsw4(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 270 /* 5個目*/
ryuna 1:807091115672 271 if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]&&((_boMotor = 0x00)||bMotor[0]||bMotor[5]||bMotor[7])){
ryuna 1:807091115672 272 u8Ct1[4] = 1;
ryuna 1:807091115672 273 //leg up
ryuna 1:807091115672 274 _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
ryuna 1:807091115672 275 _boMotor = _boMotor | 0x20;//00100000
ryuna 1:807091115672 276 _doLed[2] = 0;
ryuna 1:807091115672 277
ryuna 1:807091115672 278 }else if(u8Ct1[4]){
ryuna 1:807091115672 279 u8Ct1[4] = 2;
ryuna 1:807091115672 280
ryuna 1:807091115672 281 }else if((u8Ct1[5] == u8Ct2[4])&&((_boMotor = 0x00)||bMotor[1]||bMotor[4]||bMotor[6])){
ryuna 1:807091115672 282 u8Ct1[5]=0;
ryuna 1:807091115672 283 u8Ct2[4]=1;
ryuna 1:807091115672 284 //leg down
ryuna 1:807091115672 285 _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
ryuna 1:807091115672 286 _boMotor = _boMotor | 0x10;//0x10 = 00010000
ryuna 1:807091115672 287 _doLed[2] = 0;
ryuna 1:807091115672 288 bWait[2] = 1;
ryuna 1:807091115672 289
ryuna 1:807091115672 290 }else{
ryuna 1:807091115672 291 u8Ct2[4] = 2;
ryuna 1:807091115672 292 }
ryuna 1:807091115672 293 }
ryuna 1:807091115672 294 void _aiiFLsw5(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 295 /* 6個目*/
ryuna 1:807091115672 296 if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]&&((_boMotor = 0x00)||bMotor[0]||bMotor[5]||bMotor[7])){
ryuna 1:807091115672 297 u8Ct1[5] = 1;
ryuna 1:807091115672 298 //leg up
ryuna 1:807091115672 299 _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
ryuna 1:807091115672 300 _boMotor = _boMotor | 0x20;// 00100000
ryuna 1:807091115672 301 _doLed[2] = 0;
ryuna 1:807091115672 302
ryuna 1:807091115672 303 }else if(u8Ct1[5]){
ryuna 1:807091115672 304 u8Ct1[5] = 2;
ryuna 1:807091115672 305
ryuna 1:807091115672 306 }else if((u8Ct1[4] == u8Ct2[5])&&((_boMotor = 0x00)||bMotor[1]||bMotor[4]||bMotor[6])){
ryuna 1:807091115672 307 u8Ct1[4]=0;
ryuna 1:807091115672 308 u8Ct2[5]=1;
ryuna 1:807091115672 309 //leg down
ryuna 1:807091115672 310 _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
ryuna 1:807091115672 311 _boMotor = _boMotor | 0x10;// 00010000
ryuna 1:807091115672 312 _doLed[2] = 0;
ryuna 1:807091115672 313 bWait[2] = 1;
ryuna 1:807091115672 314
ryuna 1:807091115672 315 }else{
ryuna 1:807091115672 316 u8Ct2[5] = 2;
ryuna 1:807091115672 317
ryuna 1:807091115672 318 }
ryuna 1:807091115672 319 }
ryuna 1:807091115672 320 void _aiiFLsw6(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 321
ryuna 1:807091115672 322 /* 7個目*/
ryuna 1:807091115672 323 if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[4])){
ryuna 1:807091115672 324 u8Ct1[6] = 1;
ryuna 1:807091115672 325 //legs up
ryuna 1:807091115672 326 _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
ryuna 1:807091115672 327 _boMotor = _boMotor | 0x02;//00000010
ryuna 1:807091115672 328 _doLed[3] = 0;
ryuna 1:807091115672 329
ryuna 1:807091115672 330 }else if(u8Ct1[6]){
ryuna 1:807091115672 331 u8Ct1[6] = 2;
ryuna 1:807091115672 332
ryuna 1:807091115672 333 }else if((u8Ct1[7] == u8Ct2[6])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[5])){
ryuna 1:807091115672 334 u8Ct1[7]=0;
ryuna 1:807091115672 335 u8Ct2[6]=1;
ryuna 1:807091115672 336 //leg down
ryuna 1:807091115672 337 _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
ryuna 1:807091115672 338 _boMotor = _boMotor | 0x01;//00000001
ryuna 1:807091115672 339 bWait[3] = 1;
ryuna 1:807091115672 340 _doLed[3] = 0;
ryuna 1:807091115672 341
ryuna 1:807091115672 342 }else{
ryuna 1:807091115672 343 u8Ct2[6] = 2;
ryuna 1:807091115672 344 }
ryuna 1:807091115672 345 }
ryuna 1:807091115672 346 void _aiiFLsw7(){ //attach interruptIn fall Limit switch
ryuna 1:807091115672 347 /* 8個目*/
ryuna 1:807091115672 348 if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]&&((_boMotor = 0x00)||bMotor[1]||bMotor[3]||bMotor[4])){
ryuna 1:807091115672 349 u8Ct1[7] = 1;
ryuna 1:807091115672 350 //leg up
ryuna 1:807091115672 351 _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
ryuna 1:807091115672 352 _boMotor = _boMotor | 0x02;// 00000010
ryuna 1:807091115672 353 //_doLed[6] = 1;
ryuna 1:807091115672 354 _doLed[3] = 0;
ryuna 1:807091115672 355
ryuna 1:807091115672 356 }else if(u8Ct1[7]){
ryuna 1:807091115672 357 u8Ct1[7] = 2;
ryuna 1:807091115672 358
ryuna 1:807091115672 359 }else if((u8Ct1[6] == u8Ct2[7])&&((_boMotor = 0x00)||bMotor[0]||bMotor[2]||bMotor[5])){
ryuna 1:807091115672 360 u8Ct1[6]=0;
ryuna 1:807091115672 361 u8Ct2[7]=1;
ryuna 1:807091115672 362 //leg down
ryuna 1:807091115672 363 _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
ryuna 1:807091115672 364 _boMotor = _boMotor | 0x01;//00000001
ryuna 1:807091115672 365 bWait[3] = 1;
ryuna 1:807091115672 366 _doLed[3] = 0;
ryuna 1:807091115672 367
ryuna 1:807091115672 368 }else{
ryuna 1:807091115672 369 u8Ct2[7] = 2;
ryuna 1:807091115672 370 }
ryuna 0:76ba33d3b4a6 371 }
ryuna 0:76ba33d3b4a6 372
ryuna 0:76ba33d3b4a6 373
ryuna 0:76ba33d3b4a6 374
ryuna 1:807091115672 375
ryuna 0:76ba33d3b4a6 376 int main() {
ryuna 1:807091115672 377
ryuna 1:807091115672 378
ryuna 0:76ba33d3b4a6 379 static bool bClim[8] = {1,1,1,1,1,1,1,1};//bool Check limit[]
ryuna 1:807091115672 380 /*static uint8_t u8Ct1[8] = {0},u8Ct2[8] = {1,1,1,1,1,1,1,1};//uint8_t Count{1,2}[]
ryuna 1:807091115672 381 */
ryuna 1:807091115672 382 /*
ryuna 0:76ba33d3b4a6 383 limit[0].mode(PullUp);
ryuna 0:76ba33d3b4a6 384 limit[1].mode(PullUp);
ryuna 0:76ba33d3b4a6 385 limit[2].mode(PullUp);
ryuna 0:76ba33d3b4a6 386 limit[3].mode(PullUp);
ryuna 0:76ba33d3b4a6 387 limit[4].mode(PullUp);
ryuna 0:76ba33d3b4a6 388 limit[5].mode(PullUp);
ryuna 0:76ba33d3b4a6 389 limit[6].mode(PullUp);
ryuna 0:76ba33d3b4a6 390 limit[7].mode(PullUp);
ryuna 1:807091115672 391 */
ryuna 1:807091115672 392
ryuna 1:807091115672 393 //Limit switch mode pullup
ryuna 1:807091115672 394 _iiLsw[0].mode(PullUp);
ryuna 1:807091115672 395 _iiLsw[1].mode(PullUp);
ryuna 1:807091115672 396 _iiLsw[2].mode(PullUp);
ryuna 1:807091115672 397 _iiLsw[3].mode(PullUp);
ryuna 1:807091115672 398 _iiLsw[4].mode(PullUp);
ryuna 1:807091115672 399 _iiLsw[5].mode(PullUp);
ryuna 1:807091115672 400 _iiLsw[6].mode(PullUp);
ryuna 1:807091115672 401 _iiLsw[7].mode(PullUp);
ryuna 0:76ba33d3b4a6 402
ryuna 1:807091115672 403
ryuna 1:807091115672 404 //interrupt attach fall set use
ryuna 1:807091115672 405 _iiLsw[0].fall(_aiiFLsw0);
ryuna 1:807091115672 406 _iiLsw[1].fall(_aiiFLsw1);
ryuna 1:807091115672 407 _iiLsw[2].fall(_aiiFLsw2);
ryuna 1:807091115672 408 _iiLsw[3].fall(_aiiFLsw3);
ryuna 1:807091115672 409 _iiLsw[4].fall(_aiiFLsw4);
ryuna 1:807091115672 410 _iiLsw[5].fall(_aiiFLsw5);
ryuna 1:807091115672 411 _iiLsw[6].fall(_aiiFLsw6);
ryuna 1:807091115672 412 _iiLsw[7].fall(_aiiFLsw7);
ryuna 1:807091115672 413
ryuna 0:76ba33d3b4a6 414
ryuna 0:76ba33d3b4a6 415
ryuna 0:76ba33d3b4a6 416 /* pwm level setting */
ryuna 0:76ba33d3b4a6 417 _poPwm[0] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 418 _poPwm[1] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 419 _poPwm[2] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 420 _poPwm[3] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 421 /* */
ryuna 0:76ba33d3b4a6 422
ryuna 1:807091115672 423
ryuna 1:807091115672 424 wait(0.5);
ryuna 1:807091115672 425 Motor_state();
ryuna 0:76ba33d3b4a6 426
ryuna 0:76ba33d3b4a6 427 while(1){
ryuna 1:807091115672 428 Motor_state();
ryuna 1:807091115672 429 wait(0.15);
ryuna 1:807091115672 430 _doLed[0] = !_doLed[0];
ryuna 1:807091115672 431
ryuna 1:807091115672 432
ryuna 1:807091115672 433
ryuna 1:807091115672 434 }
ryuna 1:807091115672 435 while(1){
ryuna 0:76ba33d3b4a6 436 /*左の方-リミットスイッチ0~3*/
ryuna 0:76ba33d3b4a6 437 if(!limit[0] && !bClim[0]){
ryuna 0:76ba33d3b4a6 438
ryuna 0:76ba33d3b4a6 439 /* 1個目*/
ryuna 0:76ba33d3b4a6 440 if(!bWait[0] && !u8Ct1[0] && !u8Ct1[1]){
ryuna 0:76ba33d3b4a6 441 u8Ct1[0] = 1;
ryuna 0:76ba33d3b4a6 442 //legs up
ryuna 0:76ba33d3b4a6 443 _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
ryuna 0:76ba33d3b4a6 444 _boMotor = _boMotor | 0x80;//10000000
ryuna 0:76ba33d3b4a6 445 _doLed[0] = 1;
ryuna 0:76ba33d3b4a6 446
ryuna 0:76ba33d3b4a6 447 }else if(u8Ct1[0]){
ryuna 0:76ba33d3b4a6 448 u8Ct1[0] = 2;
ryuna 0:76ba33d3b4a6 449
ryuna 0:76ba33d3b4a6 450 }else if((u8Ct1[1] == u8Ct2[0])){
ryuna 0:76ba33d3b4a6 451 u8Ct1[1]=0;
ryuna 0:76ba33d3b4a6 452 u8Ct2[0]=1;
ryuna 0:76ba33d3b4a6 453 //legs down
ryuna 0:76ba33d3b4a6 454 _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
ryuna 0:76ba33d3b4a6 455 _boMotor = _boMotor | 0x40;//0x40 = 01000000
ryuna 1:807091115672 456 _doLed[0] = 1;
ryuna 0:76ba33d3b4a6 457 bWait[0] = 1;
ryuna 0:76ba33d3b4a6 458
ryuna 0:76ba33d3b4a6 459 }else{
ryuna 0:76ba33d3b4a6 460 u8Ct2[0] = 2;
ryuna 0:76ba33d3b4a6 461 }
ryuna 0:76ba33d3b4a6 462 bClim[0] = 1;
ryuna 0:76ba33d3b4a6 463
ryuna 0:76ba33d3b4a6 464 }else if(limit[0]){
ryuna 0:76ba33d3b4a6 465 bClim[0] = 0;
ryuna 0:76ba33d3b4a6 466 }
ryuna 0:76ba33d3b4a6 467
ryuna 0:76ba33d3b4a6 468
ryuna 0:76ba33d3b4a6 469 if(!limit[1] && !bClim[1]){
ryuna 0:76ba33d3b4a6 470
ryuna 0:76ba33d3b4a6 471 /* 2個目*/
ryuna 0:76ba33d3b4a6 472 if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]){
ryuna 0:76ba33d3b4a6 473 u8Ct1[1] = 1;
ryuna 0:76ba33d3b4a6 474 //legs up
ryuna 0:76ba33d3b4a6 475 _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
ryuna 0:76ba33d3b4a6 476 _boMotor = _boMotor | 0x80;// 10000000
ryuna 0:76ba33d3b4a6 477 _doLed[0] = 1;
ryuna 0:76ba33d3b4a6 478
ryuna 0:76ba33d3b4a6 479 }else if(u8Ct1[1]){
ryuna 0:76ba33d3b4a6 480 u8Ct1[1] = 2;
ryuna 0:76ba33d3b4a6 481
ryuna 0:76ba33d3b4a6 482 }else if((u8Ct1[0] == u8Ct2[1])){
ryuna 0:76ba33d3b4a6 483 u8Ct1[0]=0;
ryuna 0:76ba33d3b4a6 484 u8Ct2[1]=1;
ryuna 0:76ba33d3b4a6 485 //legs down
ryuna 0:76ba33d3b4a6 486 _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
ryuna 0:76ba33d3b4a6 487 _boMotor = _boMotor | 0x40;// 01000000
ryuna 1:807091115672 488 _doLed[0] = 1;
ryuna 0:76ba33d3b4a6 489 bWait[0] = 1;
ryuna 0:76ba33d3b4a6 490
ryuna 0:76ba33d3b4a6 491 }else{
ryuna 0:76ba33d3b4a6 492 u8Ct2[1] = 2;
ryuna 0:76ba33d3b4a6 493
ryuna 0:76ba33d3b4a6 494 }
ryuna 0:76ba33d3b4a6 495 bClim[1] = 1;
ryuna 0:76ba33d3b4a6 496
ryuna 0:76ba33d3b4a6 497 }else if(limit[1]){
ryuna 0:76ba33d3b4a6 498 bClim[1] = 0;
ryuna 0:76ba33d3b4a6 499 }
ryuna 0:76ba33d3b4a6 500
ryuna 0:76ba33d3b4a6 501
ryuna 0:76ba33d3b4a6 502 if(!limit[2] && !bClim[2]){
ryuna 0:76ba33d3b4a6 503
ryuna 0:76ba33d3b4a6 504 /* 3個目*/
ryuna 0:76ba33d3b4a6 505 if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]){
ryuna 0:76ba33d3b4a6 506 u8Ct1[2] = 1;
ryuna 0:76ba33d3b4a6 507 //legs up
ryuna 0:76ba33d3b4a6 508 _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
ryuna 0:76ba33d3b4a6 509 _boMotor = _boMotor | 0x08;//00001000
ryuna 1:807091115672 510 _doLed[1] = 1;
ryuna 0:76ba33d3b4a6 511 }else if(u8Ct1[2]){
ryuna 0:76ba33d3b4a6 512 u8Ct1[2] = 2;
ryuna 0:76ba33d3b4a6 513
ryuna 0:76ba33d3b4a6 514 }else if((u8Ct1[3] == u8Ct2[2])){
ryuna 0:76ba33d3b4a6 515 u8Ct1[3]=0;
ryuna 0:76ba33d3b4a6 516 u8Ct2[2]=1;
ryuna 0:76ba33d3b4a6 517 //legs down
ryuna 0:76ba33d3b4a6 518 _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
ryuna 0:76ba33d3b4a6 519 _boMotor = _boMotor | 0x04;//00000100
ryuna 1:807091115672 520 _doLed[1] = 1;
ryuna 0:76ba33d3b4a6 521 bWait[1] = 1;
ryuna 0:76ba33d3b4a6 522 }else{
ryuna 0:76ba33d3b4a6 523 u8Ct2[2] = 2;
ryuna 0:76ba33d3b4a6 524 }
ryuna 0:76ba33d3b4a6 525
ryuna 0:76ba33d3b4a6 526 bClim[2] = 1;
ryuna 0:76ba33d3b4a6 527 }else if(limit[2]){
ryuna 0:76ba33d3b4a6 528 bClim[2] = 0;
ryuna 0:76ba33d3b4a6 529 }
ryuna 0:76ba33d3b4a6 530
ryuna 0:76ba33d3b4a6 531
ryuna 0:76ba33d3b4a6 532
ryuna 0:76ba33d3b4a6 533 if(!limit[3] && !bClim[3]){
ryuna 0:76ba33d3b4a6 534 /* 4個目*/
ryuna 0:76ba33d3b4a6 535 if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]){
ryuna 0:76ba33d3b4a6 536 u8Ct1[3] = 1;
ryuna 0:76ba33d3b4a6 537 //leg up
ryuna 0:76ba33d3b4a6 538 _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
ryuna 0:76ba33d3b4a6 539 _boMotor = _boMotor | 0x08;// 00001000
ryuna 1:807091115672 540 _doLed[1] = 1;
ryuna 0:76ba33d3b4a6 541
ryuna 0:76ba33d3b4a6 542 }else if(u8Ct1[3]){
ryuna 0:76ba33d3b4a6 543 u8Ct1[3] = 2;
ryuna 0:76ba33d3b4a6 544
ryuna 0:76ba33d3b4a6 545 }else if((u8Ct1[2] == u8Ct2[3])){
ryuna 0:76ba33d3b4a6 546 u8Ct1[2]=0;
ryuna 0:76ba33d3b4a6 547 u8Ct2[3]=1;
ryuna 0:76ba33d3b4a6 548 //leg down
ryuna 0:76ba33d3b4a6 549 _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
ryuna 0:76ba33d3b4a6 550 _boMotor = _boMotor | 0x04;//00000100
ryuna 1:807091115672 551 _doLed[1] = 1;
ryuna 0:76ba33d3b4a6 552 bWait[1] = 1;
ryuna 0:76ba33d3b4a6 553 }else{
ryuna 0:76ba33d3b4a6 554 u8Ct2[3] = 2;
ryuna 0:76ba33d3b4a6 555 }
ryuna 0:76ba33d3b4a6 556
ryuna 0:76ba33d3b4a6 557 bClim[3] = 1;
ryuna 0:76ba33d3b4a6 558 }else if(limit[3]){
ryuna 0:76ba33d3b4a6 559 bClim[3] = 0;
ryuna 0:76ba33d3b4a6 560 }
ryuna 0:76ba33d3b4a6 561
ryuna 0:76ba33d3b4a6 562
ryuna 0:76ba33d3b4a6 563 /*右の方 リミットスイッチ4~7 */
ryuna 0:76ba33d3b4a6 564
ryuna 0:76ba33d3b4a6 565 if(!limit[4] && !bClim[4]){
ryuna 0:76ba33d3b4a6 566
ryuna 0:76ba33d3b4a6 567 /* 5個目*/
ryuna 0:76ba33d3b4a6 568 if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]){
ryuna 0:76ba33d3b4a6 569 u8Ct1[4] = 1;
ryuna 0:76ba33d3b4a6 570 //leg up
ryuna 0:76ba33d3b4a6 571 _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
ryuna 0:76ba33d3b4a6 572 _boMotor = _boMotor | 0x20;//00100000
ryuna 1:807091115672 573 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 574
ryuna 0:76ba33d3b4a6 575 }else if(u8Ct1[4]){
ryuna 0:76ba33d3b4a6 576 u8Ct1[4] = 2;
ryuna 0:76ba33d3b4a6 577
ryuna 0:76ba33d3b4a6 578 }else if((u8Ct1[5] == u8Ct2[4])){
ryuna 0:76ba33d3b4a6 579 u8Ct1[5]=0;
ryuna 0:76ba33d3b4a6 580 u8Ct2[4]=1;
ryuna 0:76ba33d3b4a6 581 //leg down
ryuna 0:76ba33d3b4a6 582 _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
ryuna 0:76ba33d3b4a6 583 _boMotor = _boMotor | 0x10;//0x10 = 00010000
ryuna 1:807091115672 584 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 585 bWait[2] = 1;
ryuna 0:76ba33d3b4a6 586
ryuna 0:76ba33d3b4a6 587 }else{
ryuna 0:76ba33d3b4a6 588 u8Ct2[4] = 2;
ryuna 0:76ba33d3b4a6 589 }
ryuna 0:76ba33d3b4a6 590 bClim[4] = 1;
ryuna 0:76ba33d3b4a6 591
ryuna 0:76ba33d3b4a6 592 }else if(limit[4]){
ryuna 0:76ba33d3b4a6 593 bClim[4] = 0;
ryuna 0:76ba33d3b4a6 594 }
ryuna 0:76ba33d3b4a6 595
ryuna 0:76ba33d3b4a6 596
ryuna 0:76ba33d3b4a6 597 if(!limit[5] && !bClim[5]){
ryuna 0:76ba33d3b4a6 598
ryuna 0:76ba33d3b4a6 599 /* 6個目*/
ryuna 0:76ba33d3b4a6 600 if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]){
ryuna 0:76ba33d3b4a6 601 u8Ct1[5] = 1;
ryuna 0:76ba33d3b4a6 602 //leg up
ryuna 0:76ba33d3b4a6 603 _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
ryuna 0:76ba33d3b4a6 604 _boMotor = _boMotor | 0x20;// 00100000
ryuna 1:807091115672 605 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 606
ryuna 0:76ba33d3b4a6 607 }else if(u8Ct1[5]){
ryuna 0:76ba33d3b4a6 608 u8Ct1[5] = 2;
ryuna 0:76ba33d3b4a6 609
ryuna 0:76ba33d3b4a6 610 }else if((u8Ct1[4] == u8Ct2[5])){
ryuna 0:76ba33d3b4a6 611 u8Ct1[4]=0;
ryuna 0:76ba33d3b4a6 612 u8Ct2[5]=1;
ryuna 0:76ba33d3b4a6 613 //leg down
ryuna 0:76ba33d3b4a6 614 _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
ryuna 0:76ba33d3b4a6 615 _boMotor = _boMotor | 0x10;// 00010000
ryuna 1:807091115672 616 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 617 bWait[2] = 1;
ryuna 0:76ba33d3b4a6 618
ryuna 0:76ba33d3b4a6 619 }else{
ryuna 0:76ba33d3b4a6 620 u8Ct2[5] = 2;
ryuna 0:76ba33d3b4a6 621
ryuna 0:76ba33d3b4a6 622 }
ryuna 0:76ba33d3b4a6 623 bClim[5] = 1;
ryuna 0:76ba33d3b4a6 624
ryuna 0:76ba33d3b4a6 625 }else if(limit[5]){
ryuna 0:76ba33d3b4a6 626 bClim[5] = 0;
ryuna 0:76ba33d3b4a6 627 }
ryuna 0:76ba33d3b4a6 628
ryuna 0:76ba33d3b4a6 629
ryuna 0:76ba33d3b4a6 630 if(!limit[6] && !bClim[6]){
ryuna 0:76ba33d3b4a6 631
ryuna 0:76ba33d3b4a6 632 /* 7個目*/
ryuna 0:76ba33d3b4a6 633 if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]){
ryuna 0:76ba33d3b4a6 634 u8Ct1[6] = 1;
ryuna 0:76ba33d3b4a6 635 //legs up
ryuna 0:76ba33d3b4a6 636 _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
ryuna 0:76ba33d3b4a6 637 _boMotor = _boMotor | 0x02;//00000010
ryuna 1:807091115672 638 _doLed[3] = 0;
ryuna 1:807091115672 639
ryuna 0:76ba33d3b4a6 640 }else if(u8Ct1[6]){
ryuna 0:76ba33d3b4a6 641 u8Ct1[6] = 2;
ryuna 0:76ba33d3b4a6 642
ryuna 0:76ba33d3b4a6 643 }else if((u8Ct1[7] == u8Ct2[6])){
ryuna 0:76ba33d3b4a6 644 u8Ct1[7]=0;
ryuna 0:76ba33d3b4a6 645 u8Ct2[6]=1;
ryuna 0:76ba33d3b4a6 646 //leg down
ryuna 0:76ba33d3b4a6 647 _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
ryuna 0:76ba33d3b4a6 648 _boMotor = _boMotor | 0x01;//00000001
ryuna 0:76ba33d3b4a6 649 bWait[3] = 1;
ryuna 1:807091115672 650 _doLed[3] = 0;
ryuna 1:807091115672 651
ryuna 0:76ba33d3b4a6 652 }else{
ryuna 0:76ba33d3b4a6 653 u8Ct2[6] = 2;
ryuna 0:76ba33d3b4a6 654 }
ryuna 0:76ba33d3b4a6 655
ryuna 0:76ba33d3b4a6 656 bClim[6] = 1;
ryuna 0:76ba33d3b4a6 657 }else if(limit[6]){
ryuna 0:76ba33d3b4a6 658 bClim[6] = 0;
ryuna 0:76ba33d3b4a6 659 }
ryuna 0:76ba33d3b4a6 660
ryuna 0:76ba33d3b4a6 661
ryuna 0:76ba33d3b4a6 662
ryuna 0:76ba33d3b4a6 663 if(!limit[7] && !bClim[7]){
ryuna 0:76ba33d3b4a6 664 /* 8個目*/
ryuna 0:76ba33d3b4a6 665 if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]){
ryuna 0:76ba33d3b4a6 666 u8Ct1[7] = 1;
ryuna 0:76ba33d3b4a6 667 //leg up
ryuna 0:76ba33d3b4a6 668 _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
ryuna 0:76ba33d3b4a6 669 _boMotor = _boMotor | 0x02;// 00000010
ryuna 0:76ba33d3b4a6 670 //_doLed[6] = 1;
ryuna 1:807091115672 671 _doLed[3] = 0;
ryuna 0:76ba33d3b4a6 672
ryuna 0:76ba33d3b4a6 673 }else if(u8Ct1[7]){
ryuna 0:76ba33d3b4a6 674 u8Ct1[7] = 2;
ryuna 0:76ba33d3b4a6 675
ryuna 0:76ba33d3b4a6 676 }else if((u8Ct1[6] == u8Ct2[7])){
ryuna 0:76ba33d3b4a6 677 u8Ct1[6]=0;
ryuna 0:76ba33d3b4a6 678 u8Ct2[7]=1;
ryuna 0:76ba33d3b4a6 679 //leg down
ryuna 0:76ba33d3b4a6 680 _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
ryuna 0:76ba33d3b4a6 681 _boMotor = _boMotor | 0x01;//00000001
ryuna 0:76ba33d3b4a6 682 bWait[3] = 1;
ryuna 1:807091115672 683 _doLed[3] = 0;
ryuna 0:76ba33d3b4a6 684
ryuna 0:76ba33d3b4a6 685 }else{
ryuna 0:76ba33d3b4a6 686 u8Ct2[7] = 2;
ryuna 0:76ba33d3b4a6 687 }
ryuna 0:76ba33d3b4a6 688
ryuna 0:76ba33d3b4a6 689 bClim[7] = 1;
ryuna 0:76ba33d3b4a6 690 }else if(limit[7]){
ryuna 0:76ba33d3b4a6 691 bClim[7] = 0;
ryuna 0:76ba33d3b4a6 692 }
ryuna 0:76ba33d3b4a6 693
ryuna 0:76ba33d3b4a6 694 }
ryuna 0:76ba33d3b4a6 695 }