Robot code for searching an object and charging at it.
Dependencies: HCSR04 Motor mbed
functions.cpp@37:a08d2e37b7e6, 2016-10-12 (annotated)
- Committer:
- alex0612
- Date:
- Wed Oct 12 11:13:45 2016 +0000
- Revision:
- 37:a08d2e37b7e6
- Parent:
- 36:0a69dc7bfcf9
- Child:
- 38:decff231d886
Cleaned up functions names and added functions to turn and light LEDs
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
abdsha01 | 0:15664f71b21f | 1 | // The following file implements all the functions necessary |
abdsha01 | 0:15664f71b21f | 2 | // for the robot. |
abdsha01 | 0:15664f71b21f | 3 | // Please don't modify any of these functions! |
abdsha01 | 0:15664f71b21f | 4 | |
abdsha01 | 0:15664f71b21f | 5 | #include "mbed.h" |
abdsha01 | 0:15664f71b21f | 6 | #include "Motor.h" |
abdsha01 | 0:15664f71b21f | 7 | #include "hcsr04.h" |
abdsha01 | 4:0507835a3dce | 8 | #include "functions.h" |
abdsha01 | 4:0507835a3dce | 9 | |
alex0612 | 19:67ea4e8be9e1 | 10 | DigitalOut myled1(LED1); |
alex0612 | 19:67ea4e8be9e1 | 11 | DigitalOut myled2(LED2); |
alex0612 | 19:67ea4e8be9e1 | 12 | DigitalOut myled3(LED3); |
alex0612 | 19:67ea4e8be9e1 | 13 | DigitalOut myled4(LED4); |
alex0612 | 19:67ea4e8be9e1 | 14 | |
abdsha01 | 4:0507835a3dce | 15 | // Two sensors are used to detect a line, line_sens1 and line_sens2 |
abdsha01 | 4:0507835a3dce | 16 | // if there is a difference between the readings of these sensors |
abdsha01 | 4:0507835a3dce | 17 | // the robot has detected a line. |
abdsha01 | 4:0507835a3dce | 18 | // Setting pins for line sensor |
lhartfield | 36:0a69dc7bfcf9 | 19 | DigitalInOut line1(p29); |
lhartfield | 36:0a69dc7bfcf9 | 20 | DigitalInOut line2(p22); |
abdsha01 | 4:0507835a3dce | 21 | |
abdsha01 | 4:0507835a3dce | 22 | // Setting pins for motor, as follows: |
abdsha01 | 4:0507835a3dce | 23 | // Example: Motor____(PWM, Forward, Reverse) |
lhartfield | 36:0a69dc7bfcf9 | 24 | Motor MotorLeft(p24, p13, p14); // pwm, fwd, rev |
lhartfield | 36:0a69dc7bfcf9 | 25 | Motor MotorRight(p23, p11, p10); // pwm, fwd, rev |
abdsha01 | 4:0507835a3dce | 26 | |
abdsha01 | 4:0507835a3dce | 27 | // Setting pins for ultrasonic sensor, as follows: |
abdsha01 | 4:0507835a3dce | 28 | // Example: usensor(Trigger, Echo) |
abdsha01 | 4:0507835a3dce | 29 | HCSR04 usensor(p25,p26); |
abdsha01 | 0:15664f71b21f | 30 | |
alex0612 | 37:a08d2e37b7e6 | 31 | void turn_led_on(int LED) { |
alex0612 | 37:a08d2e37b7e6 | 32 | switch(LED) { |
alex0612 | 37:a08d2e37b7e6 | 33 | case 1: |
alex0612 | 37:a08d2e37b7e6 | 34 | myled1 = 1; |
alex0612 | 37:a08d2e37b7e6 | 35 | wait(0.1); |
alex0612 | 37:a08d2e37b7e6 | 36 | myled1 = 0; |
alex0612 | 37:a08d2e37b7e6 | 37 | case 2: |
alex0612 | 37:a08d2e37b7e6 | 38 | myled2 = 1; |
alex0612 | 37:a08d2e37b7e6 | 39 | wait(0.1); |
alex0612 | 37:a08d2e37b7e6 | 40 | myled2 = 0; |
alex0612 | 37:a08d2e37b7e6 | 41 | case 3: |
alex0612 | 37:a08d2e37b7e6 | 42 | myled3 = 1; |
alex0612 | 37:a08d2e37b7e6 | 43 | wait(0.1); |
alex0612 | 37:a08d2e37b7e6 | 44 | myled3 = 0; |
alex0612 | 37:a08d2e37b7e6 | 45 | case 4: |
alex0612 | 37:a08d2e37b7e6 | 46 | myled4 = 1; |
alex0612 | 37:a08d2e37b7e6 | 47 | wait(0.1); |
alex0612 | 37:a08d2e37b7e6 | 48 | myled4 = 0; |
alex0612 | 37:a08d2e37b7e6 | 49 | } |
alex0612 | 37:a08d2e37b7e6 | 50 | } |
alex0612 | 37:a08d2e37b7e6 | 51 | |
alex0612 | 19:67ea4e8be9e1 | 52 | void turn_leds_on() { |
alex0612 | 19:67ea4e8be9e1 | 53 | myled1 = 1; |
alex0612 | 19:67ea4e8be9e1 | 54 | myled2 = 1; |
alex0612 | 19:67ea4e8be9e1 | 55 | myled3 = 1; |
alex0612 | 19:67ea4e8be9e1 | 56 | myled4 = 1; |
alex0612 | 19:67ea4e8be9e1 | 57 | wait(0.1); |
alex0612 | 19:67ea4e8be9e1 | 58 | myled1 = 0; |
alex0612 | 19:67ea4e8be9e1 | 59 | myled2 = 0; |
alex0612 | 19:67ea4e8be9e1 | 60 | myled3 = 0; |
alex0612 | 19:67ea4e8be9e1 | 61 | myled4 = 0; |
alex0612 | 19:67ea4e8be9e1 | 62 | } |
alex0612 | 19:67ea4e8be9e1 | 63 | |
abdsha01 | 26:5ee2a32949e6 | 64 | void turn_led_left() { |
abdsha01 | 26:5ee2a32949e6 | 65 | myled1 = 1; |
abdsha01 | 26:5ee2a32949e6 | 66 | myled2 = 1; |
abdsha01 | 26:5ee2a32949e6 | 67 | wait(0.1); |
abdsha01 | 26:5ee2a32949e6 | 68 | myled1 = 0; |
abdsha01 | 26:5ee2a32949e6 | 69 | myled2 = 0; |
abdsha01 | 26:5ee2a32949e6 | 70 | } |
abdsha01 | 26:5ee2a32949e6 | 71 | |
abdsha01 | 26:5ee2a32949e6 | 72 | void turn_led_right() { |
abdsha01 | 26:5ee2a32949e6 | 73 | myled3 = 1; |
abdsha01 | 26:5ee2a32949e6 | 74 | myled4 = 1; |
abdsha01 | 26:5ee2a32949e6 | 75 | wait(0.1); |
abdsha01 | 26:5ee2a32949e6 | 76 | myled3 = 0; |
abdsha01 | 26:5ee2a32949e6 | 77 | myled4 = 0; |
abdsha01 | 26:5ee2a32949e6 | 78 | } |
abdsha01 | 26:5ee2a32949e6 | 79 | |
alex0612 | 19:67ea4e8be9e1 | 80 | void flash_leds() { |
alex0612 | 19:67ea4e8be9e1 | 81 | Timer t; |
alex0612 | 19:67ea4e8be9e1 | 82 | t.start(); |
alex0612 | 19:67ea4e8be9e1 | 83 | while(t.read_ms() < 1000) { |
alex0612 | 19:67ea4e8be9e1 | 84 | myled1 = 0; |
alex0612 | 19:67ea4e8be9e1 | 85 | myled2 = 0; |
alex0612 | 19:67ea4e8be9e1 | 86 | myled3 = 0; |
alex0612 | 19:67ea4e8be9e1 | 87 | myled4 = 0; |
alex0612 | 19:67ea4e8be9e1 | 88 | wait(0.15); |
alex0612 | 19:67ea4e8be9e1 | 89 | myled1 = 1; |
alex0612 | 19:67ea4e8be9e1 | 90 | wait(0.15); |
alex0612 | 19:67ea4e8be9e1 | 91 | myled2 = 1; |
alex0612 | 19:67ea4e8be9e1 | 92 | wait(0.15); |
alex0612 | 19:67ea4e8be9e1 | 93 | myled3 = 1; |
alex0612 | 19:67ea4e8be9e1 | 94 | wait(0.15); |
alex0612 | 19:67ea4e8be9e1 | 95 | myled4 = 1; |
alex0612 | 19:67ea4e8be9e1 | 96 | wait(0.15); |
alex0612 | 19:67ea4e8be9e1 | 97 | myled1 = 0; |
alex0612 | 19:67ea4e8be9e1 | 98 | myled2 = 0; |
alex0612 | 19:67ea4e8be9e1 | 99 | myled3 = 0; |
alex0612 | 19:67ea4e8be9e1 | 100 | myled4 = 0; |
alex0612 | 19:67ea4e8be9e1 | 101 | } |
alex0612 | 19:67ea4e8be9e1 | 102 | t.stop(); |
alex0612 | 19:67ea4e8be9e1 | 103 | } |
alex0612 | 19:67ea4e8be9e1 | 104 | |
abdsha01 | 0:15664f71b21f | 105 | // Returns value from the QRE1113 |
abdsha01 | 0:15664f71b21f | 106 | // lower numbers mean more refleacive |
abdsha01 | 0:15664f71b21f | 107 | // more than 3000 means nothing was reflected. |
abdsha01 | 6:af897173cb75 | 108 | int read_line1() { |
abdsha01 | 0:15664f71b21f | 109 | // Time to record how long input is active |
abdsha01 | 1:bd88d4062c97 | 110 | Timer temp_t; |
abdsha01 | 0:15664f71b21f | 111 | |
abdsha01 | 0:15664f71b21f | 112 | // Activate the line sensor and then read |
abdsha01 | 0:15664f71b21f | 113 | line1.output(); |
abdsha01 | 0:15664f71b21f | 114 | line1 = 1; |
abdsha01 | 0:15664f71b21f | 115 | wait_us(15); |
abdsha01 | 0:15664f71b21f | 116 | line1.input(); |
abdsha01 | 0:15664f71b21f | 117 | |
abdsha01 | 0:15664f71b21f | 118 | // Start timer |
abdsha01 | 1:bd88d4062c97 | 119 | temp_t.start(); |
abdsha01 | 0:15664f71b21f | 120 | |
abdsha01 | 0:15664f71b21f | 121 | // Time how long the input is HIGH, but quit |
abdsha01 | 0:15664f71b21f | 122 | // after 1ms as nothing happens after that |
abdsha01 | 1:bd88d4062c97 | 123 | while (line1 == 1 && temp_t.read_us() < 1000); |
abdsha01 | 1:bd88d4062c97 | 124 | return temp_t.read_us(); |
abdsha01 | 0:15664f71b21f | 125 | } |
abdsha01 | 0:15664f71b21f | 126 | |
abdsha01 | 6:af897173cb75 | 127 | int read_line2() { |
abdsha01 | 0:15664f71b21f | 128 | // Time to record how long input is active |
abdsha01 | 1:bd88d4062c97 | 129 | Timer temp_t; |
abdsha01 | 0:15664f71b21f | 130 | |
abdsha01 | 0:15664f71b21f | 131 | // Activate the line sensor and then read |
abdsha01 | 0:15664f71b21f | 132 | line2.output(); |
abdsha01 | 0:15664f71b21f | 133 | line2 = 1; |
abdsha01 | 0:15664f71b21f | 134 | wait_us(15); |
abdsha01 | 0:15664f71b21f | 135 | line2.input(); |
abdsha01 | 0:15664f71b21f | 136 | |
abdsha01 | 0:15664f71b21f | 137 | // Start timer |
abdsha01 | 4:0507835a3dce | 138 | temp_t.start(); |
abdsha01 | 0:15664f71b21f | 139 | |
abdsha01 | 0:15664f71b21f | 140 | // Time how long the input is HIGH, but quit |
abdsha01 | 0:15664f71b21f | 141 | // after 1ms as nothing happens after that |
abdsha01 | 1:bd88d4062c97 | 142 | while (line2 == 1 && temp_t.read_us() < 1000); |
abdsha01 | 4:0507835a3dce | 143 | return temp_t.read_us(); |
abdsha01 | 0:15664f71b21f | 144 | } |
abdsha01 | 0:15664f71b21f | 145 | |
abdsha01 | 6:af897173cb75 | 146 | int detect_line() { |
abdsha01 | 0:15664f71b21f | 147 | int line1val = read_line1(); |
abdsha01 | 0:15664f71b21f | 148 | int line2val = read_line2(); |
abdsha01 | 0:15664f71b21f | 149 | |
abdsha01 | 26:5ee2a32949e6 | 150 | printf("LineSensor readings: %d \n", line1val-line2val); |
abdsha01 | 26:5ee2a32949e6 | 151 | |
abdsha01 | 26:5ee2a32949e6 | 152 | if ((line1val-line2val) > 70) { |
abdsha01 | 26:5ee2a32949e6 | 153 | //printf("Line detected from front"); |
abdsha01 | 0:15664f71b21f | 154 | return 1; |
abdsha01 | 26:5ee2a32949e6 | 155 | } else if ((line1val-line2val) < -70) { |
abdsha01 | 26:5ee2a32949e6 | 156 | //printf("Line detected from back"); |
abdsha01 | 0:15664f71b21f | 157 | return -1; |
abdsha01 | 0:15664f71b21f | 158 | } else { |
abdsha01 | 26:5ee2a32949e6 | 159 | //printf("Line not detected"); |
abdsha01 | 0:15664f71b21f | 160 | return 0; |
abdsha01 | 0:15664f71b21f | 161 | } |
abdsha01 | 0:15664f71b21f | 162 | } |
abdsha01 | 0:15664f71b21f | 163 | |
alex0612 | 30:8a5570b2de68 | 164 | void reverse(float speed, float adj) { |
abdsha01 | 26:5ee2a32949e6 | 165 | //printf("Reverse\n"); |
alex0612 | 33:4d0fe8fbed68 | 166 | MotorLeft.speed(-(speed)-adj); |
alex0612 | 33:4d0fe8fbed68 | 167 | MotorRight.speed(-(speed)); |
abdsha01 | 9:7770a84228c0 | 168 | } |
abdsha01 | 9:7770a84228c0 | 169 | |
alex0612 | 37:a08d2e37b7e6 | 170 | void turn_left(float speed) { |
alex0612 | 37:a08d2e37b7e6 | 171 | //printf("Turning left\n"); |
abdsha01 | 9:7770a84228c0 | 172 | MotorLeft.speed(speed); |
abdsha01 | 9:7770a84228c0 | 173 | MotorRight.speed(-(speed)); |
abdsha01 | 0:15664f71b21f | 174 | } |
abdsha01 | 0:15664f71b21f | 175 | |
alex0612 | 37:a08d2e37b7e6 | 176 | void turn_right(float speed) { |
alex0612 | 37:a08d2e37b7e6 | 177 | //printf("Turning right\n"); |
alex0612 | 37:a08d2e37b7e6 | 178 | MotorLeft.speed(-(speed)); |
alex0612 | 37:a08d2e37b7e6 | 179 | MotorRight.speed(speed); |
alex0612 | 37:a08d2e37b7e6 | 180 | } |
alex0612 | 37:a08d2e37b7e6 | 181 | |
abdsha01 | 26:5ee2a32949e6 | 182 | void reverseandturn(float speed) { |
abdsha01 | 26:5ee2a32949e6 | 183 | //printf("Reverse and turn\n"); |
abdsha01 | 4:0507835a3dce | 184 | MotorLeft.speed((speed-0.3)); |
abdsha01 | 4:0507835a3dce | 185 | MotorRight.speed(-(speed-0.1)); |
abdsha01 | 0:15664f71b21f | 186 | } |
abdsha01 | 0:15664f71b21f | 187 | |
alex0612 | 37:a08d2e37b7e6 | 188 | void forwards(float speed, float adj) { |
alex0612 | 33:4d0fe8fbed68 | 189 | MotorLeft.speed(speed+adj); |
alex0612 | 33:4d0fe8fbed68 | 190 | MotorRight.speed(speed); |
abdsha01 | 1:bd88d4062c97 | 191 | } |
abdsha01 | 1:bd88d4062c97 | 192 | |
lhartfield | 22:e808fb71847d | 193 | void move_random(float speed) |
alex0612 | 7:d94d23c55015 | 194 | { |
alex0612 | 7:d94d23c55015 | 195 | int counter; |
alex0612 | 11:b45798cc3c10 | 196 | int fwd_bck; |
alex0612 | 11:b45798cc3c10 | 197 | int fwd; |
alex0612 | 7:d94d23c55015 | 198 | float random_time; |
alex0612 | 7:d94d23c55015 | 199 | |
abdsha01 | 26:5ee2a32949e6 | 200 | //printf("Moving randomly\n"); |
alex0612 | 19:67ea4e8be9e1 | 201 | counter = rand() % 5; |
alex0612 | 7:d94d23c55015 | 202 | |
alex0612 | 7:d94d23c55015 | 203 | for (int i = 0; i < counter; i++) |
alex0612 | 7:d94d23c55015 | 204 | { |
alex0612 | 7:d94d23c55015 | 205 | random_time = rand() % 1000 + 1000; |
alex0612 | 7:d94d23c55015 | 206 | |
alex0612 | 11:b45798cc3c10 | 207 | fwd_bck = rand()%2; |
alex0612 | 19:67ea4e8be9e1 | 208 | |
alex0612 | 11:b45798cc3c10 | 209 | // If fwd_back == 1 move forward or backwards |
alex0612 | 11:b45798cc3c10 | 210 | if (fwd_bck == 1) { |
alex0612 | 11:b45798cc3c10 | 211 | fwd = rand()%2; |
alex0612 | 11:b45798cc3c10 | 212 | // If fwd == 1 move forward |
alex0612 | 11:b45798cc3c10 | 213 | if (fwd == 1) { |
abdsha01 | 26:5ee2a32949e6 | 214 | //printf("Moving forward\n"); |
lhartfield | 22:e808fb71847d | 215 | move_detect(speed, fwd_bck, random_time); |
alex0612 | 11:b45798cc3c10 | 216 | // If fwd == 0 move bacward |
alex0612 | 7:d94d23c55015 | 217 | } else { |
abdsha01 | 26:5ee2a32949e6 | 218 | //printf("Moving bacwards\n"); |
lhartfield | 22:e808fb71847d | 219 | move_detect(speed, fwd_bck, random_time); |
alex0612 | 7:d94d23c55015 | 220 | } |
alex0612 | 11:b45798cc3c10 | 221 | // Turn |
alex0612 | 7:d94d23c55015 | 222 | } else { |
abdsha01 | 26:5ee2a32949e6 | 223 | //printf("Turning\n"); |
lhartfield | 22:e808fb71847d | 224 | move_detect(speed, fwd_bck, random_time); |
alex0612 | 7:d94d23c55015 | 225 | } |
alex0612 | 7:d94d23c55015 | 226 | } |
alex0612 | 7:d94d23c55015 | 227 | } |
alex0612 | 7:d94d23c55015 | 228 | |
abdsha01 | 6:af897173cb75 | 229 | void stop() { |
abdsha01 | 4:0507835a3dce | 230 | MotorLeft.speed(0.0); |
abdsha01 | 4:0507835a3dce | 231 | MotorRight.speed(0.0); |
abdsha01 | 4:0507835a3dce | 232 | } |
abdsha01 | 4:0507835a3dce | 233 | |
abdsha01 | 26:5ee2a32949e6 | 234 | int detect_object(int range_t, float speed) { |
alex0612 | 14:e639a42edf2e | 235 | // Start a timer - finds an object for 5 seconds |
abdsha01 | 1:bd88d4062c97 | 236 | // if it doesn't find anything returns 0 |
abdsha01 | 6:af897173cb75 | 237 | Timer usensor_t, inner_t; |
abdsha01 | 1:bd88d4062c97 | 238 | usensor_t.start(); |
abdsha01 | 1:bd88d4062c97 | 239 | |
abdsha01 | 1:bd88d4062c97 | 240 | // Variable to store sensed value |
abdsha01 | 6:af897173cb75 | 241 | unsigned int sense, dist, reverse; |
abdsha01 | 4:0507835a3dce | 242 | sense = 0; |
abdsha01 | 4:0507835a3dce | 243 | dist = 0; |
abdsha01 | 6:af897173cb75 | 244 | reverse = 0; |
abdsha01 | 26:5ee2a32949e6 | 245 | |
alex0612 | 30:8a5570b2de68 | 246 | while (usensor_t.read_ms() < 10000) { |
abdsha01 | 1:bd88d4062c97 | 247 | // Start the ultrasonic sensor |
abdsha01 | 1:bd88d4062c97 | 248 | usensor.start(); |
abdsha01 | 6:af897173cb75 | 249 | inner_t.start(); |
abdsha01 | 1:bd88d4062c97 | 250 | dist = usensor.get_dist_cm(); |
abdsha01 | 1:bd88d4062c97 | 251 | |
abdsha01 | 26:5ee2a32949e6 | 252 | printf("UltraSonic readings: %d \n", dist); |
abdsha01 | 26:5ee2a32949e6 | 253 | |
abdsha01 | 1:bd88d4062c97 | 254 | // If an object is detected based on out set range return 1 |
abdsha01 | 21:42c0db071a7f | 255 | if (dist <= range_t && dist >= 1) { |
abdsha01 | 1:bd88d4062c97 | 256 | sense = 1; |
alex0612 | 19:67ea4e8be9e1 | 257 | stop(); |
alex0612 | 19:67ea4e8be9e1 | 258 | flash_leds(); |
abdsha01 | 1:bd88d4062c97 | 259 | break; |
abdsha01 | 1:bd88d4062c97 | 260 | } else { |
abdsha01 | 1:bd88d4062c97 | 261 | sense = 0; |
alex0612 | 37:a08d2e37b7e6 | 262 | turn_left(speed); |
abdsha01 | 1:bd88d4062c97 | 263 | } |
abdsha01 | 6:af897173cb75 | 264 | |
abdsha01 | 6:af897173cb75 | 265 | if (inner_t.read_ms() >=100) { |
abdsha01 | 6:af897173cb75 | 266 | if (reverse == 2) { |
abdsha01 | 6:af897173cb75 | 267 | speed = 0.7; |
abdsha01 | 6:af897173cb75 | 268 | reverse = 0; |
abdsha01 | 6:af897173cb75 | 269 | } else { |
abdsha01 | 6:af897173cb75 | 270 | speed = 0.0; |
abdsha01 | 6:af897173cb75 | 271 | } |
abdsha01 | 6:af897173cb75 | 272 | reverse++; |
abdsha01 | 6:af897173cb75 | 273 | inner_t.reset(); |
abdsha01 | 6:af897173cb75 | 274 | } |
abdsha01 | 1:bd88d4062c97 | 275 | } |
abdsha01 | 6:af897173cb75 | 276 | |
abdsha01 | 6:af897173cb75 | 277 | usensor_t.stop(); |
abdsha01 | 6:af897173cb75 | 278 | usensor_t.reset(); |
abdsha01 | 1:bd88d4062c97 | 279 | return sense; |
alex0612 | 11:b45798cc3c10 | 280 | } |
alex0612 | 11:b45798cc3c10 | 281 | |
lhartfield | 27:96a0ff2f474c | 282 | void move_detect(float speed, int fwd_bck, int time, int wait) { |
alex0612 | 11:b45798cc3c10 | 283 | Timer t; |
alex0612 | 11:b45798cc3c10 | 284 | t.start(); |
alex0612 | 11:b45798cc3c10 | 285 | int detect = 0; |
alex0612 | 11:b45798cc3c10 | 286 | |
alex0612 | 19:67ea4e8be9e1 | 287 | if(fwd_bck == 1) { |
alex0612 | 37:a08d2e37b7e6 | 288 | forwards(speed); |
lhartfield | 27:96a0ff2f474c | 289 | wait_ms(wait); |
alex0612 | 19:67ea4e8be9e1 | 290 | } else { |
alex0612 | 37:a08d2e37b7e6 | 291 | turn_left(speed + 0.2); |
alex0612 | 19:67ea4e8be9e1 | 292 | } |
alex0612 | 19:67ea4e8be9e1 | 293 | |
alex0612 | 11:b45798cc3c10 | 294 | while (t.read_ms() < time) { |
alex0612 | 11:b45798cc3c10 | 295 | detect = detect_line(); |
alex0612 | 11:b45798cc3c10 | 296 | // If line is detected from front then reverse |
alex0612 | 11:b45798cc3c10 | 297 | if(detect == 1) { |
alex0612 | 19:67ea4e8be9e1 | 298 | stop(); |
alex0612 | 19:67ea4e8be9e1 | 299 | turn_leds_on(); |
alex0612 | 30:8a5570b2de68 | 300 | move_detect(-forwardspeed,1,1000,300); |
alex0612 | 11:b45798cc3c10 | 301 | stop(); |
alex0612 | 11:b45798cc3c10 | 302 | break; |
alex0612 | 17:47aa9ef2bec6 | 303 | // If line is detected from back just keep on moving forward |
alex0612 | 11:b45798cc3c10 | 304 | } else if (detect == -1) { |
alex0612 | 19:67ea4e8be9e1 | 305 | stop(); |
alex0612 | 19:67ea4e8be9e1 | 306 | turn_leds_on(); |
alex0612 | 30:8a5570b2de68 | 307 | move_detect(forwardspeed,1,1000,300); |
alex0612 | 11:b45798cc3c10 | 308 | stop(); |
alex0612 | 11:b45798cc3c10 | 309 | break; |
alex0612 | 11:b45798cc3c10 | 310 | } |
alex0612 | 11:b45798cc3c10 | 311 | } |
alex0612 | 19:67ea4e8be9e1 | 312 | stop(); |
alex0612 | 11:b45798cc3c10 | 313 | t.stop(); |
abdsha01 | 0:15664f71b21f | 314 | } |