Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@30:8a5570b2de68, 2015-06-07 (annotated)

Committer:
alex0612
Date:
Sun Jun 07 18:35:04 2015 +0000
Revision:
30:8a5570b2de68
Parent:
27:96a0ff2f474c
Child:
33:4d0fe8fbed68
Minor changes

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)
alex0612 30:8a5570b2de68 24 Motor MotorRight(p23, p27, p28);
alex0612 30:8a5570b2de68 25 Motor MotorLeft(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
alex0612 30:8a5570b2de68 143 void reverse(float speed, float adj) {
abdsha01 26:5ee2a32949e6 144 //printf("Reverse\n");
abdsha01 9:7770a84228c0 145 MotorLeft.speed(-(speed));
alex0612 30:8a5570b2de68 146 MotorRight.speed(-(speed)-adj);
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
alex0612 30:8a5570b2de68 161 void move_forward(float speed, float adj) {
abdsha01 4:0507835a3dce 162 MotorLeft.speed(speed);
alex0612 30:8a5570b2de68 163 MotorRight.speed(speed+adj);
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
alex0612 30:8a5570b2de68 219 while (usensor_t.read_ms() < 10000) {
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
lhartfield 27:96a0ff2f474c 255 void move_detect(float speed, int fwd_bck, int time, int wait) {
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);
lhartfield 27:96a0ff2f474c 262 wait_ms(wait);
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();
alex0612 30:8a5570b2de68 273 move_detect(-forwardspeed,1,1000,300);
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();
alex0612 30:8a5570b2de68 280 move_detect(forwardspeed,1,1000,300);
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 }