| 1 | /* |
| 2 | This program is free software: you can redistribute it and/or modify |
| 3 | it under the terms of the GNU Affero General Public License as |
| 4 | published by the Free Software Foundation, either version 3 of the |
| 5 | License, or (at your option) any later version. |
| 6 | |
| 7 | This program is distributed in the hope that it will be useful, |
| 8 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 10 | GNU Affero General Public License for more details. |
| 11 | |
| 12 | You should have received a copy of the GNU Affero General Public License |
| 13 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 14 | */ |
| 15 | // This program implements table top challenge level 1 -- |
| 16 | // The robot stays on the table without falling off. |
| 17 | |
| 18 | // Set *debug* to 1 to enable debugging statements: |
| 19 | int debug = 1; |
| 20 | |
| 21 | // Set *stop_at_edge* to 1 to have robot stop at the edge; |
| 22 | // otherwise, it will back away from edge: |
| 23 | int stop_at_edge = 0; |
| 24 | |
| 25 | // Set *table_top_threshold* to a value that captures the |
| 26 | // tabletop distance for *your* robot: |
| 27 | int table_top_threshold = 600; |
| 28 | |
| 29 | // Set *sonar_timeout* to a value a 200-300 uSec longer |
| 30 | // that your table top threshold: |
| 31 | |
| 32 | int sonar_timeout = 1600; |
| 33 | |
| 34 | // Set *sonar_set_point* to a value that specifies the approximate |
| 35 | // follow distance: |
| 36 | |
| 37 | int sonar_follow_distance = 800; |
| 38 | |
| 39 | // Pin definitions: |
| 40 | int led = 13; |
| 41 | int left_motor_enable = 3; |
| 42 | int left_motor_a = 4; |
| 43 | int left_motor_b = 5; |
| 44 | int right_motor_enable = 10; |
| 45 | int right_motor_a = 9; |
| 46 | int right_motor_b = 8; |
| 47 | int right_sonar_trigger = 7; |
| 48 | int right_sonar_echo = 6; |
| 49 | int left_sonar_trigger = 12; |
| 50 | int left_sonar_echo = 11; |
| 51 | |
| 52 | // the setup routine runs once when you press reset: |
| 53 | void setup() { |
| 54 | // If debugging is enabled, |
| 55 | if (debug) { |
| 56 | // 115200 is the fastest "standard" baud rate for debugging. |
| 57 | // Be sure to set the baud Tools=>Serial Monitor to 115200: |
| 58 | Serial.begin(115200); |
| 59 | } |
| 60 | |
| 61 | // Set LED output pin: |
| 62 | pinMode(led, OUTPUT); |
| 63 | |
| 64 | // Set H-bridge control pins: |
| 65 | pinMode(left_motor_enable, OUTPUT); |
| 66 | pinMode(left_motor_a, OUTPUT); |
| 67 | pinMode(left_motor_b, OUTPUT); |
| 68 | pinMode(right_motor_enable, OUTPUT); |
| 69 | pinMode(right_motor_a, OUTPUT); |
| 70 | pinMode(right_motor_b, OUTPUT); |
| 71 | |
| 72 | // Set control pins for both left and right sonars: |
| 73 | pinMode(left_sonar_trigger, OUTPUT); |
| 74 | pinMode(left_sonar_echo, INPUT); |
| 75 | pinMode(right_sonar_trigger, OUTPUT); |
| 76 | pinMode(right_sonar_echo, INPUT); |
| 77 | |
| 78 | // Set all the pin values to predefined values: |
| 79 | digitalWrite(led, HIGH); |
| 80 | digitalWrite(left_motor_enable, LOW); |
| 81 | digitalWrite(left_motor_a, LOW); |
| 82 | digitalWrite(left_motor_b, LOW); |
| 83 | digitalWrite(right_motor_enable, LOW); |
| 84 | digitalWrite(right_motor_a, LOW); |
| 85 | digitalWrite(right_motor_b, LOW); |
| 86 | digitalWrite(left_sonar_trigger, LOW); |
| 87 | digitalWrite(right_sonar_trigger, LOW); |
| 88 | digitalWrite(left_sonar_trigger, LOW); |
| 89 | digitalWrite(right_sonar_trigger, LOW); |
| 90 | } |
| 91 | |
| 92 | void motorsRun(int left, int right, int ms_delay) { |
| 93 | // Set left motor direction: |
| 94 | if (left > 0) { |
| 95 | // Set left motor to go forward: |
| 96 | digitalWrite(left_motor_a, HIGH); |
| 97 | digitalWrite(left_motor_b, LOW); |
| 98 | } else { |
| 99 | // Set left motor to go backward: |
| 100 | digitalWrite(left_motor_a, LOW); |
| 101 | digitalWrite(left_motor_b, HIGH); |
| 102 | left = -left; // Make left a positive value: |
| 103 | } |
| 104 | // Make sure we cap the speed at 255: |
| 105 | if (left > 255) { |
| 106 | left = 255; |
| 107 | } |
| 108 | analogWrite(left_motor_enable, left); // Start motor in right direction |
| 109 | |
| 110 | // Set right motor direction: |
| 111 | if (right > 0) { |
| 112 | // Set right motor to go forward: |
| 113 | digitalWrite(right_motor_a, HIGH); |
| 114 | digitalWrite(right_motor_b, LOW); |
| 115 | } else { |
| 116 | // Set right motor to go backward: |
| 117 | digitalWrite(right_motor_a, LOW); |
| 118 | digitalWrite(right_motor_b, HIGH); |
| 119 | right = -right; // Make right a positive value: |
| 120 | } |
| 121 | // Make sure we cap the speed at 255: |
| 122 | if (right > 255) { |
| 123 | right = 255; |
| 124 | } |
| 125 | analogWrite(right_motor_enable, right); // Start motor in right direction |
| 126 | |
| 127 | delay(ms_delay); // Wait the specified amount of time |
| 128 | } |
| 129 | |
| 130 | // This routine will trigger a HC-SR04 sonar that has its |
| 131 | // trigger pin attached to *sonar_trigger* and its echo |
| 132 | // pin attached to *sonar_echo*. If the echo response |
| 133 | // takes longer than *timeout* microseconds, *timeout* is |
| 134 | // returned. |
| 135 | int ping(int sonar_trigger, int sonar_echo, int timeout) { |
| 136 | int result = timeout; |
| 137 | if (!digitalRead(sonar_echo)) { |
| 138 | // *sonar_echo* is LOW, so we can safely trigger a 12uS |
| 139 | // trigger pulse: |
| 140 | digitalWrite(sonar_trigger, HIGH); |
| 141 | delayMicroseconds(12); |
| 142 | digitalWrite(sonar_trigger, LOW); |
| 143 | |
| 144 | // Time the resulting echo pulse. If the resulting echo pulse |
| 145 | // is longer than *timeout*, *pulseIn* returns 0. |
| 146 | result = pulseIn(sonar_echo, HIGH, timeout); |
| 147 | |
| 148 | if (result == 0) { |
| 149 | // We timed out, so return *timeout*: |
| 150 | result = timeout; |
| 151 | } |
| 152 | } // else *sonar_echo* is still high from the previous trigger; |
| 153 | // return *timeout* when this occurs. |
| 154 | return result; |
| 155 | } |
| 156 | |
| 157 | // the loop routine runs over and over again forever: |
| 158 | void loop() { |
| 159 | // Get the left and right sonar values: |
| 160 | int left_ping = ping(left_sonar_trigger, left_sonar_echo, sonar_timeout); |
| 161 | int right_ping = ping(right_sonar_trigger, right_sonar_echo, sonar_timeout); |
| 162 | |
| 163 | // Set the left and right motor speed: |
| 164 | int left_speed = left_ping - sonar_follow_distance; |
| 165 | int right_speed = right_ping - sonar_follow_distance; |
| 166 | |
| 167 | // Debugging code: |
| 168 | if (debug) { |
| 169 | // Print some debugging information: |
| 170 | Serial.print("lp="); |
| 171 | Serial.print(left_ping); |
| 172 | Serial.print(" rp="); |
| 173 | Serial.print(right_ping); |
| 174 | Serial.print(" ls="); |
| 175 | Serial.print(left_speed); |
| 176 | Serial.print(" rs="); |
| 177 | Serial.println(right_speed); |
| 178 | |
| 179 | // Delay a little so that the serial port does not get |
| 180 | // swamped with debugging information: |
| 181 | delay(100); |
| 182 | } |
| 183 | |
| 184 | // Set the motor speeds: |
| 185 | motorsRun(left_speed, right_speed, 0); |
| 186 | } |