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