73ad8845 |
1 | |
2 | /* |
3 | This program is free software: you can redistribute it and/or modify |
4 | it under the terms of the GNU Affero General Public License as |
5 | published by the Free Software Foundation, either version 3 of the |
6 | License, or (at your option) any later version. |
7 | |
8 | This program is distributed in the hope that it will be useful, |
9 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
11 | GNU Affero General Public License for more details. |
12 | |
13 | You should have received a copy of the GNU Affero General Public License |
14 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
15 | */ |
16 | |
17 | #include "bot_map.h" |
18 | |
19 | void on(int pin){ |
20 | digitalWrite(pin, HIGH);} |
21 | |
22 | void off(int pin){ |
23 | digitalWrite(pin, LOW);} |
24 | |
25 | void set_motor(int speed_pin, |
26 | int forward_pin, |
27 | int backward_pin, |
28 | int speed){ |
29 | if(speed > 0){ |
30 | off(backward_pin); |
31 | on(forward_pin);} |
32 | else if(speed < 0){ |
33 | off(forward_pin); |
34 | on(backward_pin); |
35 | speed = -speed;} |
36 | else{ // speed is 0 |
37 | off(forward_pin); |
38 | off(backward_pin);} |
39 | // since speed has been set positive, no need to check if speed < -255. |
40 | if(speed > 255){ |
41 | speed = 255;} |
42 | analogWrite(speed_pin, speed);} |
43 | |
44 | void go(int left_motor_speed, int right_motor_speed){ |
45 | set_motor(left_motor_speed_pin, |
46 | left_motor_forward_pin, |
47 | left_motor_backward_pin, |
48 | left_motor_speed); |
49 | set_motor(right_motor_speed_pin, |
50 | right_motor_forward_pin, |
51 | right_motor_backward_pin, |
52 | right_motor_speed);} |
53 | |
54 | int ping(int trigger, int echo){ |
55 | int ping_time = 0; |
56 | // turn off trigger |
57 | off(trigger); |
58 | delayMicroseconds(2); |
59 | // turn on the trigger and leave it on long enough for the |
60 | // sonar sensor to notice |
61 | on(trigger); |
62 | delayMicroseconds(10); |
63 | off(trigger); |
64 | ping_time = pulseIn(echo, HIGH); |
65 | if(ping_time <= 0){ |
66 | ping_time = 3000;} |
67 | // sonar needs some time to recover before pinging again, |
68 | // so make sure it gets enough sleep right here. 50 milliseconds |
69 | delay(50); |
70 | return ping_time;} |
71 | |
72 | void backup(int backup_time){ |
73 | go(-250, -250); |
74 | delay(backup_time);} |
75 | |
76 | void turn_around(int turn_around_time){ |
77 | go(-250, 250); |
78 | delay(turn_around_time);} |
79 | |
80 | void setup(){ |
81 | Serial.begin(9600); |
82 | |
83 | pinMode(LED_BUILTIN, OUTPUT); |
84 | |
85 | pinMode(button_pin, INPUT_PULLUP); |
86 | |
87 | pinMode(right_motor_speed_pin, OUTPUT); |
88 | pinMode(right_motor_forward_pin, OUTPUT); |
89 | pinMode(right_motor_backward_pin, OUTPUT); |
90 | |
91 | pinMode(right_echo_pin, INPUT); |
92 | pinMode(right_trigger_pin, OUTPUT); |
93 | |
94 | pinMode(left_motor_speed_pin, OUTPUT); |
95 | pinMode(left_motor_forward_pin, OUTPUT); |
96 | pinMode(left_motor_backward_pin, OUTPUT); |
97 | |
98 | pinMode(left_echo_pin, INPUT); |
99 | pinMode(left_trigger_pin, OUTPUT); |
100 | |
101 | off(left_motor_speed_pin); |
102 | off(left_motor_forward_pin); |
103 | off(left_motor_backward_pin); |
104 | off(left_trigger_pin); |
105 | |
106 | off(right_motor_speed_pin); |
107 | off(right_motor_forward_pin); |
108 | off(right_motor_backward_pin); |
109 | off(right_trigger_pin);} |
110 | |
111 | void stay_on_table(){ |
112 | int left_speed; |
113 | int right_speed; |
114 | |
115 | int forward_speed = 250; |
116 | int stop_speed = 0; |
117 | |
118 | // adjust this number as necessary for your robot. |
119 | // it represents how far the table is from your sonar sensor. |
120 | // larger values mean larger distance. default is 800 |
121 | int right_max_ping_time_over_table = 800; |
122 | int left_max_ping_time_over_table = 800; |
123 | int backup_time = 500; |
124 | // the exact amount of time for turning around might need |
125 | // twerking for your robot. the default value is 3200 |
126 | int turn_around_time = 2500; |
127 | |
128 | int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin); |
129 | int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin); |
130 | |
131 | /* int left_led_value = (int)(actual_left_ping_time / 16.0); */ |
132 | /* int right_led_value = (int)(actual_right_ping_time / 16.0); */ |
133 | |
134 | /* Serial.print("left led value = "); */ |
135 | /* Serial.print(left_led_value); */ |
136 | /* Serial.print(", right led value = "); */ |
137 | /* Serial.print(right_led_value); */ |
138 | |
139 | /* analogWrite(left_led_pin, left_led_value); */ |
140 | /* analogWrite(right_led_pin, right_led_value); */ |
141 | |
142 | Serial.print("left ping = "); |
143 | Serial.print(actual_left_ping_time); |
144 | Serial.print("\tright_ping = "); |
145 | Serial.println(actual_right_ping_time); |
146 | |
147 | // if the left sonar senses a table, keep driving left side forward, |
148 | // otherwise, stop left side |
149 | if(actual_left_ping_time < left_max_ping_time_over_table){ |
150 | left_speed = forward_speed;} |
151 | else{ |
152 | left_speed = stop_speed;} |
153 | // if the right sonar senses a table, keep driving right side forward, |
154 | // otherwise, stop right side |
155 | if(actual_right_ping_time < right_max_ping_time_over_table){ |
156 | right_speed = forward_speed;} |
157 | else{ |
158 | right_speed = stop_speed;} |
159 | |
160 | // if both sonars detect being off the table, backup and turn around |
161 | // otherwise, go the correct speed for each wheel |
162 | if(actual_left_ping_time >= left_max_ping_time_over_table |
163 | && actual_right_ping_time >= right_max_ping_time_over_table){ |
164 | Serial.println("backing up"); |
165 | backup(backup_time); |
166 | Serial.println("turning around"); |
167 | turn_around(turn_around_time);} |
168 | else{ |
169 | Serial.println("going"); |
170 | go(left_speed, right_speed);}} |
171 | |
172 | void follow() { |
173 | Serial.println("following!"); |
174 | delay(100); } |
175 | |
176 | void loop() { |
177 | digitalWrite(LED_BUILTIN, digitalRead(button_pin)); } |
178 | |
179 | /* |
180 | enum Behavior {STAY_ON_TABLE, FOLLOW}; |
181 | Behavior behavior = STAY_ON_TABLE; |
182 | |
183 | enum Button_state {UP, DOWN}; |
184 | Button_state prior_button_state = UP; |
185 | |
186 | enum Button_state get_button_state() { |
187 | return |
188 | (digitalRead(button_pin) == LOW) |
189 | ? DOWN |
190 | : UP; } |
191 | void loop_asdf() { |
192 | static Button_state button_state = getButtonState(); |
193 | |
194 | // if button was just pressed |
195 | if (prior_button_state == UP && button_state == DOWN) { |
196 | // indicate button press recognized |
197 | on(LED_BUILTIN); |
198 | // turn off motors, to allow robot to be set down |
199 | go(0, 0); |
200 | delay(1000); |
201 | switch(behavior) { |
202 | case STAY_ON_TABLE: behavior = FOLLOW; break; |
203 | case FOLLOW: behavior = STAY_ON_TABLE; break; } |
204 | off(LED_BUILTIN); } |
205 | |
206 | switch(behavior) { |
207 | case STAY_ON_TABLE: stay_on_table(); break; |
208 | case FOLLOW: follow(); break; } |
209 | |
210 | prior_button_state = button_state; } |
211 | */ |