Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@22:e808fb71847d, 2015-06-07 (annotated)

Committer:
lhartfield
Date:
Sun Jun 07 15:03:54 2015 +0000
Revision:
22:e808fb71847d
Parent:
20:37a89edd1cde
Child:
23:07b3c12800a6
Changed function name from moverandom to move_random for consistency. Parameterized move_random().

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
lhartfield 22:e808fb71847d 148 void move_random(float speed)
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
alex0612 7:d94d23c55015 155 printf("Moving randomly\n");
alex0612 19:67ea4e8be9e1 156 counter = rand() % 5;
alex0612 7:d94d23c55015 157
alex0612 7:d94d23c55015 158 for (int i = 0; i < counter; i++)
alex0612 7:d94d23c55015 159 {
alex0612 7:d94d23c55015 160 random_time = rand() % 1000 + 1000;
alex0612 7:d94d23c55015 161
alex0612 11:b45798cc3c10 162 fwd_bck = rand()%2;
alex0612 19:67ea4e8be9e1 163
alex0612 11:b45798cc3c10 164 // If fwd_back == 1 move forward or backwards
alex0612 11:b45798cc3c10 165 if (fwd_bck == 1) {
alex0612 11:b45798cc3c10 166 fwd = rand()%2;
alex0612 11:b45798cc3c10 167 // If fwd == 1 move forward
alex0612 11:b45798cc3c10 168 if (fwd == 1) {
alex0612 14:e639a42edf2e 169 printf("Moving forward\n");
lhartfield 22:e808fb71847d 170 move_detect(speed, fwd_bck, random_time);
alex0612 11:b45798cc3c10 171 // If fwd == 0 move bacward
alex0612 7:d94d23c55015 172 } else {
alex0612 14:e639a42edf2e 173 printf("Moving bacwards\n");
lhartfield 22:e808fb71847d 174 move_detect(speed, fwd_bck, random_time);
alex0612 7:d94d23c55015 175 }
alex0612 11:b45798cc3c10 176 // Turn
alex0612 7:d94d23c55015 177 } else {
alex0612 11:b45798cc3c10 178 printf("Turning\n");
lhartfield 22:e808fb71847d 179 move_detect(speed, fwd_bck, random_time);
alex0612 7:d94d23c55015 180 }
alex0612 7:d94d23c55015 181 }
alex0612 7:d94d23c55015 182 }
alex0612 7:d94d23c55015 183
abdsha01 6:af897173cb75 184 void stop() {
abdsha01 4:0507835a3dce 185 MotorLeft.speed(0.0);
abdsha01 4:0507835a3dce 186 MotorRight.speed(0.0);
abdsha01 4:0507835a3dce 187 }
abdsha01 4:0507835a3dce 188
abdsha01 6:af897173cb75 189 int detect_object(int range, float speed) {
alex0612 14:e639a42edf2e 190 // Start a timer - finds an object for 5 seconds
abdsha01 1:bd88d4062c97 191 // if it doesn't find anything returns 0
abdsha01 6:af897173cb75 192 Timer usensor_t, inner_t;
abdsha01 1:bd88d4062c97 193 usensor_t.start();
abdsha01 1:bd88d4062c97 194
abdsha01 1:bd88d4062c97 195 // Variable to store sensed value
abdsha01 6:af897173cb75 196 unsigned int sense, dist, reverse;
abdsha01 4:0507835a3dce 197 sense = 0;
abdsha01 4:0507835a3dce 198 dist = 0;
abdsha01 6:af897173cb75 199 reverse = 0;
abdsha01 1:bd88d4062c97 200
alex0612 11:b45798cc3c10 201 while (usensor_t.read_ms() < 5000) {
abdsha01 1:bd88d4062c97 202 // Start the ultrasonic sensor
abdsha01 1:bd88d4062c97 203 usensor.start();
abdsha01 6:af897173cb75 204 inner_t.start();
abdsha01 1:bd88d4062c97 205 dist = usensor.get_dist_cm();
abdsha01 1:bd88d4062c97 206
abdsha01 1:bd88d4062c97 207 // If an object is detected based on out set range return 1
abdsha01 1:bd88d4062c97 208 if (dist <= range && dist >= 1) {
abdsha01 1:bd88d4062c97 209 sense = 1;
alex0612 19:67ea4e8be9e1 210 stop();
alex0612 19:67ea4e8be9e1 211 flash_leds();
abdsha01 1:bd88d4062c97 212 break;
abdsha01 1:bd88d4062c97 213 } else {
abdsha01 1:bd88d4062c97 214 sense = 0;
abdsha01 9:7770a84228c0 215 turn(speed);
abdsha01 1:bd88d4062c97 216 }
abdsha01 6:af897173cb75 217
abdsha01 6:af897173cb75 218 if (inner_t.read_ms() >=100) {
abdsha01 6:af897173cb75 219 if (reverse == 2) {
abdsha01 6:af897173cb75 220 speed = 0.7;
abdsha01 6:af897173cb75 221 reverse = 0;
abdsha01 6:af897173cb75 222 } else {
abdsha01 6:af897173cb75 223 speed = 0.0;
abdsha01 6:af897173cb75 224 }
abdsha01 6:af897173cb75 225 reverse++;
abdsha01 6:af897173cb75 226 inner_t.reset();
abdsha01 6:af897173cb75 227 }
abdsha01 1:bd88d4062c97 228 }
abdsha01 6:af897173cb75 229
abdsha01 6:af897173cb75 230 usensor_t.stop();
abdsha01 6:af897173cb75 231 usensor_t.reset();
abdsha01 1:bd88d4062c97 232 return sense;
alex0612 11:b45798cc3c10 233 }
alex0612 11:b45798cc3c10 234
alex0612 11:b45798cc3c10 235 void move_detect(float speed, int fwd_bck, int time) {
alex0612 11:b45798cc3c10 236 Timer t;
alex0612 11:b45798cc3c10 237 t.start();
alex0612 11:b45798cc3c10 238 int detect = 0;
alex0612 11:b45798cc3c10 239
alex0612 19:67ea4e8be9e1 240 if(fwd_bck == 1) {
alex0612 19:67ea4e8be9e1 241 move_forward(speed);
alex0612 19:67ea4e8be9e1 242 wait_ms(500);
alex0612 19:67ea4e8be9e1 243 } else {
alex0612 19:67ea4e8be9e1 244 turn(speed + 0.2);
alex0612 19:67ea4e8be9e1 245 }
alex0612 19:67ea4e8be9e1 246
alex0612 11:b45798cc3c10 247 while (t.read_ms() < time) {
alex0612 11:b45798cc3c10 248 detect = detect_line();
alex0612 11:b45798cc3c10 249 // If line is detected from front then reverse
alex0612 11:b45798cc3c10 250 if(detect == 1) {
alex0612 19:67ea4e8be9e1 251 stop();
alex0612 19:67ea4e8be9e1 252 turn_leds_on();
abdsha01 20:37a89edd1cde 253 move_detect(-move_forwardspeed,1,1000);
alex0612 11:b45798cc3c10 254 stop();
alex0612 11:b45798cc3c10 255 break;
alex0612 17:47aa9ef2bec6 256 // If line is detected from back just keep on moving forward
alex0612 11:b45798cc3c10 257 } else if (detect == -1) {
alex0612 19:67ea4e8be9e1 258 stop();
alex0612 19:67ea4e8be9e1 259 turn_leds_on();
abdsha01 20:37a89edd1cde 260 move_detect(move_forwardspeed,1,1000);
alex0612 11:b45798cc3c10 261 stop();
alex0612 11:b45798cc3c10 262 break;
alex0612 11:b45798cc3c10 263 }
alex0612 11:b45798cc3c10 264 }
alex0612 19:67ea4e8be9e1 265 stop();
alex0612 11:b45798cc3c10 266 t.stop();
abdsha01 0:15664f71b21f 267 }