Bチーム / Mbed 2 deprecated 4LegUpDown_8sense_ver2

足上げセンサ八個版仮

Dependencies:   mbed

Fork of 4LegUpDown_8sense by Bチーム

main.cpp@0:76ba33d3b4a6, 2014-08-20 (annotated)

Committer:
ryuna
Date:
Wed Aug 20 08:59:12 2014 +0000
Revision:
0:76ba33d3b4a6
Child:
1:807091115672
8 limit_switch wheel up

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 0:76ba33d3b4a6 12
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 0:76ba33d3b4a6 20 DigitalIn limit[8] = {p5,p6,p7,p8,p9,p10,p11,p12 };
ryuna 0:76ba33d3b4a6 21 DigitalOut _doLed[4] = {LED1, LED2, LED3, LED4 };
ryuna 0:76ba33d3b4a6 22
ryuna 0:76ba33d3b4a6 23 BusOut _boMotor(p13, p14, p15, p16, p17, p18, p19, p20 );//four motors
ryuna 0:76ba33d3b4a6 24 //{left front, right front, left back ，right back } in sequence?
ryuna 0:76ba33d3b4a6 25 PwmOut _poPwm[4] = {p21, p22, p23, p24 };
ryuna 0:76ba33d3b4a6 26
ryuna 0:76ba33d3b4a6 27 /**more use binary number
ryuna 0:76ba33d3b4a6 28 0xA0 = 10100000
ryuna 0:76ba33d3b4a6 29 0x50 = 01010000
ryuna 0:76ba33d3b4a6 30 0x0A = 00001010
ryuna 0:76ba33d3b4a6 31 0x05 = 00000101
ryuna 0:76ba33d3b4a6 32 **/
ryuna 0:76ba33d3b4a6 33
ryuna 0:76ba33d3b4a6 34 volatile bool bWait[4] = {0};
ryuna 0:76ba33d3b4a6 35
ryuna 0:76ba33d3b4a6 36 void _atWaitStop0(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 37 //make delay time
ryuna 0:76ba33d3b4a6 38 bWait[0] = 0;
ryuna 0:76ba33d3b4a6 39
ryuna 0:76ba33d3b4a6 40 }
ryuna 0:76ba33d3b4a6 41
ryuna 0:76ba33d3b4a6 42 void _atWaitStop1(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 43 //make delay time
ryuna 0:76ba33d3b4a6 44 bWait[1] = 0;
ryuna 0:76ba33d3b4a6 45
ryuna 0:76ba33d3b4a6 46 }
ryuna 0:76ba33d3b4a6 47
ryuna 0:76ba33d3b4a6 48 void _atWaitStop2(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 49 //make delay time
ryuna 0:76ba33d3b4a6 50 bWait[2] = 0;
ryuna 0:76ba33d3b4a6 51
ryuna 0:76ba33d3b4a6 52 }
ryuna 0:76ba33d3b4a6 53
ryuna 0:76ba33d3b4a6 54 void _atWaitStop3(){//atatch Time Stop0
ryuna 0:76ba33d3b4a6 55 //make delay time
ryuna 0:76ba33d3b4a6 56 bWait[3] = 0;
ryuna 0:76ba33d3b4a6 57
ryuna 0:76ba33d3b4a6 58 }
ryuna 0:76ba33d3b4a6 59
ryuna 0:76ba33d3b4a6 60
ryuna 0:76ba33d3b4a6 61
ryuna 0:76ba33d3b4a6 62 void _atMotorStop0(){//atatch Motor Stop0
ryuna 0:76ba33d3b4a6 63
ryuna 0:76ba33d3b4a6 64 _boMotor = _boMotor & 0x7F;
ryuna 0:76ba33d3b4a6 65 //01111111 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 66 //left front leg up
ryuna 0:76ba33d3b4a6 67 _doLed[0] = 0;
ryuna 0:76ba33d3b4a6 68 }
ryuna 0:76ba33d3b4a6 69
ryuna 0:76ba33d3b4a6 70
ryuna 0:76ba33d3b4a6 71 void _atMotorStop1(){
ryuna 0:76ba33d3b4a6 72 _toWait[0].attach(&_atWaitStop0,WAIT_TIME);
ryuna 0:76ba33d3b4a6 73 _boMotor = _boMotor & 0xBF;
ryuna 0:76ba33d3b4a6 74 //10111111 　指定ピンのみ変更
ryuna 0:76ba33d3b4a6 75 //left front leg down
ryuna 0:76ba33d3b4a6 76 _doLed[1] = 0;
ryuna 0:76ba33d3b4a6 77
ryuna 0:76ba33d3b4a6 78 }
ryuna 0:76ba33d3b4a6 79
ryuna 0:76ba33d3b4a6 80 void _atMotorStop2(){
ryuna 0:76ba33d3b4a6 81
ryuna 0:76ba33d3b4a6 82 _boMotor = _boMotor & 0xF7;
ryuna 0:76ba33d3b4a6 83 //11110111 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 84 //left back leg up
ryuna 0:76ba33d3b4a6 85 _doLed[2] = 0;
ryuna 0:76ba33d3b4a6 86
ryuna 0:76ba33d3b4a6 87 }
ryuna 0:76ba33d3b4a6 88
ryuna 0:76ba33d3b4a6 89 void _atMotorStop3(){
ryuna 0:76ba33d3b4a6 90 _toWait[1].attach(&_atWaitStop1,WAIT_TIME);
ryuna 0:76ba33d3b4a6 91 _boMotor = _boMotor & 0xFB;
ryuna 0:76ba33d3b4a6 92 //11111011 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 93 //left back leg down
ryuna 0:76ba33d3b4a6 94 _doLed[3] = 0;
ryuna 0:76ba33d3b4a6 95
ryuna 0:76ba33d3b4a6 96 }
ryuna 0:76ba33d3b4a6 97 void _atMotorStop4(){//atatch Motor Stop0
ryuna 0:76ba33d3b4a6 98
ryuna 0:76ba33d3b4a6 99 _boMotor = _boMotor & 0xDF;
ryuna 0:76ba33d3b4a6 100 //11011111 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 101 //right front leg up
ryuna 0:76ba33d3b4a6 102
ryuna 0:76ba33d3b4a6 103
ryuna 0:76ba33d3b4a6 104 }
ryuna 0:76ba33d3b4a6 105
ryuna 0:76ba33d3b4a6 106
ryuna 0:76ba33d3b4a6 107 void _atMotorStop5(){
ryuna 0:76ba33d3b4a6 108 _toWait[2].attach(&_atWaitStop2,WAIT_TIME);
ryuna 0:76ba33d3b4a6 109 _boMotor = _boMotor & 0xEF;
ryuna 0:76ba33d3b4a6 110 //11101111 　指定ピンのみ変更
ryuna 0:76ba33d3b4a6 111 //right front leg down
ryuna 0:76ba33d3b4a6 112
ryuna 0:76ba33d3b4a6 113
ryuna 0:76ba33d3b4a6 114 }
ryuna 0:76ba33d3b4a6 115
ryuna 0:76ba33d3b4a6 116 void _atMotorStop6(){
ryuna 0:76ba33d3b4a6 117
ryuna 0:76ba33d3b4a6 118 _boMotor = _boMotor & 0xFD;
ryuna 0:76ba33d3b4a6 119 //11111101 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 120 //right back leg up
ryuna 0:76ba33d3b4a6 121
ryuna 0:76ba33d3b4a6 122 }
ryuna 0:76ba33d3b4a6 123
ryuna 0:76ba33d3b4a6 124 void _atMotorStop7(){
ryuna 0:76ba33d3b4a6 125 _toWait[3].attach(&_atWaitStop3,WAIT_TIME);
ryuna 0:76ba33d3b4a6 126 _boMotor = _boMotor & 0xFE;
ryuna 0:76ba33d3b4a6 127 //11111110 指定ピンのみ変更
ryuna 0:76ba33d3b4a6 128 //right back leg down
ryuna 0:76ba33d3b4a6 129
ryuna 0:76ba33d3b4a6 130
ryuna 0:76ba33d3b4a6 131 }
ryuna 0:76ba33d3b4a6 132
ryuna 0:76ba33d3b4a6 133
ryuna 0:76ba33d3b4a6 134
ryuna 0:76ba33d3b4a6 135 int main() {
ryuna 0:76ba33d3b4a6 136
ryuna 0:76ba33d3b4a6 137 static bool bClim[8] = {1,1,1,1,1,1,1,1};//bool Check limit[]
ryuna 0:76ba33d3b4a6 138 static uint8_t u8Ct1[8] = {0},u8Ct2[8] = {1,1,1,1,1,1,1,1};//uint8_t Count{1,2}[]
ryuna 0:76ba33d3b4a6 139
ryuna 0:76ba33d3b4a6 140 limit[0].mode(PullUp);
ryuna 0:76ba33d3b4a6 141 limit[1].mode(PullUp);
ryuna 0:76ba33d3b4a6 142 limit[2].mode(PullUp);
ryuna 0:76ba33d3b4a6 143 limit[3].mode(PullUp);
ryuna 0:76ba33d3b4a6 144 limit[4].mode(PullUp);
ryuna 0:76ba33d3b4a6 145 limit[5].mode(PullUp);
ryuna 0:76ba33d3b4a6 146 limit[6].mode(PullUp);
ryuna 0:76ba33d3b4a6 147 limit[7].mode(PullUp);
ryuna 0:76ba33d3b4a6 148
ryuna 0:76ba33d3b4a6 149
ryuna 0:76ba33d3b4a6 150 wait(1);
ryuna 0:76ba33d3b4a6 151
ryuna 0:76ba33d3b4a6 152
ryuna 0:76ba33d3b4a6 153 /* pwm level setting */
ryuna 0:76ba33d3b4a6 154 _poPwm[0] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 155 _poPwm[1] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 156 _poPwm[2] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 157 _poPwm[3] = PWM_LEVEL ;
ryuna 0:76ba33d3b4a6 158 /* */
ryuna 0:76ba33d3b4a6 159
ryuna 0:76ba33d3b4a6 160
ryuna 0:76ba33d3b4a6 161 while(1){
ryuna 0:76ba33d3b4a6 162 /*左の方-リミットスイッチ0~3*/
ryuna 0:76ba33d3b4a6 163 if(!limit[0] && !bClim[0]){
ryuna 0:76ba33d3b4a6 164
ryuna 0:76ba33d3b4a6 165 /* 1個目*/
ryuna 0:76ba33d3b4a6 166 if(!bWait[0] && !u8Ct1[0] && !u8Ct1[1]){
ryuna 0:76ba33d3b4a6 167 u8Ct1[0] = 1;
ryuna 0:76ba33d3b4a6 168 //legs up
ryuna 0:76ba33d3b4a6 169 _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
ryuna 0:76ba33d3b4a6 170 _boMotor = _boMotor | 0x80;//10000000
ryuna 0:76ba33d3b4a6 171 _doLed[0] = 1;
ryuna 0:76ba33d3b4a6 172
ryuna 0:76ba33d3b4a6 173 }else if(u8Ct1[0]){
ryuna 0:76ba33d3b4a6 174 u8Ct1[0] = 2;
ryuna 0:76ba33d3b4a6 175
ryuna 0:76ba33d3b4a6 176 }else if((u8Ct1[1] == u8Ct2[0])){
ryuna 0:76ba33d3b4a6 177 u8Ct1[1]=0;
ryuna 0:76ba33d3b4a6 178 u8Ct2[0]=1;
ryuna 0:76ba33d3b4a6 179 //legs down
ryuna 0:76ba33d3b4a6 180 _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
ryuna 0:76ba33d3b4a6 181 _boMotor = _boMotor | 0x40;//0x40 = 01000000
ryuna 0:76ba33d3b4a6 182 _doLed[1] = 1;
ryuna 0:76ba33d3b4a6 183 bWait[0] = 1;
ryuna 0:76ba33d3b4a6 184
ryuna 0:76ba33d3b4a6 185 }else{
ryuna 0:76ba33d3b4a6 186 u8Ct2[0] = 2;
ryuna 0:76ba33d3b4a6 187 }
ryuna 0:76ba33d3b4a6 188 bClim[0] = 1;
ryuna 0:76ba33d3b4a6 189
ryuna 0:76ba33d3b4a6 190 }else if(limit[0]){
ryuna 0:76ba33d3b4a6 191 bClim[0] = 0;
ryuna 0:76ba33d3b4a6 192 }
ryuna 0:76ba33d3b4a6 193
ryuna 0:76ba33d3b4a6 194
ryuna 0:76ba33d3b4a6 195 if(!limit[1] && !bClim[1]){
ryuna 0:76ba33d3b4a6 196
ryuna 0:76ba33d3b4a6 197 /* 2個目*/
ryuna 0:76ba33d3b4a6 198 if(!bWait[0] && !u8Ct1[1] && !u8Ct1[0]){
ryuna 0:76ba33d3b4a6 199 u8Ct1[1] = 1;
ryuna 0:76ba33d3b4a6 200 //legs up
ryuna 0:76ba33d3b4a6 201 _toMotor[0].attach(&_atMotorStop0,MOVE_TIME);
ryuna 0:76ba33d3b4a6 202 _boMotor = _boMotor | 0x80;// 10000000
ryuna 0:76ba33d3b4a6 203 _doLed[0] = 1;
ryuna 0:76ba33d3b4a6 204
ryuna 0:76ba33d3b4a6 205 }else if(u8Ct1[1]){
ryuna 0:76ba33d3b4a6 206 u8Ct1[1] = 2;
ryuna 0:76ba33d3b4a6 207
ryuna 0:76ba33d3b4a6 208 }else if((u8Ct1[0] == u8Ct2[1])){
ryuna 0:76ba33d3b4a6 209 u8Ct1[0]=0;
ryuna 0:76ba33d3b4a6 210 u8Ct2[1]=1;
ryuna 0:76ba33d3b4a6 211 //legs down
ryuna 0:76ba33d3b4a6 212 _toMotor[1].attach(&_atMotorStop1,MOVE_TIME);
ryuna 0:76ba33d3b4a6 213 _boMotor = _boMotor | 0x40;// 01000000
ryuna 0:76ba33d3b4a6 214 _doLed[1] = 1;
ryuna 0:76ba33d3b4a6 215 bWait[0] = 1;
ryuna 0:76ba33d3b4a6 216
ryuna 0:76ba33d3b4a6 217 }else{
ryuna 0:76ba33d3b4a6 218 u8Ct2[1] = 2;
ryuna 0:76ba33d3b4a6 219
ryuna 0:76ba33d3b4a6 220 }
ryuna 0:76ba33d3b4a6 221 bClim[1] = 1;
ryuna 0:76ba33d3b4a6 222
ryuna 0:76ba33d3b4a6 223 }else if(limit[1]){
ryuna 0:76ba33d3b4a6 224 bClim[1] = 0;
ryuna 0:76ba33d3b4a6 225 }
ryuna 0:76ba33d3b4a6 226
ryuna 0:76ba33d3b4a6 227
ryuna 0:76ba33d3b4a6 228 if(!limit[2] && !bClim[2]){
ryuna 0:76ba33d3b4a6 229
ryuna 0:76ba33d3b4a6 230 /* 3個目*/
ryuna 0:76ba33d3b4a6 231 if(!bWait[1] && !u8Ct1[2] && !u8Ct1[3]){
ryuna 0:76ba33d3b4a6 232 u8Ct1[2] = 1;
ryuna 0:76ba33d3b4a6 233 //legs up
ryuna 0:76ba33d3b4a6 234 _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
ryuna 0:76ba33d3b4a6 235 _boMotor = _boMotor | 0x08;//00001000
ryuna 0:76ba33d3b4a6 236 _doLed[2] = 1;
ryuna 0:76ba33d3b4a6 237 }else if(u8Ct1[2]){
ryuna 0:76ba33d3b4a6 238 u8Ct1[2] = 2;
ryuna 0:76ba33d3b4a6 239
ryuna 0:76ba33d3b4a6 240 }else if((u8Ct1[3] == u8Ct2[2])){
ryuna 0:76ba33d3b4a6 241 u8Ct1[3]=0;
ryuna 0:76ba33d3b4a6 242 u8Ct2[2]=1;
ryuna 0:76ba33d3b4a6 243 //legs down
ryuna 0:76ba33d3b4a6 244 _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
ryuna 0:76ba33d3b4a6 245 _boMotor = _boMotor | 0x04;//00000100
ryuna 0:76ba33d3b4a6 246 _doLed[3] = 1;
ryuna 0:76ba33d3b4a6 247 bWait[1] = 1;
ryuna 0:76ba33d3b4a6 248 }else{
ryuna 0:76ba33d3b4a6 249 u8Ct2[2] = 2;
ryuna 0:76ba33d3b4a6 250 }
ryuna 0:76ba33d3b4a6 251
ryuna 0:76ba33d3b4a6 252 bClim[2] = 1;
ryuna 0:76ba33d3b4a6 253 }else if(limit[2]){
ryuna 0:76ba33d3b4a6 254 bClim[2] = 0;
ryuna 0:76ba33d3b4a6 255 }
ryuna 0:76ba33d3b4a6 256
ryuna 0:76ba33d3b4a6 257
ryuna 0:76ba33d3b4a6 258
ryuna 0:76ba33d3b4a6 259 if(!limit[3] && !bClim[3]){
ryuna 0:76ba33d3b4a6 260 /* 4個目*/
ryuna 0:76ba33d3b4a6 261 if(!bWait[1] && !u8Ct1[3] && !u8Ct1[2]){
ryuna 0:76ba33d3b4a6 262 u8Ct1[3] = 1;
ryuna 0:76ba33d3b4a6 263 //leg up
ryuna 0:76ba33d3b4a6 264 _toMotor[2].attach(&_atMotorStop2,MOVE_TIME);
ryuna 0:76ba33d3b4a6 265 _boMotor = _boMotor | 0x08;// 00001000
ryuna 0:76ba33d3b4a6 266 _doLed[2] = 1;
ryuna 0:76ba33d3b4a6 267
ryuna 0:76ba33d3b4a6 268 }else if(u8Ct1[3]){
ryuna 0:76ba33d3b4a6 269 u8Ct1[3] = 2;
ryuna 0:76ba33d3b4a6 270
ryuna 0:76ba33d3b4a6 271 }else if((u8Ct1[2] == u8Ct2[3])){
ryuna 0:76ba33d3b4a6 272 u8Ct1[2]=0;
ryuna 0:76ba33d3b4a6 273 u8Ct2[3]=1;
ryuna 0:76ba33d3b4a6 274 //leg down
ryuna 0:76ba33d3b4a6 275 _toMotor[3].attach(&_atMotorStop3,MOVE_TIME);
ryuna 0:76ba33d3b4a6 276 _boMotor = _boMotor | 0x04;//00000100
ryuna 0:76ba33d3b4a6 277 _doLed[3] = 1;
ryuna 0:76ba33d3b4a6 278 bWait[1] = 1;
ryuna 0:76ba33d3b4a6 279 }else{
ryuna 0:76ba33d3b4a6 280 u8Ct2[3] = 2;
ryuna 0:76ba33d3b4a6 281 }
ryuna 0:76ba33d3b4a6 282
ryuna 0:76ba33d3b4a6 283 bClim[3] = 1;
ryuna 0:76ba33d3b4a6 284 }else if(limit[3]){
ryuna 0:76ba33d3b4a6 285 bClim[3] = 0;
ryuna 0:76ba33d3b4a6 286 }
ryuna 0:76ba33d3b4a6 287
ryuna 0:76ba33d3b4a6 288
ryuna 0:76ba33d3b4a6 289 /*右の方 リミットスイッチ4~7 */
ryuna 0:76ba33d3b4a6 290
ryuna 0:76ba33d3b4a6 291 if(!limit[4] && !bClim[4]){
ryuna 0:76ba33d3b4a6 292
ryuna 0:76ba33d3b4a6 293 /* 5個目*/
ryuna 0:76ba33d3b4a6 294 if(!bWait[2] && !u8Ct1[4] && !u8Ct1[5]){
ryuna 0:76ba33d3b4a6 295 u8Ct1[4] = 1;
ryuna 0:76ba33d3b4a6 296 //leg up
ryuna 0:76ba33d3b4a6 297 _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
ryuna 0:76ba33d3b4a6 298 _boMotor = _boMotor | 0x20;//00100000
ryuna 0:76ba33d3b4a6 299
ryuna 0:76ba33d3b4a6 300 }else if(u8Ct1[4]){
ryuna 0:76ba33d3b4a6 301 u8Ct1[4] = 2;
ryuna 0:76ba33d3b4a6 302
ryuna 0:76ba33d3b4a6 303 }else if((u8Ct1[5] == u8Ct2[4])){
ryuna 0:76ba33d3b4a6 304 u8Ct1[5]=0;
ryuna 0:76ba33d3b4a6 305 u8Ct2[4]=1;
ryuna 0:76ba33d3b4a6 306 //leg down
ryuna 0:76ba33d3b4a6 307 _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
ryuna 0:76ba33d3b4a6 308 _boMotor = _boMotor | 0x10;//0x10 = 00010000
ryuna 0:76ba33d3b4a6 309 bWait[2] = 1;
ryuna 0:76ba33d3b4a6 310
ryuna 0:76ba33d3b4a6 311 }else{
ryuna 0:76ba33d3b4a6 312 u8Ct2[4] = 2;
ryuna 0:76ba33d3b4a6 313 }
ryuna 0:76ba33d3b4a6 314 bClim[4] = 1;
ryuna 0:76ba33d3b4a6 315
ryuna 0:76ba33d3b4a6 316 }else if(limit[4]){
ryuna 0:76ba33d3b4a6 317 bClim[4] = 0;
ryuna 0:76ba33d3b4a6 318 }
ryuna 0:76ba33d3b4a6 319
ryuna 0:76ba33d3b4a6 320
ryuna 0:76ba33d3b4a6 321 if(!limit[5] && !bClim[5]){
ryuna 0:76ba33d3b4a6 322
ryuna 0:76ba33d3b4a6 323 /* 6個目*/
ryuna 0:76ba33d3b4a6 324 if(!bWait[2] && !u8Ct1[5] && !u8Ct1[4]){
ryuna 0:76ba33d3b4a6 325 u8Ct1[5] = 1;
ryuna 0:76ba33d3b4a6 326 //leg up
ryuna 0:76ba33d3b4a6 327 _toMotor[4].attach(&_atMotorStop4,MOVE_TIME);
ryuna 0:76ba33d3b4a6 328 _boMotor = _boMotor | 0x20;// 00100000
ryuna 0:76ba33d3b4a6 329
ryuna 0:76ba33d3b4a6 330 }else if(u8Ct1[5]){
ryuna 0:76ba33d3b4a6 331 u8Ct1[5] = 2;
ryuna 0:76ba33d3b4a6 332
ryuna 0:76ba33d3b4a6 333 }else if((u8Ct1[4] == u8Ct2[5])){
ryuna 0:76ba33d3b4a6 334 u8Ct1[4]=0;
ryuna 0:76ba33d3b4a6 335 u8Ct2[5]=1;
ryuna 0:76ba33d3b4a6 336 //leg down
ryuna 0:76ba33d3b4a6 337 _toMotor[5].attach(&_atMotorStop5,MOVE_TIME);
ryuna 0:76ba33d3b4a6 338 _boMotor = _boMotor | 0x10;// 00010000
ryuna 0:76ba33d3b4a6 339 bWait[2] = 1;
ryuna 0:76ba33d3b4a6 340
ryuna 0:76ba33d3b4a6 341 }else{
ryuna 0:76ba33d3b4a6 342 u8Ct2[5] = 2;
ryuna 0:76ba33d3b4a6 343
ryuna 0:76ba33d3b4a6 344 }
ryuna 0:76ba33d3b4a6 345 bClim[5] = 1;
ryuna 0:76ba33d3b4a6 346
ryuna 0:76ba33d3b4a6 347 }else if(limit[5]){
ryuna 0:76ba33d3b4a6 348 bClim[5] = 0;
ryuna 0:76ba33d3b4a6 349 }
ryuna 0:76ba33d3b4a6 350
ryuna 0:76ba33d3b4a6 351
ryuna 0:76ba33d3b4a6 352 if(!limit[6] && !bClim[6]){
ryuna 0:76ba33d3b4a6 353
ryuna 0:76ba33d3b4a6 354 /* 7個目*/
ryuna 0:76ba33d3b4a6 355 if(!bWait[3] && !u8Ct1[6] && !u8Ct1[7]){
ryuna 0:76ba33d3b4a6 356 u8Ct1[6] = 1;
ryuna 0:76ba33d3b4a6 357 //legs up
ryuna 0:76ba33d3b4a6 358 _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
ryuna 0:76ba33d3b4a6 359 _boMotor = _boMotor | 0x02;//00000010
ryuna 0:76ba33d3b4a6 360
ryuna 0:76ba33d3b4a6 361 }else if(u8Ct1[6]){
ryuna 0:76ba33d3b4a6 362 u8Ct1[6] = 2;
ryuna 0:76ba33d3b4a6 363
ryuna 0:76ba33d3b4a6 364 }else if((u8Ct1[7] == u8Ct2[6])){
ryuna 0:76ba33d3b4a6 365 u8Ct1[7]=0;
ryuna 0:76ba33d3b4a6 366 u8Ct2[6]=1;
ryuna 0:76ba33d3b4a6 367 //leg down
ryuna 0:76ba33d3b4a6 368 _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
ryuna 0:76ba33d3b4a6 369 _boMotor = _boMotor | 0x01;//00000001
ryuna 0:76ba33d3b4a6 370 bWait[3] = 1;
ryuna 0:76ba33d3b4a6 371 }else{
ryuna 0:76ba33d3b4a6 372 u8Ct2[6] = 2;
ryuna 0:76ba33d3b4a6 373 }
ryuna 0:76ba33d3b4a6 374
ryuna 0:76ba33d3b4a6 375 bClim[6] = 1;
ryuna 0:76ba33d3b4a6 376 }else if(limit[6]){
ryuna 0:76ba33d3b4a6 377 bClim[6] = 0;
ryuna 0:76ba33d3b4a6 378 }
ryuna 0:76ba33d3b4a6 379
ryuna 0:76ba33d3b4a6 380
ryuna 0:76ba33d3b4a6 381
ryuna 0:76ba33d3b4a6 382 if(!limit[7] && !bClim[7]){
ryuna 0:76ba33d3b4a6 383 /* 8個目*/
ryuna 0:76ba33d3b4a6 384 if(!bWait[3] && !u8Ct1[7] && !u8Ct1[6]){
ryuna 0:76ba33d3b4a6 385 u8Ct1[7] = 1;
ryuna 0:76ba33d3b4a6 386 //leg up
ryuna 0:76ba33d3b4a6 387 _toMotor[6].attach(&_atMotorStop6,MOVE_TIME);
ryuna 0:76ba33d3b4a6 388 _boMotor = _boMotor | 0x02;// 00000010
ryuna 0:76ba33d3b4a6 389 //_doLed[6] = 1;
ryuna 0:76ba33d3b4a6 390
ryuna 0:76ba33d3b4a6 391 }else if(u8Ct1[7]){
ryuna 0:76ba33d3b4a6 392 u8Ct1[7] = 2;
ryuna 0:76ba33d3b4a6 393
ryuna 0:76ba33d3b4a6 394 }else if((u8Ct1[6] == u8Ct2[7])){
ryuna 0:76ba33d3b4a6 395 u8Ct1[6]=0;
ryuna 0:76ba33d3b4a6 396 u8Ct2[7]=1;
ryuna 0:76ba33d3b4a6 397 //leg down
ryuna 0:76ba33d3b4a6 398 _toMotor[7].attach(&_atMotorStop7,MOVE_TIME);
ryuna 0:76ba33d3b4a6 399 _boMotor = _boMotor | 0x01;//00000001
ryuna 0:76ba33d3b4a6 400 bWait[3] = 1;
ryuna 0:76ba33d3b4a6 401
ryuna 0:76ba33d3b4a6 402 }else{
ryuna 0:76ba33d3b4a6 403 u8Ct2[7] = 2;
ryuna 0:76ba33d3b4a6 404 }
ryuna 0:76ba33d3b4a6 405
ryuna 0:76ba33d3b4a6 406 bClim[7] = 1;
ryuna 0:76ba33d3b4a6 407 }else if(limit[7]){
ryuna 0:76ba33d3b4a6 408 bClim[7] = 0;
ryuna 0:76ba33d3b4a6 409 }
ryuna 0:76ba33d3b4a6 410
ryuna 0:76ba33d3b4a6 411 }
ryuna 0:76ba33d3b4a6 412 }