Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

Committer:
abdsha01
Date:
Sat May 23 17:48:01 2015 +0000
Revision:
10:cec68ef272cd
Parent:
7:d94d23c55015
Parent:
9:7770a84228c0
Child:
11:b45798cc3c10
Updated stuff

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;
abdsha01 10:cec68ef272cd 109 int dir;
alex0612 7:d94d23c55015 110 int direction;
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
abdsha01 10:cec68ef272cd 121 dir = rand()/(RAND_MAX);
alex0612 7:d94d23c55015 122 random_speed = (float)rand()/(float)(RAND_MAX);
alex0612 7:d94d23c55015 123 if (random_speed < 0.5) {
alex0612 7:d94d23c55015 124 random_speed = 0.5;
alex0612 7:d94d23c55015 125 } else if (random_speed > 0.8) {
alex0612 7:d94d23c55015 126 random_speed = 0.8;
alex0612 7:d94d23c55015 127 }
alex0612 7:d94d23c55015 128
abdsha01 10:cec68ef272cd 129 if (dir == 0) {
alex0612 7:d94d23c55015 130 direction = rand()/(RAND_MAX);
alex0612 7:d94d23c55015 131 if (direction == 1) {
abdsha01 10:cec68ef272cd 132 charge(random_speed);
alex0612 7:d94d23c55015 133 wait_ms(random_time);
alex0612 7:d94d23c55015 134 } else {
abdsha01 10:cec68ef272cd 135 reverse(random_speed);
alex0612 7:d94d23c55015 136 wait_ms(random_time);
alex0612 7:d94d23c55015 137 }
alex0612 7:d94d23c55015 138 } else {
abdsha01 10:cec68ef272cd 139 turn(random_speed);
alex0612 7:d94d23c55015 140 wait_ms(random_time);
alex0612 7:d94d23c55015 141 }
alex0612 7:d94d23c55015 142 return;
alex0612 7:d94d23c55015 143 }
alex0612 7:d94d23c55015 144 }
alex0612 7:d94d23c55015 145
abdsha01 6:af897173cb75 146 void stop() {
abdsha01 4:0507835a3dce 147 MotorLeft.speed(0.0);
abdsha01 4:0507835a3dce 148 MotorRight.speed(0.0);
abdsha01 4:0507835a3dce 149 }
abdsha01 4:0507835a3dce 150
abdsha01 6:af897173cb75 151 int detect_object(int range, float speed) {
abdsha01 1:bd88d4062c97 152 // Start a timer - finds an object for 10 seconds
abdsha01 1:bd88d4062c97 153 // if it doesn't find anything returns 0
abdsha01 6:af897173cb75 154 Timer usensor_t, inner_t;
abdsha01 1:bd88d4062c97 155 usensor_t.start();
abdsha01 1:bd88d4062c97 156
abdsha01 1:bd88d4062c97 157 // Variable to store sensed value
abdsha01 6:af897173cb75 158 unsigned int sense, dist, reverse;
abdsha01 4:0507835a3dce 159 sense = 0;
abdsha01 4:0507835a3dce 160 dist = 0;
abdsha01 6:af897173cb75 161 reverse = 0;
abdsha01 1:bd88d4062c97 162
abdsha01 6:af897173cb75 163 while (usensor_t.read_ms() < 15000) {
abdsha01 1:bd88d4062c97 164 // Start the ultrasonic sensor
abdsha01 1:bd88d4062c97 165 usensor.start();
abdsha01 6:af897173cb75 166 inner_t.start();
abdsha01 1:bd88d4062c97 167 dist = usensor.get_dist_cm();
abdsha01 1:bd88d4062c97 168
abdsha01 1:bd88d4062c97 169 // If an object is detected based on out set range return 1
abdsha01 1:bd88d4062c97 170 if (dist <= range && dist >= 1) {
abdsha01 1:bd88d4062c97 171 sense = 1;
abdsha01 1:bd88d4062c97 172 break;
abdsha01 1:bd88d4062c97 173 } else {
abdsha01 1:bd88d4062c97 174 sense = 0;
abdsha01 9:7770a84228c0 175 turn(speed);
abdsha01 1:bd88d4062c97 176 }
abdsha01 6:af897173cb75 177
abdsha01 6:af897173cb75 178 if (inner_t.read_ms() >=100) {
abdsha01 6:af897173cb75 179 if (reverse == 2) {
abdsha01 6:af897173cb75 180 speed = 0.7;
abdsha01 6:af897173cb75 181 reverse = 0;
abdsha01 6:af897173cb75 182 } else {
abdsha01 6:af897173cb75 183 speed = 0.0;
abdsha01 6:af897173cb75 184 }
abdsha01 6:af897173cb75 185 reverse++;
abdsha01 6:af897173cb75 186 inner_t.reset();
abdsha01 6:af897173cb75 187 }
abdsha01 1:bd88d4062c97 188 }
abdsha01 6:af897173cb75 189
abdsha01 6:af897173cb75 190 usensor_t.stop();
abdsha01 6:af897173cb75 191 usensor_t.reset();
abdsha01 1:bd88d4062c97 192 return sense;
abdsha01 0:15664f71b21f 193 }