Bチーム / Mbed 2 deprecated 4LegUpDown_8sense_ver2

足上げセンサ八個版仮

Dependencies:   mbed

Fork of 4LegUpDown_8sense by Bチーム

main.cpp@11:748134b48d24, 2014-09-27 (annotated)

Committer:
ryuna
Date:
Sat Sep 27 12:07:31 2014 +0000
Revision:
11:748134b48d24
Parent:
10:87c6b540b3b3
Child:
12:8ff721b67e49
?????????????????????????????????????????

Who changed what in which revision?

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