Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

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?

UserRevisionLine numberNew 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 }