Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@19:67ea4e8be9e1, 2015-05-30 (annotated)

Committer:
alex0612
Date:
Sat May 30 19:08:26 2015 +0000
Revision:
19:67ea4e8be9e1
Parent:
17:47aa9ef2bec6
Child:
20:37a89edd1cde
Added led blinking

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