5eff75da |
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 | |
16 | // define which pins are connected to which components |
34a3efe6 |
17 | int right_motor_forward_pin = 3; |
18 | int right_motor_backward_pin = 4; |
19 | int right_motor_speed_pin = 5; |
5eff75da |
20 | |
21 | int follow_right_echo_pin = 6; |
22 | int follow_right_trigger_pin = 7; |
23 | |
34a3efe6 |
24 | int left_motor_forward_pin = 8; |
25 | int left_motor_backward_pin = 9; |
26 | int left_motor_speed_pin = 10; |
5eff75da |
27 | |
28 | int follow_left_echo_pin = 11; |
29 | int follow_left_trigger_pin = 12; |
30 | |
31 | int table_right_echo_pin = 13; |
32 | int table_right_trigger_pin = 2; |
33 | |
34 | // you'll need to adjust these based on your sonar sensor's behavior |
35 | int desired_right_follow_ping_time = 800; |
36 | int desired_left_follow_ping_time = 800; |
37 | |
38 | int right_max_ping_time_over_table = 500; |
39 | |
40 | void on(int pin){ |
41 | digitalWrite(pin, HIGH);} |
42 | |
43 | void off(int pin){ |
44 | digitalWrite(pin, LOW);} |
45 | |
46 | void set_motor(int speed_pin, |
47 | int forward_pin, |
48 | int backward_pin, |
49 | int speed){ |
50 | if(speed > 0){ |
51 | off(backward_pin); |
52 | on(forward_pin);} |
53 | else if(speed < 0){ |
54 | off(forward_pin); |
55 | on(backward_pin); |
56 | speed = -speed;} |
57 | else{ // speed is 0 |
58 | off(forward_pin); |
59 | off(backward_pin);} |
60 | // since speed has been set positive, no need to check if speed < -255. |
61 | if(speed > 255){ |
62 | speed = 255;} |
63 | analogWrite(speed_pin, speed);} |
64 | |
65 | void go(int left_motor_speed, int right_motor_speed){ |
66 | set_motor(left_motor_speed_pin, |
67 | left_motor_forward_pin, |
68 | left_motor_backward_pin, |
69 | left_motor_speed); |
70 | set_motor(right_motor_speed_pin, |
71 | right_motor_forward_pin, |
72 | right_motor_backward_pin, |
73 | right_motor_speed);} |
74 | |
75 | int ping(int trigger, int echo){ |
76 | int ping_time = 0; |
77 | // turn off trigger |
78 | off(trigger); |
79 | delayMicroseconds(2); |
80 | // turn on the trigger and leave it on long enough for the |
81 | // sonar sensor to notice |
82 | on(trigger); |
83 | delayMicroseconds(10); |
84 | off(trigger); |
85 | ping_time = pulseIn(echo, HIGH); |
86 | if(ping_time <= 0){ |
87 | ping_time = 3000;} |
88 | // sonar needs some time to recover before pinging again, |
89 | // so make sure it gets enough sleep right here. 50 milliseconds |
90 | delay(50); |
91 | return ping_time;} |
92 | |
93 | void backup(int backup_time){ |
94 | go(-250, -250); |
95 | delay(backup_time);} |
96 | |
97 | void turn_around(int turn_around_time){ |
98 | go(-250, 250); |
99 | delay(turn_around_time);} |
100 | |
101 | void setup(){ |
102 | Serial.begin(9600); |
103 | |
104 | pinMode(right_motor_speed_pin, OUTPUT); |
105 | pinMode(right_motor_forward_pin, OUTPUT); |
106 | pinMode(right_motor_backward_pin, OUTPUT); |
107 | |
108 | pinMode(follow_right_echo_pin, INPUT); |
109 | pinMode(follow_right_trigger_pin, OUTPUT); |
110 | |
111 | pinMode(table_right_echo_pin, INPUT); |
112 | pinMode(table_right_trigger_pin, OUTPUT); |
113 | |
114 | pinMode(left_motor_speed_pin, OUTPUT); |
115 | pinMode(left_motor_forward_pin, OUTPUT); |
116 | pinMode(left_motor_backward_pin, OUTPUT); |
117 | |
118 | pinMode(follow_left_echo_pin, INPUT); |
119 | pinMode(follow_left_trigger_pin, OUTPUT); |
120 | |
121 | off(left_motor_speed_pin); |
122 | off(left_motor_forward_pin); |
123 | off(left_motor_backward_pin); |
124 | off(follow_left_trigger_pin); |
125 | |
126 | off(right_motor_speed_pin); |
127 | off(right_motor_forward_pin); |
128 | off(right_motor_backward_pin); |
129 | off(follow_right_trigger_pin);} |
130 | |
131 | void loop(){ |
132 | int wall_right_min_ping_time = desired_right_follow_ping_time - 50; |
133 | int wall_right_max_ping_time = desired_right_follow_ping_time + 50; |
134 | int wall_left_min_ping_time = desired_left_follow_ping_time - 50; |
135 | int wall_left_max_ping_time = desired_left_follow_ping_time + 50; |
136 | int left_follow_speed; |
137 | int right_follow_speed; |
138 | |
139 | unsigned int actual_left_ping_time = |
140 | ping(follow_left_trigger_pin, follow_left_echo_pin); |
141 | unsigned int actual_right_ping_time = |
142 | ping(follow_right_trigger_pin, follow_right_echo_pin); |
143 | |
144 | unsigned int actual_right_table_ping_time = |
145 | ping(table_right_trigger_pin, table_right_echo_pin); |
146 | |
147 | left_follow_speed = actual_left_ping_time - desired_left_follow_ping_time; |
148 | right_follow_speed = actual_right_ping_time - desired_right_follow_ping_time; |
149 | |
150 | Serial.print("left: ping = "); |
151 | Serial.print(actual_left_ping_time); |
152 | Serial.print(" speed = "); |
153 | Serial.print(left_follow_speed); |
154 | Serial.print(" right: ping = "); |
155 | Serial.print(actual_right_ping_time); |
156 | Serial.print(" speed = "); |
157 | Serial.println(right_follow_speed); |
158 | |
159 | boolean over_table = |
160 | actual_right_table_ping_time < right_max_ping_time_over_table; |
161 | // if the actual ping time is in between the min and max ping time, |
162 | // then the robot is at the desired distance from the wall. |
163 | boolean left_follow_sonar_desired_distance = |
164 | (wall_left_min_ping_time < actual_left_ping_time |
165 | && actual_left_ping_time < wall_left_max_ping_time); |
166 | boolean right_follow_sonar_desired_distance = |
167 | (wall_right_min_ping_time < actual_right_ping_time |
168 | && actual_right_ping_time < wall_right_max_ping_time); |
169 | // if the left and right pings show the desired distance, |
170 | // the robot is facing the wall |
171 | boolean facing_wall = |
172 | left_follow_sonar_desired_distance && right_follow_sonar_desired_distance; |
173 | |
174 | if(over_table){ |
175 | if(facing_wall){ |
176 | backup(1500); |
177 | turn_around(3000);} |
178 | else{ // not facing wall |
179 | go(left_follow_speed, right_follow_speed);}} |
180 | else{ // not over table |
181 | backup(1500); |
182 | turn_around(3000);}} |