Robotics - ARM / Mbed 2 deprecated ARM_Robotics

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?

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