fa6c6440 |
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 |
661ac21e |
17 | // blue wire |
7c21fba2 |
18 | int right_motor_forward_pin = 3; |
661ac21e |
19 | // orange wire |
7c21fba2 |
20 | int right_motor_backward_pin = 4; |
661ac21e |
21 | // white wire |
7c21fba2 |
22 | int right_motor_speed_pin = 5; |
fa6c6440 |
23 | |
661ac21e |
24 | // yellow wire |
5218c75f |
25 | int right_echo_pin = 6; |
661ac21e |
26 | // blue wire |
5218c75f |
27 | int right_trigger_pin = 7; |
fa6c6440 |
28 | |
661ac21e |
29 | // blue wire |
34a3efe6 |
30 | int left_motor_forward_pin = 8; |
661ac21e |
31 | // orange wire |
34a3efe6 |
32 | int left_motor_backward_pin = 9; |
661ac21e |
33 | // white wire |
34a3efe6 |
34 | int left_motor_speed_pin = 10; |
fa6c6440 |
35 | |
661ac21e |
36 | // yellow wire |
5218c75f |
37 | int left_echo_pin = 11; |
661ac21e |
38 | // blue wire |
5218c75f |
39 | int left_trigger_pin = 12; |
fa6c6440 |
40 | |
41 | void on(int pin){ |
42 | digitalWrite(pin, HIGH);} |
43 | |
44 | void off(int pin){ |
45 | digitalWrite(pin, LOW);} |
46 | |
47 | void set_motor(int speed_pin, |
48 | int forward_pin, |
49 | int backward_pin, |
50 | int speed){ |
51 | if(speed > 0){ |
52 | off(backward_pin); |
53 | on(forward_pin);} |
54 | else if(speed < 0){ |
55 | off(forward_pin); |
56 | on(backward_pin); |
57 | speed = -speed;} |
58 | else{ // speed is 0 |
59 | off(forward_pin); |
60 | off(backward_pin);} |
d77c514c |
61 | // since speed has been set positive, no need to check if speed < -255. |
fa6c6440 |
62 | if(speed > 255){ |
63 | speed = 255;} |
64 | analogWrite(speed_pin, speed);} |
65 | |
66 | void go(int left_motor_speed, int right_motor_speed){ |
67 | set_motor(left_motor_speed_pin, |
68 | left_motor_forward_pin, |
69 | left_motor_backward_pin, |
70 | left_motor_speed); |
71 | set_motor(right_motor_speed_pin, |
72 | right_motor_forward_pin, |
73 | right_motor_backward_pin, |
74 | right_motor_speed);} |
75 | |
76 | int ping(int trigger, int echo){ |
77 | int ping_time = 0; |
78 | // turn off trigger |
79 | off(trigger); |
80 | delayMicroseconds(2); |
81 | // turn on the trigger and leave it on long enough for the |
82 | // sonar sensor to notice |
83 | on(trigger); |
84 | delayMicroseconds(10); |
85 | off(trigger); |
86 | ping_time = pulseIn(echo, HIGH); |
87 | if(ping_time <= 0){ |
88 | ping_time = 3000;} |
89 | // sonar needs some time to recover before pinging again, |
90 | // so make sure it gets enough sleep right here. 50 milliseconds |
91 | delay(50); |
92 | return ping_time;} |
93 | |
94 | void backup(int backup_time){ |
95 | go(-250, -250); |
96 | delay(backup_time);} |
97 | |
98 | void turn_around(int turn_around_time){ |
99 | go(-250, 250); |
100 | delay(turn_around_time);} |
101 | |
102 | void setup(){ |
103 | Serial.begin(9600); |
104 | |
105 | pinMode(right_motor_speed_pin, OUTPUT); |
106 | pinMode(right_motor_forward_pin, OUTPUT); |
107 | pinMode(right_motor_backward_pin, OUTPUT); |
108 | |
109 | pinMode(right_echo_pin, INPUT); |
110 | pinMode(right_trigger_pin, OUTPUT); |
111 | |
112 | pinMode(left_motor_speed_pin, OUTPUT); |
113 | pinMode(left_motor_forward_pin, OUTPUT); |
114 | pinMode(left_motor_backward_pin, OUTPUT); |
115 | |
116 | pinMode(left_echo_pin, INPUT); |
117 | pinMode(left_trigger_pin, OUTPUT); |
118 | |
119 | off(left_motor_speed_pin); |
120 | off(left_motor_forward_pin); |
121 | off(left_motor_backward_pin); |
122 | off(left_trigger_pin); |
123 | |
124 | off(right_motor_speed_pin); |
125 | off(right_motor_forward_pin); |
126 | off(right_motor_backward_pin); |
127 | off(right_trigger_pin);} |
128 | |
129 | void loop(){ |
130 | int left_speed; |
131 | int right_speed; |
132 | |
133 | int forward_speed = 250; |
134 | int stop_speed = 0; |
135 | |
136 | // adjust this number as necessary for your robot. |
137 | // it represents how far the table is from your sonar sensor. |
b3ab14a7 |
138 | // larger values mean larger distance. default is 800 |
139 | int right_max_ping_time_over_table = 800; |
140 | int left_max_ping_time_over_table = 800; |
fa6c6440 |
141 | int backup_time = 2000; |
142 | // the exact amount of time for turning around might need |
143 | // twerking for your robot. the default value is 3200 |
b3ab14a7 |
144 | int turn_around_time = 3200; |
fa6c6440 |
145 | |
146 | int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin); |
147 | int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin); |
148 | |
149 | Serial.print("left ping = "); |
150 | Serial.print(actual_left_ping_time); |
151 | Serial.print("\tright_ping = "); |
152 | Serial.println(actual_right_ping_time); |
153 | |
154 | // if the left sonar senses a table, keep driving left side forward, |
155 | // otherwise, stop left side |
156 | if(actual_left_ping_time < left_max_ping_time_over_table){ |
157 | left_speed = forward_speed;} |
158 | else{ |
159 | left_speed = stop_speed;} |
160 | // if the right sonar senses a table, keep driving right side forward, |
161 | // otherwise, stop right side |
162 | if(actual_right_ping_time < right_max_ping_time_over_table){ |
163 | right_speed = forward_speed;} |
164 | else{ |
165 | right_speed = stop_speed;} |
166 | |
167 | // if both sonars detect being off the table, backup and turn around |
168 | // otherwise, go the correct speed for each wheel |
169 | if(actual_left_ping_time >= left_max_ping_time_over_table |
170 | && actual_right_ping_time >= right_max_ping_time_over_table){ |
171 | Serial.println("backing up"); |
172 | backup(backup_time); |
173 | Serial.println("turning around"); |
174 | turn_around(turn_around_time);} |
175 | else{ |
176 | Serial.println("going"); |
177 | go(left_speed, right_speed);}} |