Trieu LE / Mbed 2 deprecated mbed_blinky

Dependencies:   mbed

main.cpp@1:552a34826f55, 2016-08-26 (annotated)

Committer:
trieule95
Date:
Fri Aug 26 10:03:17 2016 +0000
Revision:
1:552a34826f55
Parent:
0:b0c572636fc2 
demo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
trieule95 0:b0c572636fc2 1 /* mbed Microcontroller Library
trieule95 0:b0c572636fc2 2 * Copyright (c) 2006-2015 ARM Limited
trieule95 0:b0c572636fc2 3 *
trieule95 0:b0c572636fc2 4 * Licensed under the Apache License, Version 2.0 (the "License");
trieule95 0:b0c572636fc2 5 * you may not use this file except in compliance with the License.
trieule95 0:b0c572636fc2 6 * You may obtain a copy of the License at
trieule95 0:b0c572636fc2 7 *
trieule95 0:b0c572636fc2 8 * http://www.apache.org/licenses/LICENSE-2.0
trieule95 0:b0c572636fc2 9 *
trieule95 0:b0c572636fc2 10 * Unless required by applicable law or agreed to in writing, software
trieule95 0:b0c572636fc2 11 * distributed under the License is distributed on an "AS IS" BASIS,
trieule95 0:b0c572636fc2 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
trieule95 0:b0c572636fc2 13 * See the License for the specific language governing permissions and
trieule95 0:b0c572636fc2 14 * limitations under the License.
trieule95 0:b0c572636fc2 15 */
trieule95 0:b0c572636fc2 16
trieule95 0:b0c572636fc2 17 #include "mbed.h"
trieule95 0:b0c572636fc2 18 //#include "ble/BLE.h"
trieule95 0:b0c572636fc2 19 //#include "ble/services/iBeacon.h"
trieule95 0:b0c572636fc2 20 //#include "ble/services/UARTService.h"
trieule95 0:b0c572636fc2 21
trieule95 0:b0c572636fc2 22 DigitalOut led1(LED1, 1);
trieule95 0:b0c572636fc2 23 DigitalOut DIR_L(p25);
trieule95 0:b0c572636fc2 24 DigitalOut PWM_L(p23);
trieule95 0:b0c572636fc2 25 DigitalOut DIR_R(p28);
trieule95 0:b0c572636fc2 26 DigitalOut PWM_R(p24);
trieule95 0:b0c572636fc2 27
trieule95 0:b0c572636fc2 28 //// Sensor
trieule95 0:b0c572636fc2 29 #define NOISE 5
trieule95 0:b0c572636fc2 30 //#define DEBUG
trieule95 0:b0c572636fc2 31
trieule95 0:b0c572636fc2 32 #ifdef DEBUG
trieule95 0:b0c572636fc2 33 Serial pc(p10, p11);
trieule95 0:b0c572636fc2 34 #endif
trieule95 0:b0c572636fc2 35 Ticker ticker;
trieule95 0:b0c572636fc2 36 AnalogIn analog_ir(p1);
trieule95 0:b0c572636fc2 37 DigitalIn SEN_UP_R(p2);
trieule95 0:b0c572636fc2 38 DigitalIn SEN_UP_L(p3);
trieule95 0:b0c572636fc2 39 DigitalIn SEN_DOWN(p4);
trieule95 0:b0c572636fc2 40 DigitalIn SEN_IR(p6);
trieule95 0:b0c572636fc2 41
trieule95 0:b0c572636fc2 42 #define ON 0
trieule95 0:b0c572636fc2 43 #define OFF 1
trieule95 0:b0c572636fc2 44 #define VREF 3300.0 //mV
trieule95 0:b0c572636fc2 45
trieule95 0:b0c572636fc2 46 //// Motor
trieule95 0:b0c572636fc2 47 #define STOP 0
trieule95 0:b0c572636fc2 48 #define UP 1
trieule95 0:b0c572636fc2 49 #define DOWN 2
trieule95 0:b0c572636fc2 50 #define LEFT 3
trieule95 0:b0c572636fc2 51 #define RIGHT 4
trieule95 0:b0c572636fc2 52 uint8_t g_time_pwm = 0, g_pwm_left = 0, g_pwm_right = 0;
trieule95 0:b0c572636fc2 53 uint16_t g_time_led = 0;
trieule95 0:b0c572636fc2 54 uint8_t g_sts_car = STOP;
trieule95 0:b0c572636fc2 55
trieule95 0:b0c572636fc2 56
trieule95 0:b0c572636fc2 57
trieule95 0:b0c572636fc2 58 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 59 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 60 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 61 /*
trieule95 0:b0c572636fc2 62 * @param dir: UP or DOWN; speed: 0-100.
trieule95 0:b0c572636fc2 63 * @return none.
trieule95 0:b0c572636fc2 64 */
trieule95 0:b0c572636fc2 65 void motor_left(uint8_t dir, uint8_t speed)
trieule95 0:b0c572636fc2 66 {
trieule95 0:b0c572636fc2 67 DIR_R = dir;
trieule95 0:b0c572636fc2 68 g_pwm_left = speed;
trieule95 0:b0c572636fc2 69 }
trieule95 0:b0c572636fc2 70 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 71 /*
trieule95 0:b0c572636fc2 72 * @param dir: UP or DOWN; speed: 0-100.
trieule95 0:b0c572636fc2 73 * @return none.
trieule95 0:b0c572636fc2 74 */
trieule95 0:b0c572636fc2 75 void motor_right(uint8_t dir, uint8_t speed)
trieule95 0:b0c572636fc2 76 {
trieule95 0:b0c572636fc2 77 DIR_R = dir;
trieule95 0:b0c572636fc2 78 g_pwm_right = speed;
trieule95 0:b0c572636fc2 79 }
trieule95 0:b0c572636fc2 80 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 81 /*
trieule95 0:b0c572636fc2 82 * @param speed: 0-100.
trieule95 0:b0c572636fc2 83 * @return none.
trieule95 0:b0c572636fc2 84 */
trieule95 0:b0c572636fc2 85 void rotator_left(uint8_t speed)
trieule95 0:b0c572636fc2 86 {
trieule95 0:b0c572636fc2 87 DIR_L = 0;
trieule95 0:b0c572636fc2 88 g_pwm_left = speed;
trieule95 0:b0c572636fc2 89 DIR_R = 1;
trieule95 0:b0c572636fc2 90 g_pwm_right = speed;
trieule95 0:b0c572636fc2 91 g_sts_car = LEFT;
trieule95 0:b0c572636fc2 92 }
trieule95 0:b0c572636fc2 93 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 94 /*
trieule95 0:b0c572636fc2 95 * @param speed: 0-100.
trieule95 0:b0c572636fc2 96 * @return none.
trieule95 0:b0c572636fc2 97 */
trieule95 0:b0c572636fc2 98 void rotator_right(uint8_t speed)
trieule95 0:b0c572636fc2 99 {
trieule95 0:b0c572636fc2 100 DIR_L = 1;
trieule95 0:b0c572636fc2 101 g_pwm_left = speed;
trieule95 0:b0c572636fc2 102 DIR_R = 0;
trieule95 0:b0c572636fc2 103 g_pwm_right = speed;
trieule95 0:b0c572636fc2 104 g_sts_car = RIGHT;
trieule95 0:b0c572636fc2 105 }
trieule95 0:b0c572636fc2 106 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 107 /*
trieule95 0:b0c572636fc2 108 * @param speed_left, speed_right: 0-100.
trieule95 0:b0c572636fc2 109 * @return none.
trieule95 0:b0c572636fc2 110 */
trieule95 0:b0c572636fc2 111 void move_up(uint8_t speed_left, uint8_t speed_right)
trieule95 0:b0c572636fc2 112 {
trieule95 0:b0c572636fc2 113 DIR_L = 0;
trieule95 0:b0c572636fc2 114 g_pwm_left = speed_left;
trieule95 0:b0c572636fc2 115 DIR_R = 0;
trieule95 0:b0c572636fc2 116 g_pwm_right = speed_right;
trieule95 0:b0c572636fc2 117 g_sts_car = UP;
trieule95 0:b0c572636fc2 118 }
trieule95 0:b0c572636fc2 119
trieule95 0:b0c572636fc2 120 void move_around(uint8_t speed_left, uint8_t speed_right)
trieule95 0:b0c572636fc2 121 {
trieule95 0:b0c572636fc2 122 DIR_L = 0;
trieule95 0:b0c572636fc2 123 g_pwm_left = speed_left;
trieule95 0:b0c572636fc2 124 DIR_R = 0;
trieule95 0:b0c572636fc2 125 g_pwm_right = speed_right;
trieule95 0:b0c572636fc2 126 g_sts_car = UP;
trieule95 0:b0c572636fc2 127 }
trieule95 0:b0c572636fc2 128
trieule95 0:b0c572636fc2 129 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 130 /*
trieule95 0:b0c572636fc2 131 * @param speed_left, speed_right: 0-100.
trieule95 0:b0c572636fc2 132 * @return none.
trieule95 0:b0c572636fc2 133 */
trieule95 0:b0c572636fc2 134 void move_down(uint8_t speed_left, uint8_t speed_right)
trieule95 0:b0c572636fc2 135 {
trieule95 0:b0c572636fc2 136 DIR_L = 1;
trieule95 0:b0c572636fc2 137 g_pwm_left = speed_left;
trieule95 0:b0c572636fc2 138 DIR_R = 1;
trieule95 0:b0c572636fc2 139 g_pwm_right = speed_right;
trieule95 0:b0c572636fc2 140 g_sts_car = DOWN;
trieule95 0:b0c572636fc2 141 }
trieule95 0:b0c572636fc2 142
trieule95 0:b0c572636fc2 143 void move_left_down (uint8_t speed_left, uint8_t speed_right)
trieule95 0:b0c572636fc2 144 {
trieule95 0:b0c572636fc2 145 DIR_L = 1;
trieule95 0:b0c572636fc2 146 g_pwm_left = speed_left;
trieule95 0:b0c572636fc2 147 DIR_R = 0;
trieule95 0:b0c572636fc2 148 g_pwm_right = speed_right;
trieule95 0:b0c572636fc2 149 g_sts_car = DOWN;
trieule95 0:b0c572636fc2 150 }
trieule95 0:b0c572636fc2 151
trieule95 0:b0c572636fc2 152 void move_right_down (uint8_t speed_left, uint8_t speed_right)
trieule95 0:b0c572636fc2 153 {
trieule95 0:b0c572636fc2 154 DIR_L = 0;
trieule95 0:b0c572636fc2 155 g_pwm_left = speed_left;
trieule95 0:b0c572636fc2 156 DIR_R = 1;
trieule95 0:b0c572636fc2 157 g_pwm_right = speed_right;
trieule95 0:b0c572636fc2 158 g_sts_car = DOWN;
trieule95 0:b0c572636fc2 159 }
trieule95 0:b0c572636fc2 160
trieule95 0:b0c572636fc2 161 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 162 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 163 /*
trieule95 0:b0c572636fc2 164 * @param none.
trieule95 0:b0c572636fc2 165 * @return Distance (cm), 1-80 cm.
trieule95 0:b0c572636fc2 166 */
trieule95 0:b0c572636fc2 167 uint16_t sensor_ir(void)
trieule95 0:b0c572636fc2 168 {
trieule95 0:b0c572636fc2 169 uint16_t adc;
trieule95 0:b0c572636fc2 170 uint16_t cm;
trieule95 0:b0c572636fc2 171 uint16_t sensor = 0, i;
trieule95 0:b0c572636fc2 172
trieule95 0:b0c572636fc2 173 for(i=0; i<NOISE; i++) {
trieule95 0:b0c572636fc2 174 adc = analog_ir.read_u16();
trieule95 0:b0c572636fc2 175 adc = 750-adc;
trieule95 0:b0c572636fc2 176 if (adc > 60000) cm = 1;
trieule95 0:b0c572636fc2 177 else if (adc > 600) cm = 0;
trieule95 0:b0c572636fc2 178 else if (adc > 550) cm = adc/8;
trieule95 0:b0c572636fc2 179 else if (adc > 500) cm = adc/10;
trieule95 0:b0c572636fc2 180 else if (adc > 450) cm = adc/12;
trieule95 0:b0c572636fc2 181 else if (adc > 400) cm = adc/14;
trieule95 0:b0c572636fc2 182 else if (adc > 350) cm = adc/16;
trieule95 0:b0c572636fc2 183 else if (adc > 300) cm = adc/18;
trieule95 0:b0c572636fc2 184 else if (adc > 200) cm = adc/16;
trieule95 0:b0c572636fc2 185 else if (adc > 200) cm = adc/14;
trieule95 0:b0c572636fc2 186 else if (adc > 150) cm = adc/12;
trieule95 0:b0c572636fc2 187 else if (adc > 100) cm = adc/10;
trieule95 0:b0c572636fc2 188 else if (adc > 60) cm = adc/9;
trieule95 0:b0c572636fc2 189 else if (adc > 30) cm = adc/8;
trieule95 0:b0c572636fc2 190 else if (adc > 0) cm = adc/7;
trieule95 0:b0c572636fc2 191
trieule95 1:552a34826f55 192 //wait(0.001);
trieule95 0:b0c572636fc2 193 sensor = sensor + cm;
trieule95 0:b0c572636fc2 194 if(cm == 0) break;
trieule95 0:b0c572636fc2 195 cm = sensor/NOISE;
trieule95 0:b0c572636fc2 196 }
trieule95 0:b0c572636fc2 197
trieule95 0:b0c572636fc2 198 #ifdef DEBUG
trieule95 0:b0c572636fc2 199 pc.printf("\r\n %d adc, %d cm", adc, cm);
trieule95 0:b0c572636fc2 200 #endif
trieule95 0:b0c572636fc2 201 return cm;
trieule95 0:b0c572636fc2 202 }
trieule95 0:b0c572636fc2 203
trieule95 0:b0c572636fc2 204 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 205 /*
trieule95 0:b0c572636fc2 206 * @param none.
trieule95 0:b0c572636fc2 207 * @return ON or OFF.
trieule95 0:b0c572636fc2 208 */
trieule95 0:b0c572636fc2 209 uint8_t sensor_ir2(void)
trieule95 0:b0c572636fc2 210 {
trieule95 0:b0c572636fc2 211 uint16_t i, sensor = 0;
trieule95 0:b0c572636fc2 212
trieule95 0:b0c572636fc2 213 for(i=0; i<NOISE; i++) {
trieule95 1:552a34826f55 214 //wait(0.001);
trieule95 0:b0c572636fc2 215 sensor = sensor + SEN_IR;
trieule95 0:b0c572636fc2 216 }
trieule95 0:b0c572636fc2 217 if(sensor > NOISE/2) return OFF;
trieule95 0:b0c572636fc2 218 else return ON;
trieule95 0:b0c572636fc2 219 }
trieule95 0:b0c572636fc2 220
trieule95 0:b0c572636fc2 221 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 222 /*
trieule95 0:b0c572636fc2 223 * @param none.
trieule95 0:b0c572636fc2 224 * @return ON or OFF.
trieule95 0:b0c572636fc2 225 */
trieule95 0:b0c572636fc2 226 uint8_t sensor_up_left(void)
trieule95 0:b0c572636fc2 227 {
trieule95 0:b0c572636fc2 228 uint16_t i, sensor = 0;
trieule95 0:b0c572636fc2 229
trieule95 0:b0c572636fc2 230 for(i=0; i<NOISE; i++) {
trieule95 1:552a34826f55 231 //wait(0.001);
trieule95 0:b0c572636fc2 232 sensor = sensor + SEN_UP_L;
trieule95 0:b0c572636fc2 233 }
trieule95 0:b0c572636fc2 234 if(sensor > NOISE/2) return OFF;
trieule95 0:b0c572636fc2 235 else return ON;
trieule95 0:b0c572636fc2 236 }
trieule95 0:b0c572636fc2 237 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 238 /*
trieule95 0:b0c572636fc2 239 * @param none.
trieule95 0:b0c572636fc2 240 * @return ON or OFF.
trieule95 0:b0c572636fc2 241 */
trieule95 0:b0c572636fc2 242 uint8_t sensor_up_right(void)
trieule95 0:b0c572636fc2 243 {
trieule95 0:b0c572636fc2 244 uint16_t i, sensor = 0;
trieule95 0:b0c572636fc2 245
trieule95 0:b0c572636fc2 246 for(i=0; i<NOISE; i++) {
trieule95 1:552a34826f55 247 //wait(0.001);
trieule95 0:b0c572636fc2 248 sensor = sensor + SEN_UP_R;
trieule95 0:b0c572636fc2 249 }
trieule95 0:b0c572636fc2 250 if(sensor > NOISE/2) return OFF;
trieule95 0:b0c572636fc2 251 else return ON;
trieule95 0:b0c572636fc2 252 }
trieule95 0:b0c572636fc2 253 //////////////////////////////////////////////////
trieule95 0:b0c572636fc2 254 /*
trieule95 0:b0c572636fc2 255 * @param none.
trieule95 0:b0c572636fc2 256 * @return ON or OFF.
trieule95 0:b0c572636fc2 257 */
trieule95 0:b0c572636fc2 258 uint8_t sensor_down(void)
trieule95 0:b0c572636fc2 259 {
trieule95 0:b0c572636fc2 260 uint16_t i, sensor = 0;
trieule95 0:b0c572636fc2 261
trieule95 0:b0c572636fc2 262 for(i=0; i<NOISE; i++) {
trieule95 1:552a34826f55 263 //wait(0.001);
trieule95 0:b0c572636fc2 264 sensor = sensor + SEN_DOWN;
trieule95 0:b0c572636fc2 265 }
trieule95 0:b0c572636fc2 266 if(sensor > NOISE/2) return OFF;
trieule95 0:b0c572636fc2 267 else return ON;
trieule95 0:b0c572636fc2 268 }
trieule95 0:b0c572636fc2 269 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 270 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 271 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 272 void periodicCallback(void)
trieule95 0:b0c572636fc2 273 {
trieule95 0:b0c572636fc2 274 if(g_time_pwm < g_pwm_left) PWM_L = 1; else PWM_L = 0;
trieule95 0:b0c572636fc2 275 if(g_time_pwm < g_pwm_right) PWM_R = 1; else PWM_R = 0;
trieule95 0:b0c572636fc2 276 g_time_pwm++; if(g_time_pwm >= 100) {g_time_pwm = 0;}
trieule95 0:b0c572636fc2 277
trieule95 0:b0c572636fc2 278 g_time_led++; if(g_time_led >= 1000) {g_time_led = 0; led1 = !led1;} //DIR_L = !DIR_L; DIR_R = !DIR_R;
trieule95 0:b0c572636fc2 279
trieule95 0:b0c572636fc2 280 }
trieule95 0:b0c572636fc2 281
trieule95 0:b0c572636fc2 282 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 283 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 284 //////////////////////////////////////////////////////////////////////////////////////////////////
trieule95 0:b0c572636fc2 285
trieule95 0:b0c572636fc2 286 bool randomBool() {
trieule95 0:b0c572636fc2 287 return rand() % 2 == 1;
trieule95 0:b0c572636fc2 288 }
trieule95 0:b0c572636fc2 289
trieule95 1:552a34826f55 290 bool isInDanger(void){
trieule95 1:552a34826f55 291 return ((sensor_up_left() == OFF) || (sensor_up_right() == OFF) || (sensor_down() == OFF));
trieule95 1:552a34826f55 292 }
trieule95 1:552a34826f55 293
trieule95 0:b0c572636fc2 294 int main(void)
trieule95 0:b0c572636fc2 295 {
trieule95 1:552a34826f55 296 //wait(0.1);
trieule95 0:b0c572636fc2 297 //Init Hardware
trieule95 0:b0c572636fc2 298 SEN_UP_L.mode(PullUp);
trieule95 0:b0c572636fc2 299 SEN_UP_R.mode(PullUp);
trieule95 0:b0c572636fc2 300 SEN_DOWN.mode(PullUp);
trieule95 0:b0c572636fc2 301 SEN_IR.mode(PullUp);
trieule95 0:b0c572636fc2 302 led1 = 1; DIR_L = 0; DIR_R = 0; PWM_L = 0; PWM_R = 0;
trieule95 0:b0c572636fc2 303 //Init interupt Timer
trieule95 0:b0c572636fc2 304 ticker.attach(periodicCallback, 0.00015);
trieule95 1:552a34826f55 305 //wait(0.5);
trieule95 1:552a34826f55 306 uint8_t sen_in = sensor_ir();
trieule95 0:b0c572636fc2 307 while (true) {
trieule95 1:552a34826f55 308 sen_in = sensor_ir();
trieule95 1:552a34826f55 309 if(isInDanger()){
trieule95 1:552a34826f55 310 g_sts_car = STOP;
trieule95 1:552a34826f55 311 //if (sensor_down() == OFF && sensor_up_left() == OFF){
trieule95 1:552a34826f55 312 // rotator_right(30);
trieule95 1:552a34826f55 313 // while(sensor_down() == OFF && sensor_up_left() == OFF);
trieule95 1:552a34826f55 314 // uint8_t timer = 0;
trieule95 1:552a34826f55 315 // while(!isInDanger() && timer < 50){
trieule95 1:552a34826f55 316 // //wait(0.01);
trieule95 1:552a34826f55 317 // timer++;
trieule95 1:552a34826f55 318 // }
trieule95 1:552a34826f55 319 // move_up(70,70);
trieule95 1:552a34826f55 320 // while(!isInDanger() && timer < 100){
trieule95 1:552a34826f55 321 // //wait(0.01);
trieule95 1:552a34826f55 322 // timer++;
trieule95 1:552a34826f55 323 // }
trieule95 1:552a34826f55 324 // g_sts_car = STOP;
trieule95 1:552a34826f55 325 // }
trieule95 1:552a34826f55 326 // else if(sensor_down() == OFF && sensor_up_right() == OFF){
trieule95 1:552a34826f55 327 // rotator_left(30);
trieule95 1:552a34826f55 328 // while(sensor_down() == OFF && sensor_up_right() == OFF);
trieule95 1:552a34826f55 329 // uint8_t timer = 0;
trieule95 1:552a34826f55 330 // while(!isInDanger() && timer < 50){
trieule95 1:552a34826f55 331 // //wait(0.01);
trieule95 1:552a34826f55 332 // timer = timer + 1;
trieule95 1:552a34826f55 333 // }
trieule95 1:552a34826f55 334 // move_up(70,70);
trieule95 1:552a34826f55 335 // while(!isInDanger() && timer < 100){
trieule95 1:552a34826f55 336 // //wait(0.01);
trieule95 1:552a34826f55 337 // timer = timer + 1;
trieule95 1:552a34826f55 338 // }
trieule95 1:552a34826f55 339 // g_sts_car = STOP;
trieule95 1:552a34826f55 340 // }
trieule95 1:552a34826f55 341 // else
trieule95 1:552a34826f55 342 if(sensor_up_left() == OFF && sensor_up_right() == OFF){
trieule95 1:552a34826f55 343 move_down(100,100);
trieule95 1:552a34826f55 344 while(sensor_up_left() == OFF && sensor_up_right() == OFF);
trieule95 1:552a34826f55 345 uint8_t timer = 0;
trieule95 1:552a34826f55 346 move_down(30,100);
trieule95 1:552a34826f55 347 while((isInDanger() == false) && timer < 150){
trieule95 1:552a34826f55 348 wait(0.001);
trieule95 1:552a34826f55 349 timer++;
trieule95 1:552a34826f55 350 }
trieule95 1:552a34826f55 351 g_sts_car = STOP;
trieule95 1:552a34826f55 352 }
trieule95 1:552a34826f55 353 else if(sensor_up_left() == OFF){
trieule95 1:552a34826f55 354 move_down(30,100);
trieule95 1:552a34826f55 355 while(sensor_up_left() == OFF);
trieule95 1:552a34826f55 356 uint8_t timer = 0;
trieule95 1:552a34826f55 357 while((isInDanger() == false) && timer < 150){
trieule95 1:552a34826f55 358 wait(0.001);
trieule95 1:552a34826f55 359 timer++;
trieule95 1:552a34826f55 360 }
trieule95 1:552a34826f55 361 // move_up(70,70);
trieule95 1:552a34826f55 362 // while((isInDanger() == false) && timer < 40){
trieule95 1:552a34826f55 363 // timer++;
trieule95 1:552a34826f55 364 // }
trieule95 1:552a34826f55 365 g_sts_car = STOP;
trieule95 1:552a34826f55 366 }
trieule95 1:552a34826f55 367 else if(sensor_up_right() == OFF){
trieule95 1:552a34826f55 368 move_down(100,30);
trieule95 1:552a34826f55 369 while(sensor_up_right() == OFF);
trieule95 1:552a34826f55 370 uint8_t timer = 0;
trieule95 1:552a34826f55 371 while((isInDanger() == false) && timer < 150){
trieule95 1:552a34826f55 372 wait(0.001);
trieule95 1:552a34826f55 373 timer++;
trieule95 1:552a34826f55 374 }
trieule95 1:552a34826f55 375 // move_up(70,70);
trieule95 1:552a34826f55 376 // while((isInDanger() == false) && timer < 40){
trieule95 1:552a34826f55 377 // timer++;
trieule95 1:552a34826f55 378 // }
trieule95 1:552a34826f55 379 g_sts_car = STOP;
trieule95 1:552a34826f55 380 }
trieule95 1:552a34826f55 381 else if (sensor_down() == OFF) {
trieule95 1:552a34826f55 382 move_up(100,100);
trieule95 1:552a34826f55 383 while(sensor_down() == OFF);
trieule95 1:552a34826f55 384 g_sts_car = STOP;
trieule95 1:552a34826f55 385 }
trieule95 1:552a34826f55 386 }
trieule95 1:552a34826f55 387 else{
trieule95 1:552a34826f55 388 g_sts_car = STOP;
trieule95 1:552a34826f55 389 if(sen_in > 0 && sen_in <30){
trieule95 1:552a34826f55 390 move_up(100,100);
trieule95 1:552a34826f55 391 while((isInDanger() == false) && sensor_ir() > 0 && sensor_ir() <30);
trieule95 1:552a34826f55 392 g_sts_car = STOP;
trieule95 1:552a34826f55 393 }
trieule95 1:552a34826f55 394 else {
trieule95 1:552a34826f55 395 rotator_right(20);
trieule95 1:552a34826f55 396 }
trieule95 1:552a34826f55 397 }
trieule95 1:552a34826f55 398
trieule95 0:b0c572636fc2 399 }
trieule95 1:552a34826f55 400 }