Bチーム / Mbed 2 deprecated 4LegUpDown_8sense_ver2

足上げセンサ八個版仮

Dependencies:   mbed

Fork of 4LegUpDown_8sense by Bチーム

main.cpp@7:583ad8798bf1, 2014-09-12 (annotated)

Committer:
ryuna
Date:
Fri Sep 12 00:42:47 2014 +0000
Revision:
7:583ad8798bf1
Parent:
6:6664c6aa67d3
Child:
8:36a85b66796f
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:76ba33d3b4a6 1 #include "mbed.h"
ryuna 0:76ba33d3b4a6 2
ryuna 3:cd7d3613e8e4 3 #define MOTOR_NUM 8
ryuna 6:6664c6aa67d3 4 #define MOVE_TIME 0.080
ryuna 6:6664c6aa67d3 5 #define MOVE_DOWN_TIME 0.090
ryuna 6:6664c6aa67d3 6 #define TEMPRA_TIME 5.0
ryuna 3:cd7d3613e8e4 7 #define PWM_LEVEL 1.0
ryuna 0:76ba33d3b4a6 8
ryuna 0:76ba33d3b4a6 9 Serial pc(USBTX,USBRX);
ryuna 0:76ba33d3b4a6 10
ryuna 3:cd7d3613e8e4 11 InterruptIn Photo[8] = {p5,p6,p7,p8,p9,p10,p11,p12};
ryuna 6:6664c6aa67d3 12 //InterruptIn Sw(NC);
ryuna 3:cd7d3613e8e4 13
ryuna 1:807091115672 14
ryuna 3:cd7d3613e8e4 15 DigitalOut Led[4] = {LED1, LED2, LED3, LED4 };
ryuna 3:cd7d3613e8e4 16 BusOut Motor(p13, p14, p15, p16, p17, p18, p19, p20 );//{left front, right front, left back ，right back }
ryuna 4:04f2479b83c1 17 BusOut Tempra(p25,p26,p27,p28);
ryuna 4:04f2479b83c1 18 PwmOut Pwm[4] = {p21, p22, p23, p24 };
ryuna 1:807091115672 19
ryuna 3:cd7d3613e8e4 20 Timeout StopTimer[8];
ryuna 6:6664c6aa67d3 21 Timeout StopTempra;
ryuna 0:76ba33d3b4a6 22
ryuna 3:cd7d3613e8e4 23
ryuna 3:cd7d3613e8e4 24 volatile bool MotorState[4] = {0},Flag[4] = {0},FlagAdd[4] = {0},And[4] = {0};
ryuna 3:cd7d3613e8e4 25 volatile uint8_t Way = 0,UpCt = 0;
ryuna 6:6664c6aa67d3 26 volatile bool TempraFlag = 0;
ryuna 0:76ba33d3b4a6 27
ryuna 6:6664c6aa67d3 28 /*motorstate 上がっているか下がっているか
ryuna 5:aba9f5e97e03 29
ryuna 1:807091115672 30
ryuna 3:cd7d3613e8e4 31 */
ryuna 3:cd7d3613e8e4 32
ryuna 1:807091115672 33 /*
ryuna 7:583ad8798bf1 34 Motor = {
ryuna 1:807091115672 35 LeftFrontUp, LeftFrontDown,
ryuna 1:807091115672 36 RightFrontUp, RightFrontDown,
ryuna 1:807091115672 37 LeftBackUp, LeftBackDown,
ryuna 1:807091115672 38 RightBackUp, RightBackDown
ryuna 1:807091115672 39 };
ryuna 1:807091115672 40 */
ryuna 1:807091115672 41
ryuna 3:cd7d3613e8e4 42
ryuna 3:cd7d3613e8e4 43
ryuna 3:cd7d3613e8e4 44 void MotorStop0(){
ryuna 3:cd7d3613e8e4 45
ryuna 3:cd7d3613e8e4 46 Motor = Motor & 0x7F;
ryuna 3:cd7d3613e8e4 47
ryuna 3:cd7d3613e8e4 48 }
ryuna 3:cd7d3613e8e4 49 void MotorStop1(){
ryuna 3:cd7d3613e8e4 50
ryuna 3:cd7d3613e8e4 51 Motor = Motor & 0xBF;
ryuna 3:cd7d3613e8e4 52
ryuna 3:cd7d3613e8e4 53 }
ryuna 3:cd7d3613e8e4 54
ryuna 3:cd7d3613e8e4 55 void MotorStop2(){
ryuna 3:cd7d3613e8e4 56
ryuna 3:cd7d3613e8e4 57 Motor = Motor & 0xF7;
ryuna 3:cd7d3613e8e4 58
ryuna 3:cd7d3613e8e4 59 }
ryuna 3:cd7d3613e8e4 60 void MotorStop3(){
ryuna 3:cd7d3613e8e4 61
ryuna 3:cd7d3613e8e4 62 Motor = Motor & 0xFB;
ryuna 3:cd7d3613e8e4 63
ryuna 3:cd7d3613e8e4 64 }
ryuna 3:cd7d3613e8e4 65 void MotorStop4(){
ryuna 3:cd7d3613e8e4 66
ryuna 3:cd7d3613e8e4 67 Motor = Motor & 0xDF;
ryuna 3:cd7d3613e8e4 68
ryuna 3:cd7d3613e8e4 69 }
ryuna 3:cd7d3613e8e4 70 void MotorStop5(){
ryuna 3:cd7d3613e8e4 71
ryuna 3:cd7d3613e8e4 72 Motor = Motor & 0xEF;
ryuna 3:cd7d3613e8e4 73
ryuna 3:cd7d3613e8e4 74 }
ryuna 3:cd7d3613e8e4 75 void MotorStop6(){
ryuna 3:cd7d3613e8e4 76
ryuna 3:cd7d3613e8e4 77 Motor = Motor & 0xFD;
ryuna 3:cd7d3613e8e4 78
ryuna 3:cd7d3613e8e4 79 }
ryuna 3:cd7d3613e8e4 80 void MotorStop7(){
ryuna 3:cd7d3613e8e4 81
ryuna 3:cd7d3613e8e4 82 Motor = Motor & 0xFE;
ryuna 3:cd7d3613e8e4 83
ryuna 3:cd7d3613e8e4 84 }
ryuna 3:cd7d3613e8e4 85 void TempraStop(){
ryuna 6:6664c6aa67d3 86 Tempra = 0x00;
ryuna 3:cd7d3613e8e4 87
ryuna 3:cd7d3613e8e4 88 }
ryuna 3:cd7d3613e8e4 89 void FallPhoto0(){//zero
ryuna 3:cd7d3613e8e4 90 //Wayの決定
ryuna 3:cd7d3613e8e4 91 if(Way == 0){
ryuna 3:cd7d3613e8e4 92 Way = 1;
ryuna 3:cd7d3613e8e4 93 }
ryuna 3:cd7d3613e8e4 94
ryuna 3:cd7d3613e8e4 95 if(Way == 1){
ryuna 3:cd7d3613e8e4 96 if(!MotorState[0]){
ryuna 3:cd7d3613e8e4 97
ryuna 3:cd7d3613e8e4 98 StopTimer[0].attach(&MotorStop0,MOVE_TIME);
ryuna 3:cd7d3613e8e4 99 Motor = Motor | 0x80;//10000000
ryuna 3:cd7d3613e8e4 100 MotorState[0] = 1;
ryuna 3:cd7d3613e8e4 101 Led[0]= 1;
ryuna 3:cd7d3613e8e4 102 }else{
ryuna 3:cd7d3613e8e4 103 Flag[0] = 1;
ryuna 3:cd7d3613e8e4 104 }
ryuna 6:6664c6aa67d3 105 }/*else if(Way == 2){
ryuna 3:cd7d3613e8e4 106
ryuna 3:cd7d3613e8e4 107 if(Flag[0]&&!FlagAdd[0]){
ryuna 3:cd7d3613e8e4 108 FlagAdd[0] = 1;
ryuna 5:aba9f5e97e03 109 }else if(MotorState[0]){
ryuna 3:cd7d3613e8e4 110 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 111 Motor = Motor | 0x40;//0x40 = 01000000
ryuna 3:cd7d3613e8e4 112 MotorState[0] = 0;
ryuna 3:cd7d3613e8e4 113 Flag[0] = 0;
ryuna 3:cd7d3613e8e4 114 Led[0] = 0;
ryuna 3:cd7d3613e8e4 115 And[0] = 1;
ryuna 3:cd7d3613e8e4 116
ryuna 3:cd7d3613e8e4 117 }
ryuna 6:6664c6aa67d3 118 }*/
ryuna 3:cd7d3613e8e4 119 }
ryuna 3:cd7d3613e8e4 120
ryuna 3:cd7d3613e8e4 121 void FallPhoto1(){//one
ryuna 3:cd7d3613e8e4 122 //Wayの決定
ryuna 6:6664c6aa67d3 123 /*if(Way ==0){
ryuna 3:cd7d3613e8e4 124 Way = 2;
ryuna 6:6664c6aa67d3 125 }*/
ryuna 3:cd7d3613e8e4 126
ryuna 6:6664c6aa67d3 127 /*if(Way == 2){
ryuna 3:cd7d3613e8e4 128 if(!MotorState[0]){
ryuna 3:cd7d3613e8e4 129
ryuna 3:cd7d3613e8e4 130 StopTimer[0].attach(&MotorStop0,MOVE_TIME);
ryuna 3:cd7d3613e8e4 131 Motor = Motor | 0x80;//10000000
ryuna 3:cd7d3613e8e4 132 MotorState[0] = 1;
ryuna 3:cd7d3613e8e4 133 Led[0] = 1;
ryuna 3:cd7d3613e8e4 134 }else{
ryuna 3:cd7d3613e8e4 135 Flag[0] = 1;
ryuna 3:cd7d3613e8e4 136 }
ryuna 6:6664c6aa67d3 137 }else */
ryuna 6:6664c6aa67d3 138 if(Way == 1){
ryuna 3:cd7d3613e8e4 139
ryuna 3:cd7d3613e8e4 140 if(Flag[0]&&!FlagAdd[0]){
ryuna 3:cd7d3613e8e4 141 FlagAdd[0] = 1;
ryuna 5:aba9f5e97e03 142 }else if(MotorState[0]) {
ryuna 3:cd7d3613e8e4 143 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 144 Motor = Motor | 0x40;//0x40 = 01000000
ryuna 3:cd7d3613e8e4 145 MotorState[0] = 0;
ryuna 3:cd7d3613e8e4 146 Flag[0] = 0;
ryuna 6:6664c6aa67d3 147 FlagAdd[0] = 0;
ryuna 3:cd7d3613e8e4 148 Led[0] = 0;
ryuna 3:cd7d3613e8e4 149 And[0] = 1;
ryuna 3:cd7d3613e8e4 150
ryuna 3:cd7d3613e8e4 151 }
ryuna 3:cd7d3613e8e4 152 }
ryuna 3:cd7d3613e8e4 153
ryuna 3:cd7d3613e8e4 154 }
ryuna 3:cd7d3613e8e4 155 void FallPhoto2(){//two
ryuna 3:cd7d3613e8e4 156 //Wayの決定
ryuna 3:cd7d3613e8e4 157
ryuna 3:cd7d3613e8e4 158 if(Way == 1){
ryuna 3:cd7d3613e8e4 159 if(!MotorState[1]){
ryuna 3:cd7d3613e8e4 160
ryuna 3:cd7d3613e8e4 161 StopTimer[2].attach(&MotorStop2,MOVE_TIME);
ryuna 3:cd7d3613e8e4 162 Motor = Motor | 0x08;
ryuna 3:cd7d3613e8e4 163 MotorState[1] = 1;
ryuna 3:cd7d3613e8e4 164 Led[1] = 1;
ryuna 3:cd7d3613e8e4 165 }else{
ryuna 3:cd7d3613e8e4 166 Flag[1] = 1;
ryuna 3:cd7d3613e8e4 167 }
ryuna 6:6664c6aa67d3 168 }/*else if(Way == 2){
ryuna 3:cd7d3613e8e4 169
ryuna 3:cd7d3613e8e4 170 if(Flag[1]&&!FlagAdd[1]){
ryuna 3:cd7d3613e8e4 171 FlagAdd[1] = 1;
ryuna 5:aba9f5e97e03 172 }else if(MotorState[1]){
ryuna 3:cd7d3613e8e4 173 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 174 Motor = Motor | 0x04;
ryuna 3:cd7d3613e8e4 175 MotorState[1] = 0;
ryuna 3:cd7d3613e8e4 176 Flag[1] = 0;
ryuna 3:cd7d3613e8e4 177 Led[1] = 0;
ryuna 3:cd7d3613e8e4 178 And[1] = 1;
ryuna 3:cd7d3613e8e4 179 }
ryuna 6:6664c6aa67d3 180 } */
ryuna 3:cd7d3613e8e4 181
ryuna 3:cd7d3613e8e4 182 }
ryuna 3:cd7d3613e8e4 183 void FallPhoto3(){ //three
ryuna 3:cd7d3613e8e4 184 //Way
ryuna 1:807091115672 185
ryuna 6:6664c6aa67d3 186 /*if(Way == 2){
ryuna 3:cd7d3613e8e4 187 if(!MotorState[1]){
ryuna 3:cd7d3613e8e4 188
ryuna 3:cd7d3613e8e4 189 StopTimer[2].attach(&MotorStop2,MOVE_TIME);
ryuna 3:cd7d3613e8e4 190 Motor = Motor | 0x08;
ryuna 3:cd7d3613e8e4 191 MotorState[1] = 1;
ryuna 3:cd7d3613e8e4 192 Led[1] = 1;
ryuna 3:cd7d3613e8e4 193 }else{
ryuna 3:cd7d3613e8e4 194 Flag[1] = 1;
ryuna 3:cd7d3613e8e4 195 }
ryuna 6:6664c6aa67d3 196 }else */
ryuna 6:6664c6aa67d3 197 if(Way == 1){
ryuna 3:cd7d3613e8e4 198
ryuna 3:cd7d3613e8e4 199 if(Flag[1]&&!FlagAdd[1]){
ryuna 3:cd7d3613e8e4 200 FlagAdd[1] = 1;
ryuna 5:aba9f5e97e03 201 }else if(MotorState[1]){
ryuna 3:cd7d3613e8e4 202 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 203 Motor = Motor | 0x04;
ryuna 3:cd7d3613e8e4 204 MotorState[1] = 0;
ryuna 3:cd7d3613e8e4 205 Flag[1] = 0;
ryuna 6:6664c6aa67d3 206 FlagAdd[1] = 0;
ryuna 3:cd7d3613e8e4 207 Led[1] = 0;
ryuna 3:cd7d3613e8e4 208 And[1] = 1;
ryuna 3:cd7d3613e8e4 209
ryuna 3:cd7d3613e8e4 210 }
ryuna 3:cd7d3613e8e4 211 }
ryuna 3:cd7d3613e8e4 212 }
ryuna 3:cd7d3613e8e4 213 void FallPhoto4(){//four
ryuna 3:cd7d3613e8e4 214
ryuna 3:cd7d3613e8e4 215 if(Way ==0){
ryuna 3:cd7d3613e8e4 216 Way = 1;
ryuna 3:cd7d3613e8e4 217 }
ryuna 3:cd7d3613e8e4 218 if(Way == 1){
ryuna 3:cd7d3613e8e4 219 if(!MotorState[2]){
ryuna 3:cd7d3613e8e4 220 StopTimer[4].attach(&MotorStop4,MOVE_TIME);
ryuna 3:cd7d3613e8e4 221 Motor = Motor | 0x20;//00100000
ryuna 3:cd7d3613e8e4 222 MotorState[2] = 1;
ryuna 3:cd7d3613e8e4 223 Led[2] = 1;
ryuna 3:cd7d3613e8e4 224 }else{
ryuna 3:cd7d3613e8e4 225 Flag[2] = 1;
ryuna 3:cd7d3613e8e4 226 }
ryuna 6:6664c6aa67d3 227 }/*else if(Way == 2){
ryuna 3:cd7d3613e8e4 228
ryuna 3:cd7d3613e8e4 229 if(Flag[2]&&!FlagAdd[2]){
ryuna 3:cd7d3613e8e4 230 FlagAdd[2] = 1;
ryuna 5:aba9f5e97e03 231 }else if(MotorState[2]){
ryuna 3:cd7d3613e8e4 232 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 233 Motor = Motor | 0x10;
ryuna 3:cd7d3613e8e4 234 MotorState[2] = 0;
ryuna 3:cd7d3613e8e4 235 Flag[2] = 0;
ryuna 3:cd7d3613e8e4 236 Led[2] = 0;
ryuna 3:cd7d3613e8e4 237 And[2] = 1;
ryuna 3:cd7d3613e8e4 238
ryuna 3:cd7d3613e8e4 239 }
ryuna 6:6664c6aa67d3 240 } */
ryuna 3:cd7d3613e8e4 241
ryuna 3:cd7d3613e8e4 242 }
ryuna 3:cd7d3613e8e4 243 void FallPhoto5(){//five
ryuna 6:6664c6aa67d3 244 /*if(Way ==0){
ryuna 3:cd7d3613e8e4 245 Way = 2;
ryuna 6:6664c6aa67d3 246 }*/
ryuna 3:cd7d3613e8e4 247
ryuna 6:6664c6aa67d3 248 /*if(Way == 2){
ryuna 3:cd7d3613e8e4 249 if(!MotorState[2]){
ryuna 3:cd7d3613e8e4 250 StopTimer[4].attach(&MotorStop4,MOVE_TIME);
ryuna 3:cd7d3613e8e4 251 Motor = Motor | 0x20;//00100000
ryuna 3:cd7d3613e8e4 252 MotorState[2] = 1;
ryuna 3:cd7d3613e8e4 253 Led[2] = 1;
ryuna 3:cd7d3613e8e4 254 }else{
ryuna 3:cd7d3613e8e4 255 Flag[2] = 1;
ryuna 3:cd7d3613e8e4 256 }
ryuna 6:6664c6aa67d3 257 }else */
ryuna 6:6664c6aa67d3 258 if(Way == 1){
ryuna 3:cd7d3613e8e4 259
ryuna 3:cd7d3613e8e4 260 if(Flag[2]&&!FlagAdd[2]){
ryuna 3:cd7d3613e8e4 261 FlagAdd[2] = 1;
ryuna 5:aba9f5e97e03 262 }else if(MotorState[2]){
ryuna 3:cd7d3613e8e4 263 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 264 Motor = Motor | 0x10;
ryuna 3:cd7d3613e8e4 265 MotorState[2] = 0;
ryuna 3:cd7d3613e8e4 266 Flag[2] = 0;
ryuna 6:6664c6aa67d3 267 FlagAdd[2] = 0;
ryuna 3:cd7d3613e8e4 268 Led[2] = 0;
ryuna 3:cd7d3613e8e4 269 And[2] = 1;
ryuna 3:cd7d3613e8e4 270 }
ryuna 3:cd7d3613e8e4 271 }
ryuna 3:cd7d3613e8e4 272 }
ryuna 3:cd7d3613e8e4 273 void FallPhoto6(){//six
ryuna 3:cd7d3613e8e4 274
ryuna 3:cd7d3613e8e4 275
ryuna 3:cd7d3613e8e4 276 if(Way == 1){
ryuna 3:cd7d3613e8e4 277 if(!MotorState[3]){
ryuna 3:cd7d3613e8e4 278 StopTimer[6].attach(&MotorStop6,MOVE_TIME);
ryuna 3:cd7d3613e8e4 279 Motor = Motor | 0x02;
ryuna 3:cd7d3613e8e4 280 MotorState[3] = 1;
ryuna 3:cd7d3613e8e4 281 Led[3] = 1;
ryuna 3:cd7d3613e8e4 282 }else{
ryuna 3:cd7d3613e8e4 283 Flag[3] = 1;
ryuna 3:cd7d3613e8e4 284 }
ryuna 6:6664c6aa67d3 285 }/*else if(Way == 2){
ryuna 3:cd7d3613e8e4 286
ryuna 3:cd7d3613e8e4 287 if(Flag[3]&&!FlagAdd[3]){
ryuna 3:cd7d3613e8e4 288 FlagAdd[3] = 1;
ryuna 5:aba9f5e97e03 289 }else if(MotorState[3]){
ryuna 3:cd7d3613e8e4 290 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 291 Motor = Motor | 0x01;
ryuna 3:cd7d3613e8e4 292 MotorState[3] = 0;
ryuna 3:cd7d3613e8e4 293 Flag[3] = 0;
ryuna 3:cd7d3613e8e4 294 Led[3] = 0;
ryuna 3:cd7d3613e8e4 295 And[3] = 1;
ryuna 3:cd7d3613e8e4 296 }
ryuna 6:6664c6aa67d3 297 } */
ryuna 3:cd7d3613e8e4 298 }
ryuna 3:cd7d3613e8e4 299 void FallPhoto7(){//seven
ryuna 3:cd7d3613e8e4 300
ryuna 3:cd7d3613e8e4 301
ryuna 3:cd7d3613e8e4 302
ryuna 6:6664c6aa67d3 303 /*if(Way == 2){
ryuna 3:cd7d3613e8e4 304 if(!MotorState[3]){
ryuna 3:cd7d3613e8e4 305 StopTimer[6].attach(&MotorStop6,MOVE_TIME);
ryuna 3:cd7d3613e8e4 306 Motor = Motor | 0x02;
ryuna 3:cd7d3613e8e4 307 MotorState[3] = 1;
ryuna 3:cd7d3613e8e4 308 Led[3] = 1;
ryuna 3:cd7d3613e8e4 309 }else{
ryuna 3:cd7d3613e8e4 310 Flag[3] = 1;
ryuna 3:cd7d3613e8e4 311 }
ryuna 6:6664c6aa67d3 312 }else */
ryuna 6:6664c6aa67d3 313 if(Way == 1){
ryuna 3:cd7d3613e8e4 314
ryuna 3:cd7d3613e8e4 315 if(Flag[3]&&!FlagAdd[3]){
ryuna 3:cd7d3613e8e4 316 FlagAdd[3] = 1;
ryuna 5:aba9f5e97e03 317 }else if(MotorState[3]){
ryuna 3:cd7d3613e8e4 318 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 319 Motor = Motor | 0x01;
ryuna 3:cd7d3613e8e4 320 MotorState[3] = 0;
ryuna 3:cd7d3613e8e4 321 Flag[3] = 0;
ryuna 6:6664c6aa67d3 322 FlagAdd[3] = 0;
ryuna 3:cd7d3613e8e4 323 Led[3] = 0;
ryuna 3:cd7d3613e8e4 324 And[3] = 1;
ryuna 3:cd7d3613e8e4 325 }
ryuna 3:cd7d3613e8e4 326 }
ryuna 3:cd7d3613e8e4 327 }
ryuna 3:cd7d3613e8e4 328
ryuna 3:cd7d3613e8e4 329 void Reset(){
ryuna 3:cd7d3613e8e4 330
ryuna 3:cd7d3613e8e4 331 int i;
ryuna 3:cd7d3613e8e4 332
ryuna 3:cd7d3613e8e4 333 if(MotorState[0]){
ryuna 3:cd7d3613e8e4 334 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 335 Motor = Motor | 0x40;//
ryuna 3:cd7d3613e8e4 336 }
ryuna 3:cd7d3613e8e4 337 if(MotorState[1]){
ryuna 3:cd7d3613e8e4 338 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 339 Motor = Motor | 0x04;//00000100
ryuna 3:cd7d3613e8e4 340 }
ryuna 3:cd7d3613e8e4 341 if(MotorState[2]){
ryuna 3:cd7d3613e8e4 342 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 343 Motor = Motor | 0x10;//00010000
ryuna 3:cd7d3613e8e4 344 }
ryuna 3:cd7d3613e8e4 345 if(MotorState[3]){
ryuna 3:cd7d3613e8e4 346 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 3:cd7d3613e8e4 347 Motor = Motor | 0x01;//00000001
ryuna 3:cd7d3613e8e4 348 }
ryuna 7:583ad8798bf1 349 for(i = 0; i < 4; i++){
ryuna 4:04f2479b83c1 350 Led[i] = 0;
ryuna 3:cd7d3613e8e4 351 Flag[i] = 0;
ryuna 3:cd7d3613e8e4 352 FlagAdd[i] = 0;
ryuna 3:cd7d3613e8e4 353 MotorState[i] = 0;
ryuna 4:04f2479b83c1 354 And[i] = 0;
ryuna 1:807091115672 355 }
ryuna 4:04f2479b83c1 356 Way = 0;
ryuna 4:04f2479b83c1 357 UpCt = 0;
ryuna 1:807091115672 358
ryuna 1:807091115672 359 }
ryuna 0:76ba33d3b4a6 360
ryuna 3:cd7d3613e8e4 361 void NumCount(){//足を上げた回数を記録
ryuna 6:6664c6aa67d3 362 UpCt ++;
ryuna 6:6664c6aa67d3 363 And[0] = 0 ;
ryuna 6:6664c6aa67d3 364 And[1] = 0 ;
ryuna 6:6664c6aa67d3 365 And[2] = 0 ;
ryuna 6:6664c6aa67d3 366 And[3] = 0 ;
ryuna 6:6664c6aa67d3 367 TempraFlag = 0;
ryuna 6:6664c6aa67d3 368 if(UpCt > 3){
ryuna 6:6664c6aa67d3 369 UpCt = 0;
ryuna 6:6664c6aa67d3 370 Way = 0;
ryuna 1:807091115672 371 }
ryuna 1:807091115672 372 }
ryuna 1:807091115672 373
ryuna 3:cd7d3613e8e4 374 void SetUp(){
ryuna 6:6664c6aa67d3 375 //int i;
ryuna 5:aba9f5e97e03 376
ryuna 5:aba9f5e97e03 377 Pwm[0] = PWM_LEVEL ;
ryuna 5:aba9f5e97e03 378 Pwm[1] = PWM_LEVEL ;
ryuna 5:aba9f5e97e03 379 Pwm[2] = PWM_LEVEL ;
ryuna 5:aba9f5e97e03 380 Pwm[3] = PWM_LEVEL ;
ryuna 7:583ad8798bf1 381 //for(i = 0 ; i < MOTOR_NUM ; i++ ){
ryuna 6:6664c6aa67d3 382 Photo[0].mode(PullUp);
ryuna 6:6664c6aa67d3 383 Photo[1].mode(PullUp);
ryuna 6:6664c6aa67d3 384 Photo[2].mode(PullUp);
ryuna 6:6664c6aa67d3 385 Photo[3].mode(PullUp);
ryuna 6:6664c6aa67d3 386 Photo[4].mode(PullUp);
ryuna 6:6664c6aa67d3 387 Photo[5].mode(PullUp);
ryuna 6:6664c6aa67d3 388 Photo[6].mode(PullUp);
ryuna 6:6664c6aa67d3 389 Photo[7].mode(PullUp);
ryuna 6:6664c6aa67d3 390
ryuna 6:6664c6aa67d3 391 //}
ryuna 3:cd7d3613e8e4 392 Photo[0].fall(FallPhoto0);
ryuna 3:cd7d3613e8e4 393 Photo[1].fall(FallPhoto1);
ryuna 3:cd7d3613e8e4 394 Photo[2].fall(FallPhoto2);
ryuna 3:cd7d3613e8e4 395 Photo[3].fall(FallPhoto3);
ryuna 3:cd7d3613e8e4 396 Photo[4].fall(FallPhoto4);
ryuna 3:cd7d3613e8e4 397 Photo[5].fall(FallPhoto5);
ryuna 3:cd7d3613e8e4 398 Photo[6].fall(FallPhoto6);
ryuna 3:cd7d3613e8e4 399 Photo[7].fall(FallPhoto7);
ryuna 1:807091115672 400
ryuna 7:583ad8798bf1 401 Sw.mode(PullUp);
ryuna 7:583ad8798bf1 402 Sw.fall(Reset);
ryuna 3:cd7d3613e8e4 403
ryuna 1:807091115672 404
ryuna 1:807091115672 405 }
ryuna 1:807091115672 406
ryuna 0:76ba33d3b4a6 407 int main() {
ryuna 2:89cf1003e119 408
ryuna 3:cd7d3613e8e4 409 SetUp();
ryuna 3:cd7d3613e8e4 410 wait(0.5);
ryuna 2:89cf1003e119 411
ryuna 1:807091115672 412 while(1){
ryuna 6:6664c6aa67d3 413 if(MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3]){
ryuna 6:6664c6aa67d3 414 TempraFlag = 1;
ryuna 6:6664c6aa67d3 415 Tempra = 0x0A;
ryuna 6:6664c6aa67d3 416 }else if(TempraFlag){
ryuna 6:6664c6aa67d3 417 StopTempra.attach(&TempraStop,TEMPRA_TIME);
ryuna 6:6664c6aa67d3 418 TempraFlag = 0;
ryuna 6:6664c6aa67d3 419 }
ryuna 6:6664c6aa67d3 420
ryuna 6:6664c6aa67d3 421 if(And[0]&&And[1]&&And[2]&&And[3]){
ryuna 6:6664c6aa67d3 422 NumCount();
ryuna 6:6664c6aa67d3 423 }
ryuna 6:6664c6aa67d3 424
ryuna 6:6664c6aa67d3 425 //printf("%d\t%d\n",UpCt,Way);
ryuna 0:76ba33d3b4a6 426 }
ryuna 0:76ba33d3b4a6 427 }