Bチーム / Mbed 2 deprecated 4LegUpDown_8sense_ver2

足上げセンサ八個版仮

Dependencies:   mbed

Fork of 4LegUpDown_8sense by Bチーム

main.cpp@9:4b15d9a3e468, 2014-09-19 (annotated)

Committer:
ryuna
Date:
Fri Sep 19 05:01:36 2014 +0000
Revision:
9:4b15d9a3e468
Parent:
8:36a85b66796f
Child:
10:87c6b540b3b3
??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 8:36a85b66796f 1 /*
ryuna 8:36a85b66796f 2 *覚え書き
ryuna 8:36a85b66796f 3 *
ryuna 8:36a85b66796f 4 *mbedのピン配置に対するこのプログラムのピンの利用
ryuna 9:4b15d9a3e468 5 *p5 ~ p12 Motor ...BusOut :Motor moved
ryuna 9:4b15d9a3e468 6 *p13 ~ p20 Photo ...BusIn :Photo Sensor
ryuna 9:4b15d9a3e468 7 *p21 ~ p24 Pwm ...PwmOut :Motor Pwm Signal
ryuna 9:4b15d9a3e468 8 *p25 ~ p28 Tempra...BusOut :Motor Tempra Moved
ryuna 9:4b15d9a3e468 9 *p29,p30 Sw[2] ...DigitalIn :Swich Multi Use
ryuna 9:4b15d9a3e468 10 *LED1 ~ LED4 Led ...DigitalOut :Led Brinked
ryuna 9:4b15d9a3e468 11 *
ryuna 9:4b15d9a3e468 12 *
ryuna 9:4b15d9a3e468 13 *@file
ryuna 9:4b15d9a3e468 14 *
ryuna 9:4b15d9a3e468 15 *My program.
ryuna 8:36a85b66796f 16 *
ryuna 8:36a85b66796f 17 *
ryuna 8:36a85b66796f 18 *
ryuna 8:36a85b66796f 19 *
ryuna 8:36a85b66796f 20 */
ryuna 8:36a85b66796f 21
ryuna 0:76ba33d3b4a6 22 #include "mbed.h"
ryuna 0:76ba33d3b4a6 23
ryuna 9:4b15d9a3e468 24
ryuna 9:4b15d9a3e468 25 #define MOTOR_NUM 8//モーター，センサーの数
ryuna 9:4b15d9a3e468 26 #define MOVE_TIME 0.090//足を上げる時間
ryuna 9:4b15d9a3e468 27 #define MOVE_DOWN_TIME 0.10//足を下げる時間
ryuna 8:36a85b66796f 28 #define TEMPRA_TIME 4.0 //ワンショットタイマー用時間設定
ryuna 9:4b15d9a3e468 29 #define WAIT_TIME 0.010//上げ下げの待ち時間
ryuna 9:4b15d9a3e468 30 #define PWM_LEVEL 1.0//モーターの回転速度のPWM
ryuna 9:4b15d9a3e468 31
ryuna 0:76ba33d3b4a6 32
ryuna 0:76ba33d3b4a6 33 Serial pc(USBTX,USBRX);
ryuna 0:76ba33d3b4a6 34
ryuna 8:36a85b66796f 35 BusIn Photo(p5,p6,p7,p8,p9,p10,p11,p12);
ryuna 8:36a85b66796f 36 DigitalIn Sw[2] = {p29,p30};
ryuna 1:807091115672 37
ryuna 3:cd7d3613e8e4 38 DigitalOut Led[4] = {LED1, LED2, LED3, LED4 };
ryuna 3:cd7d3613e8e4 39 BusOut Motor(p13, p14, p15, p16, p17, p18, p19, p20 );//{left front, right front, left back ，right back }
ryuna 4:04f2479b83c1 40 BusOut Tempra(p25,p26,p27,p28);
ryuna 9:4b15d9a3e468 41 PwmOut Pwm[4] = {p21, p22, p23, p24 };
ryuna 1:807091115672 42
ryuna 3:cd7d3613e8e4 43 Timeout StopTimer[8];
ryuna 6:6664c6aa67d3 44 Timeout StopTempra;
ryuna 8:36a85b66796f 45 Timeout StopWait[4];
ryuna 8:36a85b66796f 46 Timer TempraTimer;
ryuna 8:36a85b66796f 47
ryuna 0:76ba33d3b4a6 48
ryuna 3:cd7d3613e8e4 49
ryuna 8:36a85b66796f 50 volatile bool MotorState[4] = {0},Flag[4] = {0},FlagAdd[4] = {0},And[4] = {0},Wait[4] = {0};;
ryuna 9:4b15d9a3e468 51 volatile uint8_t Way = 0, DownCount = 0;
ryuna 8:36a85b66796f 52 volatile bool TempraFlag = 1;
ryuna 3:cd7d3613e8e4 53
ryuna 1:807091115672 54 /*
ryuna 7:583ad8798bf1 55 Motor = {
ryuna 9:4b15d9a3e468 56 LeftFrontUp, LeftFrontDown,
ryuna 1:807091115672 57 RightFrontUp, RightFrontDown,
ryuna 9:4b15d9a3e468 58 LeftBackUp, LeftBackDown,
ryuna 9:4b15d9a3e468 59 RightBackUp, RightBackDown
ryuna 1:807091115672 60 };
ryuna 1:807091115672 61 */
ryuna 1:807091115672 62
ryuna 3:cd7d3613e8e4 63
ryuna 9:4b15d9a3e468 64 void WaitStop0()
ryuna 9:4b15d9a3e468 65 {
ryuna 8:36a85b66796f 66 Wait[0] = 0;
ryuna 8:36a85b66796f 67 }
ryuna 9:4b15d9a3e468 68 void WaitStop1()
ryuna 9:4b15d9a3e468 69 {
ryuna 8:36a85b66796f 70 Wait[1] = 0;
ryuna 8:36a85b66796f 71 }
ryuna 9:4b15d9a3e468 72 void WaitStop2()
ryuna 9:4b15d9a3e468 73 {
ryuna 8:36a85b66796f 74 Wait[2] = 0;
ryuna 8:36a85b66796f 75 }
ryuna 9:4b15d9a3e468 76 void WaitStop3()
ryuna 9:4b15d9a3e468 77 {
ryuna 8:36a85b66796f 78 Wait[3] = 0;
ryuna 9:4b15d9a3e468 79 }
ryuna 9:4b15d9a3e468 80 void MotorStop0()
ryuna 9:4b15d9a3e468 81 {
ryuna 9:4b15d9a3e468 82 Motor = Motor & 0x7F;
ryuna 8:36a85b66796f 83 }
ryuna 8:36a85b66796f 84
ryuna 3:cd7d3613e8e4 85
ryuna 3:cd7d3613e8e4 86
ryuna 9:4b15d9a3e468 87 void MotorStop1()
ryuna 9:4b15d9a3e468 88 {
ryuna 8:36a85b66796f 89 Motor = Motor & 0xBF;
ryuna 9:4b15d9a3e468 90 StopWait[0].attach(&WaitStop0,WAIT_TIME);
ryuna 3:cd7d3613e8e4 91 }
ryuna 9:4b15d9a3e468 92 void MotorStop2()
ryuna 9:4b15d9a3e468 93 {
ryuna 3:cd7d3613e8e4 94 Motor = Motor & 0xF7;
ryuna 3:cd7d3613e8e4 95 }
ryuna 9:4b15d9a3e468 96 void MotorStop3()
ryuna 9:4b15d9a3e468 97 {
ryuna 3:cd7d3613e8e4 98 Motor = Motor & 0xFB;
ryuna 8:36a85b66796f 99 StopWait[1].attach(&WaitStop1,WAIT_TIME);
ryuna 3:cd7d3613e8e4 100 }
ryuna 9:4b15d9a3e468 101 void MotorStop4()
ryuna 9:4b15d9a3e468 102 {
ryuna 3:cd7d3613e8e4 103 Motor = Motor & 0xDF;
ryuna 3:cd7d3613e8e4 104 }
ryuna 9:4b15d9a3e468 105 void MotorStop5()
ryuna 9:4b15d9a3e468 106 {
ryuna 9:4b15d9a3e468 107 Motor = Motor & 0xEF;
ryuna 8:36a85b66796f 108 StopWait[2].attach(&WaitStop2,WAIT_TIME);
ryuna 3:cd7d3613e8e4 109 }
ryuna 9:4b15d9a3e468 110 void MotorStop6()
ryuna 9:4b15d9a3e468 111 {
ryuna 3:cd7d3613e8e4 112 Motor = Motor & 0xFD;
ryuna 3:cd7d3613e8e4 113 }
ryuna 9:4b15d9a3e468 114 void MotorStop7()
ryuna 9:4b15d9a3e468 115 {
ryuna 3:cd7d3613e8e4 116 Motor = Motor & 0xFE;
ryuna 8:36a85b66796f 117 StopWait[3].attach(&WaitStop3,WAIT_TIME);
ryuna 3:cd7d3613e8e4 118 }
ryuna 9:4b15d9a3e468 119
ryuna 9:4b15d9a3e468 120
ryuna 9:4b15d9a3e468 121 void TempraStop()
ryuna 9:4b15d9a3e468 122 {
ryuna 9:4b15d9a3e468 123 Tempra = 0x00;
ryuna 3:cd7d3613e8e4 124 }
ryuna 9:4b15d9a3e468 125
ryuna 9:4b15d9a3e468 126 void Photo0() //zero Up
ryuna 9:4b15d9a3e468 127 {
ryuna 9:4b15d9a3e468 128 if(Way == 0) {
ryuna 3:cd7d3613e8e4 129 Way = 1;
ryuna 3:cd7d3613e8e4 130 }
ryuna 9:4b15d9a3e468 131 if((Way == 1)&&!Wait[0]) {
ryuna 9:4b15d9a3e468 132 if(!MotorState[0]) {
ryuna 3:cd7d3613e8e4 133 StopTimer[0].attach(&MotorStop0,MOVE_TIME);
ryuna 9:4b15d9a3e468 134 Motor = Motor | 0x80;//10000000
ryuna 9:4b15d9a3e468 135 MotorState[0] = 1;
ryuna 3:cd7d3613e8e4 136 Led[0]= 1;
ryuna 9:4b15d9a3e468 137 } else {
ryuna 3:cd7d3613e8e4 138 Flag[0] = 1;
ryuna 3:cd7d3613e8e4 139 }
ryuna 8:36a85b66796f 140 }
ryuna 3:cd7d3613e8e4 141 }
ryuna 9:4b15d9a3e468 142 void Photo1() //one Down
ryuna 9:4b15d9a3e468 143 {
ryuna 9:4b15d9a3e468 144 if(Way == 1) {
ryuna 9:4b15d9a3e468 145 if(Flag[0]&&!FlagAdd[0]) {
ryuna 3:cd7d3613e8e4 146 FlagAdd[0] = 1;
ryuna 9:4b15d9a3e468 147 } else if(MotorState[0]) {
ryuna 3:cd7d3613e8e4 148 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 149 Motor = Motor | 0x40;//0x40 = 01000000
ryuna 3:cd7d3613e8e4 150 MotorState[0] = 0;
ryuna 3:cd7d3613e8e4 151 Flag[0] = 0;
ryuna 6:6664c6aa67d3 152 FlagAdd[0] = 0;
ryuna 3:cd7d3613e8e4 153 Led[0] = 0;
ryuna 3:cd7d3613e8e4 154 And[0] = 1;
ryuna 8:36a85b66796f 155 Wait[0] = 1;
ryuna 3:cd7d3613e8e4 156 }
ryuna 3:cd7d3613e8e4 157 }
ryuna 3:cd7d3613e8e4 158 }
ryuna 9:4b15d9a3e468 159 void Photo2() //two Up
ryuna 9:4b15d9a3e468 160 {
ryuna 9:4b15d9a3e468 161 if((Way == 1)&&!Wait[1]) {
ryuna 9:4b15d9a3e468 162 if(!MotorState[1]) {
ryuna 3:cd7d3613e8e4 163 StopTimer[2].attach(&MotorStop2,MOVE_TIME);
ryuna 9:4b15d9a3e468 164 Motor = Motor | 0x08;
ryuna 9:4b15d9a3e468 165 MotorState[1] = 1;
ryuna 3:cd7d3613e8e4 166 Led[1] = 1;
ryuna 9:4b15d9a3e468 167 } else {
ryuna 3:cd7d3613e8e4 168 Flag[1] = 1;
ryuna 3:cd7d3613e8e4 169 }
ryuna 8:36a85b66796f 170 }
ryuna 3:cd7d3613e8e4 171 }
ryuna 9:4b15d9a3e468 172 void Photo3() //three Down
ryuna 9:4b15d9a3e468 173 {
ryuna 9:4b15d9a3e468 174 if(Way == 1) {
ryuna 9:4b15d9a3e468 175 if(Flag[1]&&!FlagAdd[1]) {
ryuna 3:cd7d3613e8e4 176 FlagAdd[1] = 1;
ryuna 9:4b15d9a3e468 177 } else if(MotorState[1]) {
ryuna 3:cd7d3613e8e4 178 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 179 Motor = Motor | 0x04;
ryuna 3:cd7d3613e8e4 180 MotorState[1] = 0;
ryuna 3:cd7d3613e8e4 181 Flag[1] = 0;
ryuna 6:6664c6aa67d3 182 FlagAdd[1] = 0;
ryuna 3:cd7d3613e8e4 183 Led[1] = 0;
ryuna 3:cd7d3613e8e4 184 And[1] = 1;
ryuna 8:36a85b66796f 185 Wait[1] = 1;
ryuna 3:cd7d3613e8e4 186 }
ryuna 9:4b15d9a3e468 187 }
ryuna 3:cd7d3613e8e4 188 }
ryuna 9:4b15d9a3e468 189 void Photo4() //four Up
ryuna 9:4b15d9a3e468 190 {
ryuna 9:4b15d9a3e468 191 if(Way == 0) {
ryuna 3:cd7d3613e8e4 192 Way = 1;
ryuna 9:4b15d9a3e468 193 }
ryuna 9:4b15d9a3e468 194 if((Way == 1)&&!Wait[2]) {
ryuna 9:4b15d9a3e468 195 if(!MotorState[2]) {
ryuna 3:cd7d3613e8e4 196 StopTimer[4].attach(&MotorStop4,MOVE_TIME);
ryuna 9:4b15d9a3e468 197 Motor = Motor | 0x20;//00100000
ryuna 9:4b15d9a3e468 198 MotorState[2] = 1;
ryuna 3:cd7d3613e8e4 199 Led[2] = 1;
ryuna 9:4b15d9a3e468 200 } else {
ryuna 3:cd7d3613e8e4 201 Flag[2] = 1;
ryuna 3:cd7d3613e8e4 202 }
ryuna 8:36a85b66796f 203 }
ryuna 3:cd7d3613e8e4 204 }
ryuna 9:4b15d9a3e468 205 void Photo5() //five Down
ryuna 9:4b15d9a3e468 206 {
ryuna 9:4b15d9a3e468 207 if(Way == 1) {
ryuna 9:4b15d9a3e468 208 if(Flag[2]&&!FlagAdd[2]) {
ryuna 3:cd7d3613e8e4 209 FlagAdd[2] = 1;
ryuna 9:4b15d9a3e468 210 } else if(MotorState[2]) {
ryuna 3:cd7d3613e8e4 211 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 212 Motor = Motor | 0x10;
ryuna 3:cd7d3613e8e4 213 MotorState[2] = 0;
ryuna 3:cd7d3613e8e4 214 Flag[2] = 0;
ryuna 6:6664c6aa67d3 215 FlagAdd[2] = 0;
ryuna 3:cd7d3613e8e4 216 Led[2] = 0;
ryuna 3:cd7d3613e8e4 217 And[2] = 1;
ryuna 8:36a85b66796f 218 Wait[2] = 1;
ryuna 3:cd7d3613e8e4 219 }
ryuna 9:4b15d9a3e468 220 }
ryuna 3:cd7d3613e8e4 221 }
ryuna 9:4b15d9a3e468 222 void Photo6() //six Up
ryuna 9:4b15d9a3e468 223 {
ryuna 9:4b15d9a3e468 224 if((Way == 1)&&!Wait[3]) {
ryuna 9:4b15d9a3e468 225 if(!MotorState[3]) {
ryuna 3:cd7d3613e8e4 226 StopTimer[6].attach(&MotorStop6,MOVE_TIME);
ryuna 9:4b15d9a3e468 227 Motor = Motor | 0x02;
ryuna 3:cd7d3613e8e4 228 MotorState[3] = 1;
ryuna 9:4b15d9a3e468 229 Led[3] = 1;
ryuna 9:4b15d9a3e468 230 } else {
ryuna 3:cd7d3613e8e4 231 Flag[3] = 1;
ryuna 3:cd7d3613e8e4 232 }
ryuna 8:36a85b66796f 233 }
ryuna 3:cd7d3613e8e4 234 }
ryuna 9:4b15d9a3e468 235 void Photo7() //seven Down
ryuna 9:4b15d9a3e468 236 {
ryuna 9:4b15d9a3e468 237 if(Way == 1) {
ryuna 9:4b15d9a3e468 238 if(Flag[3]&&!FlagAdd[3]) {
ryuna 3:cd7d3613e8e4 239 FlagAdd[3] = 1;
ryuna 9:4b15d9a3e468 240 } else if(MotorState[3]) {
ryuna 3:cd7d3613e8e4 241 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 242 Motor = Motor | 0x01;
ryuna 3:cd7d3613e8e4 243 MotorState[3] = 0;
ryuna 3:cd7d3613e8e4 244 Flag[3] = 0;
ryuna 6:6664c6aa67d3 245 FlagAdd[3] = 0;
ryuna 3:cd7d3613e8e4 246 Led[3] = 0;
ryuna 3:cd7d3613e8e4 247 And[3] = 1;
ryuna 8:36a85b66796f 248 Wait[3] = 1;
ryuna 3:cd7d3613e8e4 249 }
ryuna 9:4b15d9a3e468 250 }
ryuna 3:cd7d3613e8e4 251 }
ryuna 3:cd7d3613e8e4 252
ryuna 9:4b15d9a3e468 253
ryuna 9:4b15d9a3e468 254 void Reset()
ryuna 9:4b15d9a3e468 255 {
ryuna 3:cd7d3613e8e4 256 int i;
ryuna 3:cd7d3613e8e4 257
ryuna 9:4b15d9a3e468 258 if(MotorState[0]) {
ryuna 3:cd7d3613e8e4 259 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 260 Motor = Motor | 0x40;//
ryuna 3:cd7d3613e8e4 261 }
ryuna 9:4b15d9a3e468 262 if(MotorState[1]) {
ryuna 3:cd7d3613e8e4 263 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 264 Motor = Motor | 0x04;//00000100
ryuna 3:cd7d3613e8e4 265 }
ryuna 9:4b15d9a3e468 266 if(MotorState[2]) {
ryuna 3:cd7d3613e8e4 267 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 268 Motor = Motor | 0x10;//00010000
ryuna 3:cd7d3613e8e4 269 }
ryuna 9:4b15d9a3e468 270 if(MotorState[3]) {
ryuna 3:cd7d3613e8e4 271 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 9:4b15d9a3e468 272 Motor = Motor | 0x01;//00000001
ryuna 3:cd7d3613e8e4 273 }
ryuna 9:4b15d9a3e468 274 for(i = 0; i < 4; i++) {
ryuna 4:04f2479b83c1 275 Led[i] = 0;
ryuna 9:4b15d9a3e468 276 Flag[i] = 0;
ryuna 3:cd7d3613e8e4 277 FlagAdd[i] = 0;
ryuna 3:cd7d3613e8e4 278 MotorState[i] = 0;
ryuna 4:04f2479b83c1 279 And[i] = 0;
ryuna 1:807091115672 280 }
ryuna 9:4b15d9a3e468 281
ryuna 4:04f2479b83c1 282 Way = 0;
ryuna 9:4b15d9a3e468 283 DownCount = 0;
ryuna 8:36a85b66796f 284 TempraFlag = 1;
ryuna 9:4b15d9a3e468 285
ryuna 9:4b15d9a3e468 286 }
ryuna 9:4b15d9a3e468 287
ryuna 9:4b15d9a3e468 288 void GainSetting ()
ryuna 9:4b15d9a3e468 289 {
ryuna 9:4b15d9a3e468 290
ryuna 9:4b15d9a3e468 291 bool CkFlag[8] = {1,1,1,1,1,1,1,1},PhotoCk[8] = {0};
ryuna 9:4b15d9a3e468 292
ryuna 9:4b15d9a3e468 293 Reset();
ryuna 9:4b15d9a3e468 294 Tempra = 0;
ryuna 9:4b15d9a3e468 295 TempraFlag = 0;
ryuna 9:4b15d9a3e468 296 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 9:4b15d9a3e468 297
ryuna 9:4b15d9a3e468 298 while(1) {
ryuna 9:4b15d9a3e468 299 if(!Sw[0]) {
ryuna 9:4b15d9a3e468 300 while(1) {
ryuna 9:4b15d9a3e468 301 PhotoCk[0] = ((~Photo)&0x01);
ryuna 9:4b15d9a3e468 302 PhotoCk[1] = ((~Photo)&0x02) >> 1;
ryuna 9:4b15d9a3e468 303 PhotoCk[2] = ((~Photo)&0x04) >> 2;
ryuna 9:4b15d9a3e468 304 PhotoCk[3] = ((~Photo)&0x08) >> 3;
ryuna 9:4b15d9a3e468 305 if(PhotoCk[0]&&CkFlag[0]) {
ryuna 9:4b15d9a3e468 306 Led[0] = 0;
ryuna 9:4b15d9a3e468 307 CkFlag[0] = 0;
ryuna 9:4b15d9a3e468 308 } else if(!PhotoCk[0]) {
ryuna 9:4b15d9a3e468 309 CkFlag[0] = 1;
ryuna 9:4b15d9a3e468 310 Led[0] = 1;
ryuna 9:4b15d9a3e468 311 }
ryuna 9:4b15d9a3e468 312
ryuna 9:4b15d9a3e468 313 if(PhotoCk[1]&&CkFlag[1]) {
ryuna 9:4b15d9a3e468 314 Led[1] = 0;
ryuna 9:4b15d9a3e468 315 CkFlag[1] = 0;
ryuna 9:4b15d9a3e468 316 } else if(!PhotoCk[1]) {
ryuna 9:4b15d9a3e468 317 CkFlag[1] = 1;
ryuna 9:4b15d9a3e468 318 Led[1] = 1;
ryuna 9:4b15d9a3e468 319 }
ryuna 9:4b15d9a3e468 320 if(PhotoCk[2]&&CkFlag[2]) {
ryuna 9:4b15d9a3e468 321 Led[2] = 0;
ryuna 9:4b15d9a3e468 322 CkFlag[2] = 0;
ryuna 9:4b15d9a3e468 323 } else if(!PhotoCk[2]) {
ryuna 9:4b15d9a3e468 324 CkFlag[2] = 1;
ryuna 9:4b15d9a3e468 325 Led[2] = 1;
ryuna 9:4b15d9a3e468 326 }
ryuna 9:4b15d9a3e468 327 if(PhotoCk[3]&&CkFlag[3]) {
ryuna 9:4b15d9a3e468 328 Led[3] = 0;
ryuna 9:4b15d9a3e468 329 CkFlag[3] = 0;
ryuna 9:4b15d9a3e468 330 } else if(!PhotoCk[3]) {
ryuna 9:4b15d9a3e468 331 CkFlag[3] = 1;
ryuna 9:4b15d9a3e468 332 Led[3] = 1;
ryuna 9:4b15d9a3e468 333 }
ryuna 9:4b15d9a3e468 334 if(!Sw[1]) break;
ryuna 9:4b15d9a3e468 335 }
ryuna 9:4b15d9a3e468 336 }
ryuna 9:4b15d9a3e468 337
ryuna 9:4b15d9a3e468 338 PhotoCk[4] = ((~Photo)&0x10) >> 4;
ryuna 9:4b15d9a3e468 339 PhotoCk[5] = ((~Photo)&0x20) >> 5;
ryuna 9:4b15d9a3e468 340 PhotoCk[6] = ((~Photo)&0x40) >> 6;
ryuna 9:4b15d9a3e468 341 PhotoCk[7] = ((~Photo)&0x80) >> 7;
ryuna 9:4b15d9a3e468 342
ryuna 9:4b15d9a3e468 343 if(PhotoCk[4]&&CkFlag[4]) {
ryuna 9:4b15d9a3e468 344 Led[0] = 0;
ryuna 9:4b15d9a3e468 345 CkFlag[4] = 0;
ryuna 9:4b15d9a3e468 346 } else if(!PhotoCk[4]) {
ryuna 9:4b15d9a3e468 347 CkFlag[4] = 1;
ryuna 9:4b15d9a3e468 348 Led[0] = 1;
ryuna 9:4b15d9a3e468 349 }
ryuna 9:4b15d9a3e468 350 if(PhotoCk[5]&&CkFlag[5]) {
ryuna 9:4b15d9a3e468 351 Led[1] = 0;
ryuna 9:4b15d9a3e468 352 CkFlag[5] = 0;
ryuna 9:4b15d9a3e468 353 } else if(!PhotoCk[5]) {
ryuna 9:4b15d9a3e468 354 CkFlag[5] = 1;
ryuna 9:4b15d9a3e468 355 Led[1] = 1;
ryuna 9:4b15d9a3e468 356 }
ryuna 9:4b15d9a3e468 357 if(PhotoCk[6]&&CkFlag[6]) {
ryuna 9:4b15d9a3e468 358 Led[2] = 0;
ryuna 9:4b15d9a3e468 359 CkFlag[6] = 0;
ryuna 9:4b15d9a3e468 360 } else if(!PhotoCk[6]) {
ryuna 9:4b15d9a3e468 361 CkFlag[6] = 1;
ryuna 9:4b15d9a3e468 362 Led[2] = 1;
ryuna 9:4b15d9a3e468 363 }
ryuna 9:4b15d9a3e468 364 if(PhotoCk[7]&&CkFlag[7]) {
ryuna 9:4b15d9a3e468 365 Led[3] = 0;
ryuna 9:4b15d9a3e468 366 CkFlag[7] = 0;
ryuna 9:4b15d9a3e468 367 } else if(!PhotoCk[7]) {
ryuna 9:4b15d9a3e468 368 CkFlag[7] = 1;
ryuna 9:4b15d9a3e468 369 Led[3] = 1;
ryuna 9:4b15d9a3e468 370 }
ryuna 9:4b15d9a3e468 371
ryuna 9:4b15d9a3e468 372 }
ryuna 9:4b15d9a3e468 373
ryuna 1:807091115672 374 }
ryuna 0:76ba33d3b4a6 375
ryuna 8:36a85b66796f 376
ryuna 9:4b15d9a3e468 377 void AndCount() //"Legs" Up and Down counting
ryuna 9:4b15d9a3e468 378 {
ryuna 8:36a85b66796f 379
ryuna 9:4b15d9a3e468 380 DownCount ++;
ryuna 6:6664c6aa67d3 381 And[0] = 0 ;
ryuna 6:6664c6aa67d3 382 And[1] = 0 ;
ryuna 6:6664c6aa67d3 383 And[2] = 0 ;
ryuna 6:6664c6aa67d3 384 And[3] = 0 ;
ryuna 9:4b15d9a3e468 385 if(DownCount > 3) {
ryuna 9:4b15d9a3e468 386 DownCount = 0;
ryuna 6:6664c6aa67d3 387 Way = 0;
ryuna 1:807091115672 388 }
ryuna 1:807091115672 389 }
ryuna 1:807091115672 390
ryuna 9:4b15d9a3e468 391 void SetUp()
ryuna 9:4b15d9a3e468 392 {
ryuna 9:4b15d9a3e468 393
ryuna 5:aba9f5e97e03 394 Pwm[0] = PWM_LEVEL ;
ryuna 5:aba9f5e97e03 395 Pwm[1] = PWM_LEVEL ;
ryuna 5:aba9f5e97e03 396 Pwm[2] = PWM_LEVEL ;
ryuna 5:aba9f5e97e03 397 Pwm[3] = PWM_LEVEL ;
ryuna 8:36a85b66796f 398 Photo.mode(PullUp);
ryuna 9:4b15d9a3e468 399 Sw[0].mode(PullUp);
ryuna 8:36a85b66796f 400 Sw[1].mode(PullUp);
ryuna 9:4b15d9a3e468 401
ryuna 8:36a85b66796f 402 TempraTimer.reset();
ryuna 1:807091115672 403 }
ryuna 1:807091115672 404
ryuna 9:4b15d9a3e468 405 int main()
ryuna 9:4b15d9a3e468 406 {
ryuna 9:4b15d9a3e468 407 SetUp();
ryuna 9:4b15d9a3e468 408
ryuna 9:4b15d9a3e468 409 uint8_t Ct = 0,PhotoState = 0,CkPhoto[4] = {0};
ryuna 8:36a85b66796f 410 bool CkFlag[8] = {1,1,1,1,1,1,1,1};
ryuna 8:36a85b66796f 411 void (*MotorMove[8])();
ryuna 9:4b15d9a3e468 412
ryuna 8:36a85b66796f 413 MotorMove[0] = Photo0;
ryuna 8:36a85b66796f 414 MotorMove[1] = Photo1;
ryuna 8:36a85b66796f 415 MotorMove[2] = Photo2;
ryuna 8:36a85b66796f 416 MotorMove[3] = Photo3;
ryuna 8:36a85b66796f 417 MotorMove[4] = Photo4;
ryuna 8:36a85b66796f 418 MotorMove[5] = Photo5;
ryuna 8:36a85b66796f 419 MotorMove[6] = Photo6;
ryuna 8:36a85b66796f 420 MotorMove[7] = Photo7;
ryuna 9:4b15d9a3e468 421
ryuna 9:4b15d9a3e468 422 wait(0.5);
ryuna 9:4b15d9a3e468 423
ryuna 9:4b15d9a3e468 424
ryuna 9:4b15d9a3e468 425 while(1) {
ryuna 9:4b15d9a3e468 426
ryuna 9:4b15d9a3e468 427 PhotoState = ~Photo;
ryuna 9:4b15d9a3e468 428 CkPhoto[0] = PhotoState & 0x03;
ryuna 9:4b15d9a3e468 429 CkPhoto[1] = (PhotoState & 0x0C) >> 2;
ryuna 9:4b15d9a3e468 430 CkPhoto[2] = (PhotoState & 0x30) >> 4;
ryuna 9:4b15d9a3e468 431 CkPhoto[3] = (PhotoState & 0xC0) >> 6 ;
ryuna 9:4b15d9a3e468 432
ryuna 9:4b15d9a3e468 433 if(CkPhoto[0]==0) { //@remarks 右前足
ryuna 8:36a85b66796f 434 CkFlag[0] = 1;
ryuna 8:36a85b66796f 435 CkFlag[1] = 1;
ryuna 9:4b15d9a3e468 436 } else if(CkPhoto[0]<3) {
ryuna 8:36a85b66796f 437 CkPhoto[0] -= 1;
ryuna 9:4b15d9a3e468 438 if(CkFlag[CkPhoto[0]]) {
ryuna 9:4b15d9a3e468 439 (*MotorMove[CkPhoto[0]])();
ryuna 9:4b15d9a3e468 440 CkFlag[CkPhoto[0]] = 0;
ryuna 8:36a85b66796f 441 }
ryuna 8:36a85b66796f 442 }
ryuna 9:4b15d9a3e468 443
ryuna 9:4b15d9a3e468 444 if(CkPhoto[1]==0) { //@remarks 右後足
ryuna 8:36a85b66796f 445 CkFlag[2] = 1;
ryuna 8:36a85b66796f 446 CkFlag[3] = 1;
ryuna 9:4b15d9a3e468 447 } else if(CkPhoto[1]<3) {
ryuna 8:36a85b66796f 448 CkPhoto[1] += 1;
ryuna 9:4b15d9a3e468 449 if(CkFlag[CkPhoto[1]]) {
ryuna 9:4b15d9a3e468 450 (*MotorMove[CkPhoto[1]])();
ryuna 9:4b15d9a3e468 451 CkFlag[CkPhoto[1]] = 0;
ryuna 8:36a85b66796f 452 }
ryuna 8:36a85b66796f 453 }
ryuna 9:4b15d9a3e468 454
ryuna 9:4b15d9a3e468 455 if(CkPhoto[2]==0) { //@remarks 左前足
ryuna 8:36a85b66796f 456 CkFlag[4] = 1;
ryuna 8:36a85b66796f 457 CkFlag[5] = 1;
ryuna 9:4b15d9a3e468 458 } else if(CkPhoto[2]<3) {
ryuna 8:36a85b66796f 459 CkPhoto[2] += 3;
ryuna 9:4b15d9a3e468 460 if(CkFlag[CkPhoto[2]]) {
ryuna 9:4b15d9a3e468 461 (*MotorMove[CkPhoto[2]])();
ryuna 9:4b15d9a3e468 462 CkFlag[CkPhoto[2]] = 0;
ryuna 8:36a85b66796f 463 }
ryuna 8:36a85b66796f 464 }
ryuna 9:4b15d9a3e468 465
ryuna 9:4b15d9a3e468 466 if(CkPhoto[3]==0) { //@remarks 左後足
ryuna 8:36a85b66796f 467 CkFlag[6] = 1;
ryuna 8:36a85b66796f 468 CkFlag[7] = 1;
ryuna 9:4b15d9a3e468 469 } else if(CkPhoto[3]<3) {
ryuna 8:36a85b66796f 470 CkPhoto[3] += 5;
ryuna 9:4b15d9a3e468 471 if(CkFlag[CkPhoto[3]]) {
ryuna 9:4b15d9a3e468 472 (*MotorMove[CkPhoto[3]])();
ryuna 9:4b15d9a3e468 473 CkFlag[CkPhoto[3]] = 0;
ryuna 8:36a85b66796f 474 }
ryuna 8:36a85b66796f 475 }
ryuna 9:4b15d9a3e468 476 if(And[0]&&And[1]&&And[2]&&And[3]) {
ryuna 9:4b15d9a3e468 477 AndCount();
ryuna 9:4b15d9a3e468 478 }
ryuna 9:4b15d9a3e468 479
ryuna 9:4b15d9a3e468 480 if((MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3])&&TempraFlag) {
ryuna 6:6664c6aa67d3 481 Tempra = 0x0A;
ryuna 6:6664c6aa67d3 482 StopTempra.attach(&TempraStop,TEMPRA_TIME);
ryuna 6:6664c6aa67d3 483 }
ryuna 9:4b15d9a3e468 484
ryuna 9:4b15d9a3e468 485 if(!Sw[0]&&!Sw[1]) {
ryuna 8:36a85b66796f 486 Ct = 0;
ryuna 9:4b15d9a3e468 487 while(!Sw[0]&&!Sw[1]&&(Ct <= 100)) {
ryuna 8:36a85b66796f 488 Ct++;
ryuna 8:36a85b66796f 489 }
ryuna 9:4b15d9a3e468 490 if(Ct >= 100) {
ryuna 9:4b15d9a3e468 491 GainSetting();
ryuna 8:36a85b66796f 492 }
ryuna 9:4b15d9a3e468 493
ryuna 9:4b15d9a3e468 494 } else if(!Sw[0]) {
ryuna 8:36a85b66796f 495 Ct = 0;
ryuna 9:4b15d9a3e468 496 while(!Sw[0]&&(Ct <= 100)) {
ryuna 8:36a85b66796f 497 Ct ++;
ryuna 8:36a85b66796f 498 //printf("%d\n",Ct);
ryuna 8:36a85b66796f 499 }
ryuna 9:4b15d9a3e468 500 if(Ct >= 100) {
ryuna 9:4b15d9a3e468 501 Reset();
ryuna 8:36a85b66796f 502 }
ryuna 9:4b15d9a3e468 503
ryuna 9:4b15d9a3e468 504 } else if(!Sw[1]) {
ryuna 8:36a85b66796f 505 Ct = 0;
ryuna 9:4b15d9a3e468 506 while(!Sw[1]&&(Ct <= 100)) {
ryuna 8:36a85b66796f 507 Ct ++;
ryuna 8:36a85b66796f 508 //printf("%d\n",Ct);
ryuna 8:36a85b66796f 509 }
ryuna 9:4b15d9a3e468 510 if(Ct >= 100) {
ryuna 8:36a85b66796f 511 Tempra = 0;
ryuna 9:4b15d9a3e468 512 TempraFlag = 0;
ryuna 8:36a85b66796f 513 }
ryuna 8:36a85b66796f 514 }
ryuna 0:76ba33d3b4a6 515 }
ryuna 8:36a85b66796f 516 }