Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@6:af897173cb75, 2015-05-23 (annotated)

Committer:
abdsha01
Date:
Sat May 23 16:09:43 2015 +0000
Revision:
6:af897173cb75
Parent:
4:0507835a3dce
Child:
7:d94d23c55015
Child:
9:7770a84228c0
Added more code

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
abdsha01 6:af897173cb75 105 void stop() {
abdsha01 4:0507835a3dce 106 MotorLeft.speed(0.0);
abdsha01 4:0507835a3dce 107 MotorRight.speed(0.0);
abdsha01 4:0507835a3dce 108 return;
abdsha01 4:0507835a3dce 109 }
abdsha01 4:0507835a3dce 110
abdsha01 6:af897173cb75 111 int detect_object(int range, float speed) {
abdsha01 1:bd88d4062c97 112 // Start a timer - finds an object for 10 seconds
abdsha01 1:bd88d4062c97 113 // if it doesn't find anything returns 0
abdsha01 6:af897173cb75 114 Timer usensor_t, inner_t;
abdsha01 1:bd88d4062c97 115 usensor_t.start();
abdsha01 1:bd88d4062c97 116
abdsha01 1:bd88d4062c97 117 // Variable to store sensed value
abdsha01 6:af897173cb75 118 unsigned int sense, dist, reverse;
abdsha01 4:0507835a3dce 119 sense = 0;
abdsha01 4:0507835a3dce 120 dist = 0;
abdsha01 6:af897173cb75 121 reverse = 0;
abdsha01 1:bd88d4062c97 122
abdsha01 6:af897173cb75 123 while (usensor_t.read_ms() < 15000) {
abdsha01 1:bd88d4062c97 124 // Start the ultrasonic sensor
abdsha01 1:bd88d4062c97 125 usensor.start();
abdsha01 6:af897173cb75 126 inner_t.start();
abdsha01 1:bd88d4062c97 127 dist = usensor.get_dist_cm();
abdsha01 1:bd88d4062c97 128
abdsha01 1:bd88d4062c97 129 // If an object is detected based on out set range return 1
abdsha01 1:bd88d4062c97 130 if (dist <= range && dist >= 1) {
abdsha01 1:bd88d4062c97 131 sense = 1;
abdsha01 1:bd88d4062c97 132 break;
abdsha01 1:bd88d4062c97 133 } else {
abdsha01 1:bd88d4062c97 134 sense = 0;
abdsha01 4:0507835a3dce 135 MotorLeft.speed(speed);
abdsha01 4:0507835a3dce 136 MotorRight.speed(-(speed));
abdsha01 1:bd88d4062c97 137 }
abdsha01 6:af897173cb75 138
abdsha01 6:af897173cb75 139 if (inner_t.read_ms() >=100) {
abdsha01 6:af897173cb75 140 if (reverse == 2) {
abdsha01 6:af897173cb75 141 speed = 0.7;
abdsha01 6:af897173cb75 142 reverse = 0;
abdsha01 6:af897173cb75 143 } else {
abdsha01 6:af897173cb75 144 speed = 0.0;
abdsha01 6:af897173cb75 145 }
abdsha01 6:af897173cb75 146 reverse++;
abdsha01 6:af897173cb75 147 inner_t.reset();
abdsha01 6:af897173cb75 148 }
abdsha01 1:bd88d4062c97 149 }
abdsha01 6:af897173cb75 150
abdsha01 6:af897173cb75 151 usensor_t.stop();
abdsha01 6:af897173cb75 152 usensor_t.reset();
abdsha01 1:bd88d4062c97 153 return sense;
abdsha01 0:15664f71b21f 154 }