Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@7:d94d23c55015, 2015-05-23 (annotated)

Committer:
alex0612
Date:
Sat May 23 17:03:48 2015 +0000
Revision:
7:d94d23c55015
Parent:
6:af897173cb75
Child:
10:cec68ef272cd
moverandom function added

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 4:0507835a3dce 85 MotorLeft.speed((speed));
abdsha01 4:0507835a3dce 86 MotorRight.speed(-(speed));
abdsha01 0:15664f71b21f 87 wait_ms(1000);
abdsha01 0:15664f71b21f 88 return;
abdsha01 0:15664f71b21f 89 }
abdsha01 0:15664f71b21f 90
abdsha01 6:af897173cb75 91 void reverseandturn(float speed) {
abdsha01 0:15664f71b21f 92 printf("Reverse and turn\n");
abdsha01 4:0507835a3dce 93 MotorLeft.speed((speed-0.3));
abdsha01 4:0507835a3dce 94 MotorRight.speed(-(speed-0.1));
abdsha01 0:15664f71b21f 95 wait_ms(2000);
abdsha01 0:15664f71b21f 96 return;
abdsha01 0:15664f71b21f 97 }
abdsha01 0:15664f71b21f 98
abdsha01 6:af897173cb75 99 void charge(float speed) {
abdsha01 4:0507835a3dce 100 MotorLeft.speed(speed);
abdsha01 4:0507835a3dce 101 MotorRight.speed(speed);
abdsha01 0:15664f71b21f 102 return;
abdsha01 1:bd88d4062c97 103 }
abdsha01 1:bd88d4062c97 104
alex0612 7:d94d23c55015 105 void moverandom()
alex0612 7:d94d23c55015 106 {
alex0612 7:d94d23c55015 107 int counter;
alex0612 7:d94d23c55015 108 int turn;
alex0612 7:d94d23c55015 109 int direction;
alex0612 7:d94d23c55015 110 float random_time;
alex0612 7:d94d23c55015 111 float random_speed;
alex0612 7:d94d23c55015 112
alex0612 7:d94d23c55015 113 printf("Moving randomly\n");
alex0612 7:d94d23c55015 114 counter = rand() % 10;
alex0612 7:d94d23c55015 115
alex0612 7:d94d23c55015 116 for (int i = 0; i < counter; i++)
alex0612 7:d94d23c55015 117 {
alex0612 7:d94d23c55015 118 random_time = rand() % 1000 + 1000;
alex0612 7:d94d23c55015 119
alex0612 7:d94d23c55015 120 turn = rand()/(RAND_MAX);
alex0612 7:d94d23c55015 121 random_speed = (float)rand()/(float)(RAND_MAX);
alex0612 7:d94d23c55015 122 if (random_speed < 0.5) {
alex0612 7:d94d23c55015 123 random_speed = 0.5;
alex0612 7:d94d23c55015 124 } else if (random_speed > 0.8) {
alex0612 7:d94d23c55015 125 random_speed = 0.8;
alex0612 7:d94d23c55015 126 }
alex0612 7:d94d23c55015 127
alex0612 7:d94d23c55015 128 if (turn == 0) {
alex0612 7:d94d23c55015 129 direction = rand()/(RAND_MAX);
alex0612 7:d94d23c55015 130 if (direction == 1) {
alex0612 7:d94d23c55015 131 MotorLeft.speed(random_speed);
alex0612 7:d94d23c55015 132 MotorRight.speed(random_speed);
alex0612 7:d94d23c55015 133 wait_ms(random_time);
alex0612 7:d94d23c55015 134 } else {
alex0612 7:d94d23c55015 135 MotorLeft.speed(-random_speed);
alex0612 7:d94d23c55015 136 MotorRight.speed(-random_speed);
alex0612 7:d94d23c55015 137 wait_ms(random_time);
alex0612 7:d94d23c55015 138 }
alex0612 7:d94d23c55015 139 } else {
alex0612 7:d94d23c55015 140 MotorLeft.speed(-random_speed);
alex0612 7:d94d23c55015 141 MotorRight.speed(random_speed);
alex0612 7:d94d23c55015 142 wait_ms(random_time);
alex0612 7:d94d23c55015 143 }
alex0612 7:d94d23c55015 144 return;
alex0612 7:d94d23c55015 145 }
alex0612 7:d94d23c55015 146 }
alex0612 7:d94d23c55015 147
abdsha01 6:af897173cb75 148 void stop() {
abdsha01 4:0507835a3dce 149 MotorLeft.speed(0.0);
abdsha01 4:0507835a3dce 150 MotorRight.speed(0.0);
abdsha01 4:0507835a3dce 151 return;
abdsha01 4:0507835a3dce 152 }
abdsha01 4:0507835a3dce 153
abdsha01 6:af897173cb75 154 int detect_object(int range, float speed) {
abdsha01 1:bd88d4062c97 155 // Start a timer - finds an object for 10 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
abdsha01 6:af897173cb75 166 while (usensor_t.read_ms() < 15000) {
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 4:0507835a3dce 178 MotorLeft.speed(speed);
abdsha01 4:0507835a3dce 179 MotorRight.speed(-(speed));
abdsha01 1:bd88d4062c97 180 }
abdsha01 6:af897173cb75 181
abdsha01 6:af897173cb75 182 if (inner_t.read_ms() >=100) {
abdsha01 6:af897173cb75 183 if (reverse == 2) {
abdsha01 6:af897173cb75 184 speed = 0.7;
abdsha01 6:af897173cb75 185 reverse = 0;
abdsha01 6:af897173cb75 186 } else {
abdsha01 6:af897173cb75 187 speed = 0.0;
abdsha01 6:af897173cb75 188 }
abdsha01 6:af897173cb75 189 reverse++;
abdsha01 6:af897173cb75 190 inner_t.reset();
abdsha01 6:af897173cb75 191 }
abdsha01 1:bd88d4062c97 192 }
abdsha01 6:af897173cb75 193
abdsha01 6:af897173cb75 194 usensor_t.stop();
abdsha01 6:af897173cb75 195 usensor_t.reset();
abdsha01 1:bd88d4062c97 196 return sense;
abdsha01 0:15664f71b21f 197 }