Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@17:47aa9ef2bec6, 2015-05-30 (annotated)

Committer:
alex0612
Date:
Sat May 30 16:14:43 2015 +0000
Revision:
17:47aa9ef2bec6
Parent:
14:e639a42edf2e
Child:
19:67ea4e8be9e1
Fixed move_detect

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
abdsha01 4:0507835a3dce 10 // Two sensors are used to detect a line, line_sens1 and line_sens2
abdsha01 4:0507835a3dce 11 // if there is a difference between the readings of these sensors
abdsha01 4:0507835a3dce 12 // the robot has detected a line.
abdsha01 4:0507835a3dce 13 // Setting pins for line sensor
abdsha01 4:0507835a3dce 14 DigitalInOut line1(p20);
abdsha01 4:0507835a3dce 15 DigitalInOut line2(p19);
abdsha01 4:0507835a3dce 16
abdsha01 4:0507835a3dce 17 // Setting pins for motor, as follows:
abdsha01 4:0507835a3dce 18 // Example: Motor____(PWM, Forward, Reverse)
abdsha01 6:af897173cb75 19 Motor MotorLeft(p23, p27, p28);
abdsha01 6:af897173cb75 20 Motor MotorRight(p22, p30, p29);
abdsha01 4:0507835a3dce 21
abdsha01 4:0507835a3dce 22 // Setting pins for ultrasonic sensor, as follows:
abdsha01 4:0507835a3dce 23 // Example: usensor(Trigger, Echo)
abdsha01 4:0507835a3dce 24 HCSR04 usensor(p25,p26);
abdsha01 0:15664f71b21f 25
abdsha01 0:15664f71b21f 26 // Returns value from the QRE1113
abdsha01 0:15664f71b21f 27 // lower numbers mean more refleacive
abdsha01 0:15664f71b21f 28 // more than 3000 means nothing was reflected.
abdsha01 6:af897173cb75 29 int read_line1() {
abdsha01 0:15664f71b21f 30 // Time to record how long input is active
abdsha01 1:bd88d4062c97 31 Timer temp_t;
abdsha01 0:15664f71b21f 32
abdsha01 0:15664f71b21f 33 // Activate the line sensor and then read
abdsha01 0:15664f71b21f 34 line1.output();
abdsha01 0:15664f71b21f 35 line1 = 1;
abdsha01 0:15664f71b21f 36 wait_us(15);
abdsha01 0:15664f71b21f 37 line1.input();
abdsha01 0:15664f71b21f 38
abdsha01 0:15664f71b21f 39 // Start timer
abdsha01 1:bd88d4062c97 40 temp_t.start();
abdsha01 0:15664f71b21f 41
abdsha01 0:15664f71b21f 42 // Time how long the input is HIGH, but quit
abdsha01 0:15664f71b21f 43 // after 1ms as nothing happens after that
abdsha01 1:bd88d4062c97 44 while (line1 == 1 && temp_t.read_us() < 1000);
abdsha01 1:bd88d4062c97 45 return temp_t.read_us();
abdsha01 0:15664f71b21f 46 }
abdsha01 0:15664f71b21f 47
abdsha01 6:af897173cb75 48 int read_line2() {
abdsha01 0:15664f71b21f 49 // Time to record how long input is active
abdsha01 1:bd88d4062c97 50 Timer temp_t;
abdsha01 0:15664f71b21f 51
abdsha01 0:15664f71b21f 52 // Activate the line sensor and then read
abdsha01 0:15664f71b21f 53 line2.output();
abdsha01 0:15664f71b21f 54 line2 = 1;
abdsha01 0:15664f71b21f 55 wait_us(15);
abdsha01 0:15664f71b21f 56 line2.input();
abdsha01 0:15664f71b21f 57
abdsha01 0:15664f71b21f 58 // Start timer
abdsha01 4:0507835a3dce 59 temp_t.start();
abdsha01 0:15664f71b21f 60
abdsha01 0:15664f71b21f 61 // Time how long the input is HIGH, but quit
abdsha01 0:15664f71b21f 62 // after 1ms as nothing happens after that
abdsha01 1:bd88d4062c97 63 while (line2 == 1 && temp_t.read_us() < 1000);
abdsha01 4:0507835a3dce 64 return temp_t.read_us();
abdsha01 0:15664f71b21f 65 }
abdsha01 0:15664f71b21f 66
abdsha01 6:af897173cb75 67 int detect_line() {
abdsha01 0:15664f71b21f 68 int line1val = read_line1();
abdsha01 0:15664f71b21f 69 int line2val = read_line2();
abdsha01 0:15664f71b21f 70
abdsha01 0:15664f71b21f 71 if ((line1val-line2val) > 50) {
abdsha01 0:15664f71b21f 72 printf("Line detected from front");
abdsha01 0:15664f71b21f 73 return 1;
abdsha01 0:15664f71b21f 74 } else if ((line1val-line2val) < -50) {
abdsha01 0:15664f71b21f 75 printf("Line detected from back");
abdsha01 0:15664f71b21f 76 return -1;
abdsha01 0:15664f71b21f 77 } else {
abdsha01 0:15664f71b21f 78 printf("Line not detected");
abdsha01 0:15664f71b21f 79 return 0;
abdsha01 0:15664f71b21f 80 }
abdsha01 0:15664f71b21f 81 }
abdsha01 0:15664f71b21f 82
abdsha01 6:af897173cb75 83 void reverse(float speed) {
abdsha01 0:15664f71b21f 84 printf("Reverse\n");
abdsha01 9:7770a84228c0 85 MotorLeft.speed(-(speed));
abdsha01 4:0507835a3dce 86 MotorRight.speed(-(speed));
abdsha01 9:7770a84228c0 87 }
abdsha01 9:7770a84228c0 88
abdsha01 9:7770a84228c0 89 void turn(float speed) {
abdsha01 9:7770a84228c0 90 printf("Turning\n");
abdsha01 9:7770a84228c0 91 MotorLeft.speed(speed);
abdsha01 9:7770a84228c0 92 MotorRight.speed(-(speed));
abdsha01 0:15664f71b21f 93 }
abdsha01 0:15664f71b21f 94
abdsha01 6:af897173cb75 95 void reverseandturn(float speed) {
abdsha01 0:15664f71b21f 96 printf("Reverse and turn\n");
abdsha01 4:0507835a3dce 97 MotorLeft.speed((speed-0.3));
abdsha01 4:0507835a3dce 98 MotorRight.speed(-(speed-0.1));
abdsha01 0:15664f71b21f 99 }
abdsha01 0:15664f71b21f 100
abdsha01 6:af897173cb75 101 void charge(float speed) {
abdsha01 4:0507835a3dce 102 MotorLeft.speed(speed);
abdsha01 4:0507835a3dce 103 MotorRight.speed(speed);
abdsha01 1:bd88d4062c97 104 }
abdsha01 1:bd88d4062c97 105
alex0612 7:d94d23c55015 106 void moverandom()
alex0612 7:d94d23c55015 107 {
alex0612 7:d94d23c55015 108 int counter;
alex0612 11:b45798cc3c10 109 int fwd_bck;
alex0612 11:b45798cc3c10 110 int fwd;
alex0612 7:d94d23c55015 111 float random_time;
alex0612 7:d94d23c55015 112 float random_speed;
alex0612 7:d94d23c55015 113
alex0612 7:d94d23c55015 114 printf("Moving randomly\n");
alex0612 7:d94d23c55015 115 counter = rand() % 10;
alex0612 7:d94d23c55015 116
alex0612 7:d94d23c55015 117 for (int i = 0; i < counter; i++)
alex0612 7:d94d23c55015 118 {
alex0612 7:d94d23c55015 119 random_time = rand() % 1000 + 1000;
alex0612 7:d94d23c55015 120
alex0612 11:b45798cc3c10 121 fwd_bck = rand()%2;
alex0612 7:d94d23c55015 122 random_speed = (float)rand()/(float)(RAND_MAX);
alex0612 7:d94d23c55015 123
alex0612 11:b45798cc3c10 124 if (random_speed < 0.6) {
alex0612 11:b45798cc3c10 125 random_speed = 0.5;
alex0612 11:b45798cc3c10 126 } else if (random_speed >= 0.9) {
alex0612 11:b45798cc3c10 127 random_speed = 0.9;
alex0612 11:b45798cc3c10 128 }
alex0612 11:b45798cc3c10 129 // If fwd_back == 1 move forward or backwards
alex0612 11:b45798cc3c10 130 if (fwd_bck == 1) {
alex0612 11:b45798cc3c10 131 fwd = rand()%2;
alex0612 11:b45798cc3c10 132 // If fwd == 1 move forward
alex0612 11:b45798cc3c10 133 if (fwd == 1) {
alex0612 14:e639a42edf2e 134 printf("Moving forward\n");
alex0612 11:b45798cc3c10 135 move_detect(random_speed, fwd_bck, random_time);
alex0612 11:b45798cc3c10 136 // If fwd == 0 move bacward
alex0612 7:d94d23c55015 137 } else {
alex0612 14:e639a42edf2e 138 printf("Moving bacwards\n");
alex0612 11:b45798cc3c10 139 move_detect(-random_speed, fwd_bck, random_time);
alex0612 7:d94d23c55015 140 }
alex0612 11:b45798cc3c10 141 // Turn
alex0612 7:d94d23c55015 142 } else {
alex0612 11:b45798cc3c10 143 printf("Turning\n");
alex0612 11:b45798cc3c10 144 move_detect(random_speed, fwd_bck, random_time);
alex0612 7:d94d23c55015 145 }
alex0612 7:d94d23c55015 146 }
alex0612 7:d94d23c55015 147 }
alex0612 7:d94d23c55015 148
abdsha01 6:af897173cb75 149 void stop() {
abdsha01 4:0507835a3dce 150 MotorLeft.speed(0.0);
abdsha01 4:0507835a3dce 151 MotorRight.speed(0.0);
abdsha01 4:0507835a3dce 152 }
abdsha01 4:0507835a3dce 153
abdsha01 6:af897173cb75 154 int detect_object(int range, float speed) {
alex0612 14:e639a42edf2e 155 // Start a timer - finds an object for 5 seconds
abdsha01 1:bd88d4062c97 156 // if it doesn't find anything returns 0
abdsha01 6:af897173cb75 157 Timer usensor_t, inner_t;
abdsha01 1:bd88d4062c97 158 usensor_t.start();
abdsha01 1:bd88d4062c97 159
abdsha01 1:bd88d4062c97 160 // Variable to store sensed value
abdsha01 6:af897173cb75 161 unsigned int sense, dist, reverse;
abdsha01 4:0507835a3dce 162 sense = 0;
abdsha01 4:0507835a3dce 163 dist = 0;
abdsha01 6:af897173cb75 164 reverse = 0;
abdsha01 1:bd88d4062c97 165
alex0612 11:b45798cc3c10 166 while (usensor_t.read_ms() < 5000) {
abdsha01 1:bd88d4062c97 167 // Start the ultrasonic sensor
abdsha01 1:bd88d4062c97 168 usensor.start();
abdsha01 6:af897173cb75 169 inner_t.start();
abdsha01 1:bd88d4062c97 170 dist = usensor.get_dist_cm();
abdsha01 1:bd88d4062c97 171
abdsha01 1:bd88d4062c97 172 // If an object is detected based on out set range return 1
abdsha01 1:bd88d4062c97 173 if (dist <= range && dist >= 1) {
abdsha01 1:bd88d4062c97 174 sense = 1;
abdsha01 1:bd88d4062c97 175 break;
abdsha01 1:bd88d4062c97 176 } else {
abdsha01 1:bd88d4062c97 177 sense = 0;
abdsha01 9:7770a84228c0 178 turn(speed);
abdsha01 1:bd88d4062c97 179 }
abdsha01 6:af897173cb75 180
abdsha01 6:af897173cb75 181 if (inner_t.read_ms() >=100) {
abdsha01 6:af897173cb75 182 if (reverse == 2) {
abdsha01 6:af897173cb75 183 speed = 0.7;
abdsha01 6:af897173cb75 184 reverse = 0;
abdsha01 6:af897173cb75 185 } else {
abdsha01 6:af897173cb75 186 speed = 0.0;
abdsha01 6:af897173cb75 187 }
abdsha01 6:af897173cb75 188 reverse++;
abdsha01 6:af897173cb75 189 inner_t.reset();
abdsha01 6:af897173cb75 190 }
abdsha01 1:bd88d4062c97 191 }
abdsha01 6:af897173cb75 192
abdsha01 6:af897173cb75 193 usensor_t.stop();
abdsha01 6:af897173cb75 194 usensor_t.reset();
abdsha01 1:bd88d4062c97 195 return sense;
alex0612 11:b45798cc3c10 196 }
alex0612 11:b45798cc3c10 197
alex0612 11:b45798cc3c10 198 void move_detect(float speed, int fwd_bck, int time) {
alex0612 11:b45798cc3c10 199 Timer t;
alex0612 11:b45798cc3c10 200 t.start();
alex0612 11:b45798cc3c10 201 int detect = 0;
alex0612 11:b45798cc3c10 202
alex0612 11:b45798cc3c10 203 while (t.read_ms() < time) {
alex0612 11:b45798cc3c10 204 if(fwd_bck == 1) {
alex0612 11:b45798cc3c10 205 charge(speed);
alex0612 11:b45798cc3c10 206 } else {
alex0612 11:b45798cc3c10 207 turn(speed);
alex0612 11:b45798cc3c10 208 }
alex0612 11:b45798cc3c10 209 detect = detect_line();
alex0612 11:b45798cc3c10 210 // If line is detected from front then reverse
alex0612 11:b45798cc3c10 211 if(detect == 1) {
alex0612 17:47aa9ef2bec6 212 move_detect(-speed,1,1500);
alex0612 11:b45798cc3c10 213 stop();
alex0612 11:b45798cc3c10 214 break;
alex0612 17:47aa9ef2bec6 215 // If line is detected from back just keep on moving forward
alex0612 11:b45798cc3c10 216 } else if (detect == -1) {
alex0612 17:47aa9ef2bec6 217 move_detect(speed,1,1500);
alex0612 11:b45798cc3c10 218 stop();
alex0612 11:b45798cc3c10 219 break;
alex0612 11:b45798cc3c10 220 }
alex0612 11:b45798cc3c10 221 }
alex0612 11:b45798cc3c10 222 t.stop();
abdsha01 0:15664f71b21f 223 }