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