Bチーム / Mbed 2 deprecated RealTime_WheelLift

足の上げ下げを行う 一つの足につき2つのセンサを使用、 RTOS使用、 NHKの足上げプログラム予定

Dependencies:   mbed-rtos mbed

main.cpp@0:0773a5499020, 2014-10-12 (annotated)

Committer:
ryuna
Date:
Sun Oct 12 00:43:25 2014 +0000
Revision:
0:0773a5499020
RTOS????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:0773a5499020 1 /*
ryuna 0:0773a5499020 2 *覚え書き
ryuna 0:0773a5499020 3 *
ryuna 0:0773a5499020 4 ***mbedのピン配置に対するこのプログラムのピンの設定
ryuna 0:0773a5499020 5 *p5 ~ p12 Motor ...BusOut :Motor moved
ryuna 0:0773a5499020 6 *p13 ~ p20 Photo ...BusIn :Photo Sensor
ryuna 0:0773a5499020 7 *p21 ~ p24 Pwm ...PwmOut :Motor Pwm Signal
ryuna 0:0773a5499020 8 *p21 ~ p24 Sp ...DigitalOut :Check sensor state,do ditto led
ryuna 0:0773a5499020 9 *p25 ~ p28 Tempra...BusOut :Motor Tempra Moved
ryuna 0:0773a5499020 10 *p29,p30 Sw[2] ...DigitalIn :Switch Multi Use
ryuna 0:0773a5499020 11 *LED1 ~ LED4 Led ...DigitalOut :Led Brinked
ryuna 0:0773a5499020 12 *
ryuna 0:0773a5499020 13 **********************
ryuna 0:0773a5499020 14 ***関数の内容
ryuna 0:0773a5499020 15 *void WaitStop() :use wait
ryuna 0:0773a5499020 16 *void MotorStop() :erase motor pulses
ryuna 0:0773a5499020 17 *void MotorStart() :motor start timing timer
ryuna 0:0773a5499020 18 *void TempraStop() :stop motor of crawler
ryuna 0:0773a5499020 19 * //void Photo() :main processing ,wheel(lift or lower)motor move start
ryuna 0:0773a5499020 20 *void Reset() :wheels lower
ryuna 0:0773a5499020 21 *void GainSetting():sensor check mode,blinking Led
ryuna 0:0773a5499020 22 *void AndCount() :and count
ryuna 0:0773a5499020 23 *void SetUp() :first in function
ryuna 0:0773a5499020 24 *int main() :main function
ryuna 0:0773a5499020 25 *
ryuna 0:0773a5499020 26 *スレッド
ryuna 0:0773a5499020 27 *void Photo01_Thread(void const *argument)
ryuna 0:0773a5499020 28 *void Photo23_Thread(void const *argument)
ryuna 0:0773a5499020 29 *void Photo45_Thread(void const *argument)
ryuna 0:0773a5499020 30 *void Photo67_Thread(void const *argument) : main processing ,wheel(lift or lower)motor move start thread
ryuna 0:0773a5499020 31 *void ParallelThread(void const *argument) : Jump reset or GainSetting function
ryuna 0:0773a5499020 32 *
ryuna 0:0773a5499020 33 ***********************
ryuna 0:0773a5499020 34 ***処理内容
ryuna 0:0773a5499020 35 *photo系
ryuna 0:0773a5499020 36 *前のセンサで検知したら足をあげる、その後後ろのセンサが検知したら足を下げる
ryuna 0:0773a5499020 37 *前センサ→前センサ→後ろセンサ→後ろセンサ この場合足の上げ下げは一回しか行われない
ryuna 0:0773a5499020 38 *
ryuna 0:0773a5499020 39 *
ryuna 0:0773a5499020 40 *
ryuna 0:0773a5499020 41 **追記
ryuna 0:0773a5499020 42 *Photo {右前々、右前後、右後前、右後々、 左前々、左前後、左後前、左後々}
ryuna 0:0773a5499020 43 *
ryuna 0:0773a5499020 44 *
ryuna 0:0773a5499020 45 */
ryuna 0:0773a5499020 46
ryuna 0:0773a5499020 47
ryuna 0:0773a5499020 48 #include "mbed.h"
ryuna 0:0773a5499020 49 #include "rtos.h"
ryuna 0:0773a5499020 50
ryuna 0:0773a5499020 51 #define MOTOR_NUM 8 //motor & sensor num
ryuna 0:0773a5499020 52 #define START_TIME_B 0.237 //motor start wait time of Back //a lot
ryuna 0:0773a5499020 53 #define START_TIME_F 0.04
ryuna 0:0773a5499020 54 #define MOVE_TIME 0.10 //lift a wheel for 0.090 seconds
ryuna 0:0773a5499020 55 #define MOVE_DOWN_TIME 0.115 //lower a wheel for 0.10 seconds
ryuna 0:0773a5499020 56 #define TEMPRA_TIME 3.0 //move a crawler for 4.0 seconds
ryuna 0:0773a5499020 57 #define WAIT_TIME 0.010 //make wait time
ryuna 0:0773a5499020 58 #define PWM_LEVEL 1.0 //motors pwm setting level
ryuna 0:0773a5499020 59
ryuna 0:0773a5499020 60
ryuna 0:0773a5499020 61 BusIn Photo(p5, p6, p7, p8, p9, p10, p11, p12 );
ryuna 0:0773a5499020 62 DigitalIn Sw[2] = {p29, p30};
ryuna 0:0773a5499020 63
ryuna 0:0773a5499020 64 DigitalOut Led[4] = {LED1, LED2, LED3, LED4 };
ryuna 0:0773a5499020 65 DigitalOut Sp[4] = {p21, p22, p23, p24 };
ryuna 0:0773a5499020 66 BusOut Motor(p13, p14, p15, p16, p17, p18, p19, p20 );//{left front, right front, left back ，right back }
ryuna 0:0773a5499020 67 BusOut Tempra(p25, p26, p27, p28 );
ryuna 0:0773a5499020 68 //PwmOut Pwm[4] = {p21, p22, p23, p24 };
ryuna 0:0773a5499020 69
ryuna 0:0773a5499020 70 Timeout StartTimer[4];
ryuna 0:0773a5499020 71 Timeout StopTimer[8];
ryuna 0:0773a5499020 72 Timeout StopTempra;
ryuna 0:0773a5499020 73 Timeout StopWait[4];
ryuna 0:0773a5499020 74
ryuna 0:0773a5499020 75
ryuna 0:0773a5499020 76 volatile bool MotorState[4] = {0} ,Flag[4] = {0} ,FlagAdd[4] = {0} ,And[4] = {0} ,Wait[4] = {0};
ryuna 0:0773a5499020 77 volatile uint8_t Way = 0, DownCount = 0;
ryuna 0:0773a5499020 78 volatile bool TempraFlag = 1;
ryuna 0:0773a5499020 79
ryuna 0:0773a5499020 80
ryuna 0:0773a5499020 81 /*
ryuna 0:0773a5499020 82 Motor = {
ryuna 0:0773a5499020 83 LeftFrontUp, LeftFrontDown,
ryuna 0:0773a5499020 84 RightFrontUp, RightFrontDown,
ryuna 0:0773a5499020 85 LeftBackUp, LeftBackDown,
ryuna 0:0773a5499020 86 RightBackUp, RightBackDown
ryuna 0:0773a5499020 87 };
ryuna 0:0773a5499020 88 */
ryuna 0:0773a5499020 89
ryuna 0:0773a5499020 90 /***********************WaitStop*/
ryuna 0:0773a5499020 91 void WaitStop0()
ryuna 0:0773a5499020 92 {
ryuna 0:0773a5499020 93 Wait[0] = 0;
ryuna 0:0773a5499020 94 }
ryuna 0:0773a5499020 95 void WaitStop1()
ryuna 0:0773a5499020 96 {
ryuna 0:0773a5499020 97 Wait[1] = 0;
ryuna 0:0773a5499020 98 }
ryuna 0:0773a5499020 99 void WaitStop2()
ryuna 0:0773a5499020 100 {
ryuna 0:0773a5499020 101 Wait[2] = 0;
ryuna 0:0773a5499020 102 }
ryuna 0:0773a5499020 103 void WaitStop3()
ryuna 0:0773a5499020 104 {
ryuna 0:0773a5499020 105 Wait[3] = 0;
ryuna 0:0773a5499020 106 }
ryuna 0:0773a5499020 107 /*
ryuna 0:0773a5499020 108 ********************************MotorStop
ryuna 0:0773a5499020 109 */
ryuna 0:0773a5499020 110 void MotorStop0()
ryuna 0:0773a5499020 111 {
ryuna 0:0773a5499020 112 Motor = Motor & 0x7F;
ryuna 0:0773a5499020 113 }
ryuna 0:0773a5499020 114 void MotorStop1()
ryuna 0:0773a5499020 115 {
ryuna 0:0773a5499020 116 Motor = Motor & 0xBF;
ryuna 0:0773a5499020 117 StopWait[0].attach(&WaitStop0,WAIT_TIME);
ryuna 0:0773a5499020 118 }
ryuna 0:0773a5499020 119 void MotorStop2()
ryuna 0:0773a5499020 120 {
ryuna 0:0773a5499020 121 Motor = Motor & 0xF7;
ryuna 0:0773a5499020 122 }
ryuna 0:0773a5499020 123 void MotorStop3()
ryuna 0:0773a5499020 124 {
ryuna 0:0773a5499020 125 Motor = Motor & 0xFB;
ryuna 0:0773a5499020 126 StopWait[1].attach(&WaitStop1,WAIT_TIME);
ryuna 0:0773a5499020 127 }
ryuna 0:0773a5499020 128 void MotorStop4()
ryuna 0:0773a5499020 129 {
ryuna 0:0773a5499020 130 Motor = Motor & 0xDF;
ryuna 0:0773a5499020 131 }
ryuna 0:0773a5499020 132 void MotorStop5()
ryuna 0:0773a5499020 133 {
ryuna 0:0773a5499020 134 Motor = Motor & 0xEF;
ryuna 0:0773a5499020 135 StopWait[2].attach(&WaitStop2,WAIT_TIME);
ryuna 0:0773a5499020 136 }
ryuna 0:0773a5499020 137 void MotorStop6()
ryuna 0:0773a5499020 138 {
ryuna 0:0773a5499020 139 Motor = Motor & 0xFD;
ryuna 0:0773a5499020 140 }
ryuna 0:0773a5499020 141 void MotorStop7()
ryuna 0:0773a5499020 142 {
ryuna 0:0773a5499020 143 Motor = Motor & 0xFE;
ryuna 0:0773a5499020 144 StopWait[3].attach(&WaitStop3,WAIT_TIME);
ryuna 0:0773a5499020 145 }
ryuna 0:0773a5499020 146
ryuna 0:0773a5499020 147 /*
ryuna 0:0773a5499020 148 ***********************MotorStart
ryuna 0:0773a5499020 149 */
ryuna 0:0773a5499020 150 void MotorStart0()
ryuna 0:0773a5499020 151 {
ryuna 0:0773a5499020 152 StopTimer[0].attach(&MotorStop0,MOVE_TIME);
ryuna 0:0773a5499020 153 Motor = Motor & 0xBF;
ryuna 0:0773a5499020 154 Motor = Motor | 0x80;//10000000
ryuna 0:0773a5499020 155
ryuna 0:0773a5499020 156 }
ryuna 0:0773a5499020 157 void MotorStart1()
ryuna 0:0773a5499020 158 {
ryuna 0:0773a5499020 159 StopTimer[2].attach(&MotorStop2,MOVE_TIME);
ryuna 0:0773a5499020 160 Motor = Motor & 0xFB;
ryuna 0:0773a5499020 161 Motor = Motor | 0x08;//00001000
ryuna 0:0773a5499020 162 }
ryuna 0:0773a5499020 163 void MotorStart2()
ryuna 0:0773a5499020 164 {
ryuna 0:0773a5499020 165 StopTimer[4].attach(&MotorStop4,MOVE_TIME);
ryuna 0:0773a5499020 166 Motor = Motor & 0xEF;
ryuna 0:0773a5499020 167 Motor = Motor | 0x20;//00100000
ryuna 0:0773a5499020 168
ryuna 0:0773a5499020 169 }
ryuna 0:0773a5499020 170 void MotorStart3()
ryuna 0:0773a5499020 171 {
ryuna 0:0773a5499020 172 StopTimer[6].attach(&MotorStop6,MOVE_TIME);
ryuna 0:0773a5499020 173 Motor = Motor & 0xFE;
ryuna 0:0773a5499020 174 Motor = Motor | 0x02;//00000010
ryuna 0:0773a5499020 175 }
ryuna 0:0773a5499020 176
ryuna 0:0773a5499020 177
ryuna 0:0773a5499020 178 void TempraStop()
ryuna 0:0773a5499020 179 {
ryuna 0:0773a5499020 180 Tempra = 0x00;
ryuna 0:0773a5499020 181 }
ryuna 0:0773a5499020 182
ryuna 0:0773a5499020 183
ryuna 0:0773a5499020 184
ryuna 0:0773a5499020 185 void Photo01_Thread(void const *argument)
ryuna 0:0773a5499020 186 {
ryuna 0:0773a5499020 187 uint8_t CkPhoto = 0;
ryuna 0:0773a5499020 188 bool CkFlag[2] = {1,1};
ryuna 0:0773a5499020 189
ryuna 0:0773a5499020 190 while(1) {
ryuna 0:0773a5499020 191 CkPhoto = (~Photo) & 0x03;
ryuna 0:0773a5499020 192 if(CkPhoto==0) { // 右前足
ryuna 0:0773a5499020 193 CkFlag[0] = 1;
ryuna 0:0773a5499020 194 CkFlag[1] = 1;
ryuna 0:0773a5499020 195 } else if(CkPhoto<3) {
ryuna 0:0773a5499020 196 CkPhoto -= 1;
ryuna 0:0773a5499020 197 if(CkFlag[CkPhoto]) {
ryuna 0:0773a5499020 198 if(CkPhoto == 0) {
ryuna 0:0773a5499020 199 if(Way ==0){
ryuna 0:0773a5499020 200 Way = 1;
ryuna 0:0773a5499020 201 }
ryuna 0:0773a5499020 202 if((Way == 1)&&!Wait[0]) {
ryuna 0:0773a5499020 203 if(!MotorState[0]) {
ryuna 0:0773a5499020 204 StartTimer[0].attach(&MotorStart0,START_TIME_F);
ryuna 0:0773a5499020 205 MotorState[0] = 1;
ryuna 0:0773a5499020 206 Led[0] = 1;
ryuna 0:0773a5499020 207 Sp [0] = 1;
ryuna 0:0773a5499020 208 } else {
ryuna 0:0773a5499020 209 Flag[0] = 1;
ryuna 0:0773a5499020 210 }
ryuna 0:0773a5499020 211 }
ryuna 0:0773a5499020 212
ryuna 0:0773a5499020 213 } else {
ryuna 0:0773a5499020 214 if(Way == 1) {
ryuna 0:0773a5499020 215 if(Flag[0]&&!FlagAdd[0]) {
ryuna 0:0773a5499020 216 FlagAdd[0] = 1;
ryuna 0:0773a5499020 217 } else if(MotorState[0]) {
ryuna 0:0773a5499020 218 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 219 Motor = Motor & 0x7F;// 01111111
ryuna 0:0773a5499020 220 Motor = Motor | 0x40;//0x40 = 01000000
ryuna 0:0773a5499020 221 MotorState[0] = 0;
ryuna 0:0773a5499020 222 Flag[0] = 0;
ryuna 0:0773a5499020 223 FlagAdd[0] = 0;
ryuna 0:0773a5499020 224 Led[0] = 0;
ryuna 0:0773a5499020 225 Sp [0] = 0;
ryuna 0:0773a5499020 226 And[0] = 1;
ryuna 0:0773a5499020 227 Wait[0] = 1;
ryuna 0:0773a5499020 228 }
ryuna 0:0773a5499020 229 }
ryuna 0:0773a5499020 230
ryuna 0:0773a5499020 231 }
ryuna 0:0773a5499020 232 CkFlag[CkPhoto] = 0;
ryuna 0:0773a5499020 233 }
ryuna 0:0773a5499020 234 }
ryuna 0:0773a5499020 235 }
ryuna 0:0773a5499020 236
ryuna 0:0773a5499020 237 }
ryuna 0:0773a5499020 238 void Photo23_Thread(void const *argument)
ryuna 0:0773a5499020 239 {
ryuna 0:0773a5499020 240 uint8_t CkPhoto = 0;
ryuna 0:0773a5499020 241 bool CkFlag[2] = {1,1};
ryuna 0:0773a5499020 242
ryuna 0:0773a5499020 243 while(1) {
ryuna 0:0773a5499020 244 CkPhoto = ((~Photo) & 0x0C) >> 2;
ryuna 0:0773a5499020 245 if(CkPhoto==0) { // 右前足
ryuna 0:0773a5499020 246 CkFlag[0] = 1;
ryuna 0:0773a5499020 247 CkFlag[1] = 1;
ryuna 0:0773a5499020 248 } else if(CkPhoto<3) {
ryuna 0:0773a5499020 249 CkPhoto -= 1;
ryuna 0:0773a5499020 250 if(CkFlag[CkPhoto]) {
ryuna 0:0773a5499020 251 if(CkPhoto == 0) {
ryuna 0:0773a5499020 252 if((Way == 1)&&!Wait[1]) {
ryuna 0:0773a5499020 253 if(!MotorState[1]) {
ryuna 0:0773a5499020 254 StartTimer[1].attach(&MotorStart1,START_TIME_B);
ryuna 0:0773a5499020 255 MotorState[1] = 1;
ryuna 0:0773a5499020 256 Led[1] = 1;
ryuna 0:0773a5499020 257 Sp [1] = 1;
ryuna 0:0773a5499020 258 } else {
ryuna 0:0773a5499020 259 Flag[1] = 1;
ryuna 0:0773a5499020 260 }
ryuna 0:0773a5499020 261 }
ryuna 0:0773a5499020 262
ryuna 0:0773a5499020 263 } else {
ryuna 0:0773a5499020 264 if(Way == 1) {
ryuna 0:0773a5499020 265 if(Flag[1]&&!FlagAdd[1]) {
ryuna 0:0773a5499020 266 FlagAdd[1] = 1;
ryuna 0:0773a5499020 267 } else if(MotorState[1]) {
ryuna 0:0773a5499020 268 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 269 Motor = Motor & 0xF7;
ryuna 0:0773a5499020 270 Motor = Motor | 0x04;//00000100
ryuna 0:0773a5499020 271 MotorState[1] = 0;
ryuna 0:0773a5499020 272 Flag[1] = 0;
ryuna 0:0773a5499020 273 FlagAdd[1] = 0;
ryuna 0:0773a5499020 274 Led[1] = 0;
ryuna 0:0773a5499020 275 Sp [1] = 0;
ryuna 0:0773a5499020 276 And[1] = 1;
ryuna 0:0773a5499020 277 Wait[1] = 1;
ryuna 0:0773a5499020 278 }
ryuna 0:0773a5499020 279 }
ryuna 0:0773a5499020 280
ryuna 0:0773a5499020 281 }
ryuna 0:0773a5499020 282 CkFlag[CkPhoto] = 0;
ryuna 0:0773a5499020 283 }
ryuna 0:0773a5499020 284 }
ryuna 0:0773a5499020 285 }
ryuna 0:0773a5499020 286
ryuna 0:0773a5499020 287 }
ryuna 0:0773a5499020 288 void Photo45_Thread(void const *argument)
ryuna 0:0773a5499020 289 {
ryuna 0:0773a5499020 290 uint8_t CkPhoto = 0;
ryuna 0:0773a5499020 291 bool CkFlag[2] = {1,1};
ryuna 0:0773a5499020 292
ryuna 0:0773a5499020 293 while(1) {
ryuna 0:0773a5499020 294 CkPhoto = ((~Photo) & 0x30) >> 4;
ryuna 0:0773a5499020 295 if(CkPhoto==0) { // 右前足
ryuna 0:0773a5499020 296 CkFlag[0] = 1;
ryuna 0:0773a5499020 297 CkFlag[1] = 1;
ryuna 0:0773a5499020 298 } else if(CkPhoto<3) {
ryuna 0:0773a5499020 299 CkPhoto -= 1;
ryuna 0:0773a5499020 300 if(CkFlag[CkPhoto]) {
ryuna 0:0773a5499020 301 if(CkPhoto == 0) {
ryuna 0:0773a5499020 302 if(Way == 0) {
ryuna 0:0773a5499020 303 Way = 1;
ryuna 0:0773a5499020 304 }
ryuna 0:0773a5499020 305 if((Way == 1)&&!Wait[2]) {
ryuna 0:0773a5499020 306 if(!MotorState[2]) {
ryuna 0:0773a5499020 307 StartTimer[2].attach(&MotorStart2,START_TIME_F);
ryuna 0:0773a5499020 308 MotorState[2] = 1;
ryuna 0:0773a5499020 309 Led[2] = 1;
ryuna 0:0773a5499020 310 Sp [2] = 1;
ryuna 0:0773a5499020 311 } else {
ryuna 0:0773a5499020 312 Flag[2] = 1;
ryuna 0:0773a5499020 313 }
ryuna 0:0773a5499020 314 }
ryuna 0:0773a5499020 315 } else {
ryuna 0:0773a5499020 316 if(Way == 1) {
ryuna 0:0773a5499020 317 if(Flag[2]&&!FlagAdd[2]) {
ryuna 0:0773a5499020 318 FlagAdd[2] = 1;
ryuna 0:0773a5499020 319 } else if(MotorState[2]) {
ryuna 0:0773a5499020 320 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 321 Motor = Motor & 0xDF;
ryuna 0:0773a5499020 322 Motor = Motor | 0x10;//00010000
ryuna 0:0773a5499020 323 MotorState[2] = 0;
ryuna 0:0773a5499020 324 Flag[2] = 0;
ryuna 0:0773a5499020 325 FlagAdd[2] = 0;
ryuna 0:0773a5499020 326 Led[2] = 0;
ryuna 0:0773a5499020 327 Sp [2] = 0;
ryuna 0:0773a5499020 328 And[2] = 1;
ryuna 0:0773a5499020 329 Wait[2] = 1;
ryuna 0:0773a5499020 330 }
ryuna 0:0773a5499020 331 }
ryuna 0:0773a5499020 332
ryuna 0:0773a5499020 333 }
ryuna 0:0773a5499020 334 CkFlag[CkPhoto] = 0;
ryuna 0:0773a5499020 335 }
ryuna 0:0773a5499020 336 }
ryuna 0:0773a5499020 337 }
ryuna 0:0773a5499020 338
ryuna 0:0773a5499020 339 }
ryuna 0:0773a5499020 340 void Photo67_Thread(void const *argument)
ryuna 0:0773a5499020 341 {
ryuna 0:0773a5499020 342 uint8_t CkPhoto = 0;
ryuna 0:0773a5499020 343 bool CkFlag[2] = {1,1};
ryuna 0:0773a5499020 344
ryuna 0:0773a5499020 345 while(1) {
ryuna 0:0773a5499020 346 CkPhoto = ((~Photo) & 0xC0) >> 6;
ryuna 0:0773a5499020 347 if(CkPhoto==0) { // 右前足
ryuna 0:0773a5499020 348 CkFlag[0] = 1;
ryuna 0:0773a5499020 349 CkFlag[1] = 1;
ryuna 0:0773a5499020 350 } else if(CkPhoto<3) {
ryuna 0:0773a5499020 351 CkPhoto -= 1;
ryuna 0:0773a5499020 352 if(CkFlag[CkPhoto]) {
ryuna 0:0773a5499020 353 if(CkPhoto == 0) {
ryuna 0:0773a5499020 354 if((Way == 1)&&!Wait[3]) {
ryuna 0:0773a5499020 355 if(!MotorState[3]) {
ryuna 0:0773a5499020 356 StartTimer[3].attach(&MotorStart3,START_TIME_B);
ryuna 0:0773a5499020 357 MotorState[3] = 1;
ryuna 0:0773a5499020 358 Led[3] = 1;
ryuna 0:0773a5499020 359 Sp [3] = 1;
ryuna 0:0773a5499020 360 } else {
ryuna 0:0773a5499020 361 Flag[3] = 1;
ryuna 0:0773a5499020 362 }
ryuna 0:0773a5499020 363 }
ryuna 0:0773a5499020 364
ryuna 0:0773a5499020 365 } else {
ryuna 0:0773a5499020 366 if(Way == 1) {
ryuna 0:0773a5499020 367 if(Flag[3]&&!FlagAdd[3]) {
ryuna 0:0773a5499020 368 FlagAdd[3] = 1;
ryuna 0:0773a5499020 369 } else if(MotorState[3]) {
ryuna 0:0773a5499020 370 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 371 Motor = Motor & 0xFD;//11111101
ryuna 0:0773a5499020 372 Motor = Motor | 0x01;//00000001
ryuna 0:0773a5499020 373 MotorState[3] = 0;
ryuna 0:0773a5499020 374 Flag[3] = 0;
ryuna 0:0773a5499020 375 FlagAdd[3] = 0;
ryuna 0:0773a5499020 376 Led[3] = 0;
ryuna 0:0773a5499020 377 Sp [3] = 0;
ryuna 0:0773a5499020 378 And[3] = 1;
ryuna 0:0773a5499020 379 Wait[3] = 1;
ryuna 0:0773a5499020 380 }
ryuna 0:0773a5499020 381 }
ryuna 0:0773a5499020 382
ryuna 0:0773a5499020 383 }
ryuna 0:0773a5499020 384 CkFlag[CkPhoto] = 0;
ryuna 0:0773a5499020 385 }
ryuna 0:0773a5499020 386 }
ryuna 0:0773a5499020 387 }
ryuna 0:0773a5499020 388
ryuna 0:0773a5499020 389 }
ryuna 0:0773a5499020 390
ryuna 0:0773a5499020 391
ryuna 0:0773a5499020 392 void Reset()
ryuna 0:0773a5499020 393 {
ryuna 0:0773a5499020 394 int i;
ryuna 0:0773a5499020 395
ryuna 0:0773a5499020 396 if(MotorState[0]) {
ryuna 0:0773a5499020 397 StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 398 Motor = Motor | 0x40;//01000000
ryuna 0:0773a5499020 399 }
ryuna 0:0773a5499020 400 if(MotorState[1]) {
ryuna 0:0773a5499020 401 StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 402 Motor = Motor | 0x04;//00000100
ryuna 0:0773a5499020 403 }
ryuna 0:0773a5499020 404 if(MotorState[2]) {
ryuna 0:0773a5499020 405 StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 406 Motor = Motor | 0x10;//00010000
ryuna 0:0773a5499020 407 }
ryuna 0:0773a5499020 408 if(MotorState[3]) {
ryuna 0:0773a5499020 409 StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
ryuna 0:0773a5499020 410 Motor = Motor | 0x01;//00000001
ryuna 0:0773a5499020 411 }
ryuna 0:0773a5499020 412 for(i = 0; i < 4; i++) {
ryuna 0:0773a5499020 413 Led[i] = 0;
ryuna 0:0773a5499020 414 Flag[i] = 0;
ryuna 0:0773a5499020 415 FlagAdd[i] = 0;
ryuna 0:0773a5499020 416 MotorState[i] = 0;
ryuna 0:0773a5499020 417 And[i] = 0;
ryuna 0:0773a5499020 418 }
ryuna 0:0773a5499020 419
ryuna 0:0773a5499020 420 Way = 0;
ryuna 0:0773a5499020 421 DownCount = 0;
ryuna 0:0773a5499020 422 TempraFlag = 1;
ryuna 0:0773a5499020 423
ryuna 0:0773a5499020 424 }
ryuna 0:0773a5499020 425
ryuna 0:0773a5499020 426 void GainSetting ()
ryuna 0:0773a5499020 427 {
ryuna 0:0773a5499020 428
ryuna 0:0773a5499020 429 bool CkFlag[8] = {1,1,1,1,1,1,1,1},PhotoCk[8] = {0};
ryuna 0:0773a5499020 430
ryuna 0:0773a5499020 431 Reset();
ryuna 0:0773a5499020 432 Tempra = 0;
ryuna 0:0773a5499020 433 TempraFlag = 0;
ryuna 0:0773a5499020 434 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 0:0773a5499020 435 Sp[0] = Sp[1] = Sp[2] = Sp[3] = 1;
ryuna 0:0773a5499020 436
ryuna 0:0773a5499020 437 while(1) {
ryuna 0:0773a5499020 438 if(!Sw[0]) {
ryuna 0:0773a5499020 439 while(1) {//右側のセンサの調整
ryuna 0:0773a5499020 440 PhotoCk[0] = ((~Photo)&0x01);
ryuna 0:0773a5499020 441 PhotoCk[1] = ((~Photo)&0x02) >> 1;
ryuna 0:0773a5499020 442 PhotoCk[2] = ((~Photo)&0x04) >> 2;
ryuna 0:0773a5499020 443 PhotoCk[3] = ((~Photo)&0x08) >> 3;
ryuna 0:0773a5499020 444 if(PhotoCk[0]&&CkFlag[0]) {
ryuna 0:0773a5499020 445 Led[0] = 0;
ryuna 0:0773a5499020 446 Sp [0] = 0;
ryuna 0:0773a5499020 447 CkFlag[0] = 0;
ryuna 0:0773a5499020 448 } else if(!PhotoCk[0]) {
ryuna 0:0773a5499020 449 CkFlag[0] = 1;
ryuna 0:0773a5499020 450 Led[0] = 1;
ryuna 0:0773a5499020 451 Sp [0] = 1;
ryuna 0:0773a5499020 452 }
ryuna 0:0773a5499020 453
ryuna 0:0773a5499020 454 if(PhotoCk[1]&&CkFlag[1]) {
ryuna 0:0773a5499020 455 Led[1] = 0;
ryuna 0:0773a5499020 456 Sp [1] = 0;
ryuna 0:0773a5499020 457 CkFlag[1] = 0;
ryuna 0:0773a5499020 458 } else if(!PhotoCk[1]) {
ryuna 0:0773a5499020 459 CkFlag[1] = 1;
ryuna 0:0773a5499020 460 Led[1] = 1;
ryuna 0:0773a5499020 461 Sp [1] = 1;
ryuna 0:0773a5499020 462 }
ryuna 0:0773a5499020 463 if(PhotoCk[2]&&CkFlag[2]) {
ryuna 0:0773a5499020 464 Led[2] = 0;
ryuna 0:0773a5499020 465 Sp [2] = 0;
ryuna 0:0773a5499020 466 CkFlag[2] = 0;
ryuna 0:0773a5499020 467 } else if(!PhotoCk[2]) {
ryuna 0:0773a5499020 468 CkFlag[2] = 1;
ryuna 0:0773a5499020 469 Led[2] = 1;
ryuna 0:0773a5499020 470 Sp [2] = 1;
ryuna 0:0773a5499020 471 }
ryuna 0:0773a5499020 472 if(PhotoCk[3]&&CkFlag[3]) {
ryuna 0:0773a5499020 473 Led[3] = 0;
ryuna 0:0773a5499020 474 Sp [3] = 0;
ryuna 0:0773a5499020 475 CkFlag[3] = 0;
ryuna 0:0773a5499020 476 } else if(!PhotoCk[3]) {
ryuna 0:0773a5499020 477 CkFlag[3] = 1;
ryuna 0:0773a5499020 478 Led[3] = 1;
ryuna 0:0773a5499020 479 Sp [3] = 1;
ryuna 0:0773a5499020 480 }
ryuna 0:0773a5499020 481 if(!Sw[1]) break;
ryuna 0:0773a5499020 482 }
ryuna 0:0773a5499020 483 }
ryuna 0:0773a5499020 484
ryuna 0:0773a5499020 485 //左側のセンサの調整
ryuna 0:0773a5499020 486 PhotoCk[4] = ((~Photo)&0x10) >> 4;
ryuna 0:0773a5499020 487 PhotoCk[5] = ((~Photo)&0x20) >> 5;
ryuna 0:0773a5499020 488 PhotoCk[6] = ((~Photo)&0x40) >> 6;
ryuna 0:0773a5499020 489 PhotoCk[7] = ((~Photo)&0x80) >> 7;
ryuna 0:0773a5499020 490
ryuna 0:0773a5499020 491 if(PhotoCk[4]&&CkFlag[4]) {
ryuna 0:0773a5499020 492 Led[0] = 0;
ryuna 0:0773a5499020 493 Sp [0] = 0;
ryuna 0:0773a5499020 494 CkFlag[4] = 0;
ryuna 0:0773a5499020 495 } else if(!PhotoCk[4]) {
ryuna 0:0773a5499020 496 CkFlag[4] = 1;
ryuna 0:0773a5499020 497 Led[0] = 1;
ryuna 0:0773a5499020 498 Sp [0] = 1;
ryuna 0:0773a5499020 499 }
ryuna 0:0773a5499020 500 if(PhotoCk[5]&&CkFlag[5]) {
ryuna 0:0773a5499020 501 Led[1] = 0;
ryuna 0:0773a5499020 502 Sp [1] = 0;
ryuna 0:0773a5499020 503 CkFlag[5] = 0;
ryuna 0:0773a5499020 504 } else if(!PhotoCk[5]) {
ryuna 0:0773a5499020 505 CkFlag[5] = 1;
ryuna 0:0773a5499020 506 Led[1] = 1;
ryuna 0:0773a5499020 507 Sp [1] = 1;
ryuna 0:0773a5499020 508 }
ryuna 0:0773a5499020 509 if(PhotoCk[6]&&CkFlag[6]) {
ryuna 0:0773a5499020 510 Led[2] = 0;
ryuna 0:0773a5499020 511 Sp [2] = 0;
ryuna 0:0773a5499020 512 CkFlag[6] = 0;
ryuna 0:0773a5499020 513 } else if(!PhotoCk[6]) {
ryuna 0:0773a5499020 514 CkFlag[6] = 1;
ryuna 0:0773a5499020 515 Led[2] = 1;
ryuna 0:0773a5499020 516 Sp [2] = 1;
ryuna 0:0773a5499020 517 }
ryuna 0:0773a5499020 518 if(PhotoCk[7]&&CkFlag[7]) {
ryuna 0:0773a5499020 519 Led[3] = 0;
ryuna 0:0773a5499020 520 Sp [3] = 0;
ryuna 0:0773a5499020 521 CkFlag[7] = 0;
ryuna 0:0773a5499020 522 } else if(!PhotoCk[7]) {
ryuna 0:0773a5499020 523 CkFlag[7] = 1;
ryuna 0:0773a5499020 524 Led[3] = 1;
ryuna 0:0773a5499020 525 Sp [3] = 1;
ryuna 0:0773a5499020 526 }
ryuna 0:0773a5499020 527
ryuna 0:0773a5499020 528 }
ryuna 0:0773a5499020 529
ryuna 0:0773a5499020 530 }
ryuna 0:0773a5499020 531
ryuna 0:0773a5499020 532
ryuna 0:0773a5499020 533 void AndCount() //"Wheels" Up and Down counting
ryuna 0:0773a5499020 534 {
ryuna 0:0773a5499020 535
ryuna 0:0773a5499020 536 DownCount ++;
ryuna 0:0773a5499020 537 And[0] = 0 ;
ryuna 0:0773a5499020 538 And[1] = 0 ;
ryuna 0:0773a5499020 539 And[2] = 0 ;
ryuna 0:0773a5499020 540 And[3] = 0 ;
ryuna 0:0773a5499020 541 if(DownCount > 3) {
ryuna 0:0773a5499020 542 DownCount = 0;
ryuna 0:0773a5499020 543 Way = 0;
ryuna 0:0773a5499020 544 }
ryuna 0:0773a5499020 545 }
ryuna 0:0773a5499020 546
ryuna 0:0773a5499020 547 /****************************Reset or Crawler_stop or CheckMode_Start Thread
ryuna 0:0773a5499020 548 */
ryuna 0:0773a5499020 549 void ParallelThread(void const *argument)
ryuna 0:0773a5499020 550 {
ryuna 0:0773a5499020 551 uint8_t Ct;
ryuna 0:0773a5499020 552 while (1) {
ryuna 0:0773a5499020 553
ryuna 0:0773a5499020 554
ryuna 0:0773a5499020 555 if(And[0]&&And[1]&&And[2]&&And[3]) {
ryuna 0:0773a5499020 556 AndCount();
ryuna 0:0773a5499020 557 }
ryuna 0:0773a5499020 558
ryuna 0:0773a5499020 559 if((MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3])&&TempraFlag) {
ryuna 0:0773a5499020 560 Tempra = 0x0A;
ryuna 0:0773a5499020 561 StopTempra.attach(&TempraStop,TEMPRA_TIME);
ryuna 0:0773a5499020 562 }
ryuna 0:0773a5499020 563
ryuna 0:0773a5499020 564 if(!Sw[0]&&!Sw[1]) {
ryuna 0:0773a5499020 565 Ct = 0;
ryuna 0:0773a5499020 566 while(!Sw[0]&&!Sw[1]&&(Ct <= 100)) {
ryuna 0:0773a5499020 567 Ct++;
ryuna 0:0773a5499020 568 }
ryuna 0:0773a5499020 569 if(Ct >= 100) {
ryuna 0:0773a5499020 570 GainSetting();
ryuna 0:0773a5499020 571 }
ryuna 0:0773a5499020 572
ryuna 0:0773a5499020 573 } else if(!Sw[0]) {
ryuna 0:0773a5499020 574 Ct = 0;
ryuna 0:0773a5499020 575 while(!Sw[0]&&(Ct <= 100)) {
ryuna 0:0773a5499020 576 Ct ++;
ryuna 0:0773a5499020 577 }
ryuna 0:0773a5499020 578 if(Ct >= 100) {
ryuna 0:0773a5499020 579 Reset();
ryuna 0:0773a5499020 580 }
ryuna 0:0773a5499020 581
ryuna 0:0773a5499020 582 } else if(!Sw[1]) {
ryuna 0:0773a5499020 583 Ct = 0;
ryuna 0:0773a5499020 584 while(!Sw[1]&&(Ct <= 100)) {
ryuna 0:0773a5499020 585 Ct ++;
ryuna 0:0773a5499020 586 }
ryuna 0:0773a5499020 587 if(Ct >= 100) {
ryuna 0:0773a5499020 588 Tempra = 0;
ryuna 0:0773a5499020 589 TempraFlag = 0;
ryuna 0:0773a5499020 590 }
ryuna 0:0773a5499020 591
ryuna 0:0773a5499020 592
ryuna 0:0773a5499020 593 }
ryuna 0:0773a5499020 594 }
ryuna 0:0773a5499020 595 }
ryuna 0:0773a5499020 596
ryuna 0:0773a5499020 597
ryuna 0:0773a5499020 598 void SetUp()
ryuna 0:0773a5499020 599
ryuna 0:0773a5499020 600 {
ryuna 0:0773a5499020 601 /*
ryuna 0:0773a5499020 602 Pwm[0] = PWM_LEVEL ;
ryuna 0:0773a5499020 603 Pwm[1] = PWM_LEVEL ;
ryuna 0:0773a5499020 604 Pwm[2] = PWM_LEVEL ;
ryuna 0:0773a5499020 605 Pwm[3] = PWM_LEVEL ;
ryuna 0:0773a5499020 606 */
ryuna 0:0773a5499020 607 Sp[0] = Sp[1] = Sp[2] = Sp[3] = 0;
ryuna 0:0773a5499020 608 Photo.mode(PullUp);
ryuna 0:0773a5499020 609 Sw[0].mode(PullUp);
ryuna 0:0773a5499020 610 Sw[1].mode(PullUp);
ryuna 0:0773a5499020 611
ryuna 0:0773a5499020 612 }
ryuna 0:0773a5499020 613 int main()
ryuna 0:0773a5499020 614 {
ryuna 0:0773a5499020 615 SetUp();
ryuna 0:0773a5499020 616
ryuna 0:0773a5499020 617 Thread thread(ParallelThread);
ryuna 0:0773a5499020 618 Thread thread0(Photo01_Thread);
ryuna 0:0773a5499020 619 Thread thread1(Photo23_Thread);
ryuna 0:0773a5499020 620 Thread thread2(Photo45_Thread);
ryuna 0:0773a5499020 621 Thread thread3(Photo67_Thread);
ryuna 0:0773a5499020 622 wait(0.5);
ryuna 0:0773a5499020 623
ryuna 0:0773a5499020 624
ryuna 0:0773a5499020 625 while(1) {
ryuna 0:0773a5499020 626
ryuna 0:0773a5499020 627 }
ryuna 0:0773a5499020 628 }
ryuna 0:0773a5499020 629