Robot code for searching an object and charging at it.
Dependencies: HCSR04 Motor mbed
functions.cpp@9:7770a84228c0, 2015-05-23 (annotated)
- Committer:
- abdsha01
- Date:
- Sat May 23 17:09:28 2015 +0000
- Revision:
- 9:7770a84228c0
- Parent:
- 6:af897173cb75
- Child:
- 10:cec68ef272cd
Improved main loop
Who changed what in which revision?
User | Revision | Line number | New 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 | |
abdsha01 | 6:af897173cb75 | 106 | void stop() { |
abdsha01 | 4:0507835a3dce | 107 | MotorLeft.speed(0.0); |
abdsha01 | 4:0507835a3dce | 108 | MotorRight.speed(0.0); |
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 | 9:7770a84228c0 | 135 | turn(speed); |
abdsha01 | 1:bd88d4062c97 | 136 | } |
abdsha01 | 6:af897173cb75 | 137 | |
abdsha01 | 6:af897173cb75 | 138 | if (inner_t.read_ms() >=100) { |
abdsha01 | 6:af897173cb75 | 139 | if (reverse == 2) { |
abdsha01 | 6:af897173cb75 | 140 | speed = 0.7; |
abdsha01 | 6:af897173cb75 | 141 | reverse = 0; |
abdsha01 | 6:af897173cb75 | 142 | } else { |
abdsha01 | 6:af897173cb75 | 143 | speed = 0.0; |
abdsha01 | 6:af897173cb75 | 144 | } |
abdsha01 | 6:af897173cb75 | 145 | reverse++; |
abdsha01 | 6:af897173cb75 | 146 | inner_t.reset(); |
abdsha01 | 6:af897173cb75 | 147 | } |
abdsha01 | 1:bd88d4062c97 | 148 | } |
abdsha01 | 6:af897173cb75 | 149 | |
abdsha01 | 6:af897173cb75 | 150 | usensor_t.stop(); |
abdsha01 | 6:af897173cb75 | 151 | usensor_t.reset(); |
abdsha01 | 1:bd88d4062c97 | 152 | return sense; |
abdsha01 | 0:15664f71b21f | 153 | } |