ce941be7 |
1 | // license info at bottom of file |
2 | // tl;dr AGPLv3+ |
e0854435 |
3 | |
ce941be7 |
4 | #include "bot_map.h" |
8a033999 |
5 | |
e0854435 |
6 | void on(int pin){ |
7 | digitalWrite(pin, HIGH);} |
8 | |
9 | void off(int pin){ |
10 | digitalWrite(pin, LOW);} |
11 | |
12 | void set_motor(int speed_pin, |
13 | int forward_pin, |
14 | int backward_pin, |
15 | int speed){ |
16 | if(speed > 0){ |
17 | off(backward_pin); |
18 | on(forward_pin);} |
19 | else if(speed < 0){ |
20 | off(forward_pin); |
21 | on(backward_pin); |
22 | speed = -speed;} |
23 | else{ // speed is 0 |
24 | off(forward_pin); |
25 | off(backward_pin);} |
26 | // since speed has been set positive, no need to check if speed < -255. |
27 | if(speed > 255){ |
28 | speed = 255;} |
29 | analogWrite(speed_pin, speed);} |
30 | |
31 | void go(int left_motor_speed, int right_motor_speed){ |
32 | set_motor(left_motor_speed_pin, |
33 | left_motor_forward_pin, |
34 | left_motor_backward_pin, |
35 | left_motor_speed); |
36 | set_motor(right_motor_speed_pin, |
37 | right_motor_forward_pin, |
38 | right_motor_backward_pin, |
39 | right_motor_speed);} |
40 | |
41 | int ping(int trigger, int echo){ |
42 | int ping_time = 0; |
43 | // turn off trigger |
44 | off(trigger); |
45 | delayMicroseconds(2); |
46 | // turn on the trigger and leave it on long enough for the |
47 | // sonar sensor to notice |
48 | on(trigger); |
49 | delayMicroseconds(10); |
50 | off(trigger); |
51 | ping_time = pulseIn(echo, HIGH); |
52 | if(ping_time <= 0){ |
53 | ping_time = 3000;} |
54 | // sonar needs some time to recover before pinging again, |
55 | // so make sure it gets enough sleep right here. 50 milliseconds |
56 | delay(50); |
57 | return ping_time;} |
58 | |
e0854435 |
59 | void setup(){ |
60 | Serial.begin(9600); |
61 | |
62 | pinMode(right_motor_speed_pin, OUTPUT); |
63 | pinMode(right_motor_forward_pin, OUTPUT); |
64 | pinMode(right_motor_backward_pin, OUTPUT); |
65 | |
66 | pinMode(right_echo_pin, INPUT); |
67 | pinMode(right_trigger_pin, OUTPUT); |
68 | |
69 | pinMode(left_motor_speed_pin, OUTPUT); |
70 | pinMode(left_motor_forward_pin, OUTPUT); |
71 | pinMode(left_motor_backward_pin, OUTPUT); |
72 | |
73 | pinMode(left_echo_pin, INPUT); |
74 | pinMode(left_trigger_pin, OUTPUT); |
75 | |
76 | off(left_motor_speed_pin); |
77 | off(left_motor_forward_pin); |
78 | off(left_motor_backward_pin); |
79 | off(left_trigger_pin); |
80 | |
81 | off(right_motor_speed_pin); |
82 | off(right_motor_forward_pin); |
83 | off(right_motor_backward_pin); |
3a60191c |
84 | off(right_trigger_pin); } |
e0854435 |
85 | |
3a60191c |
86 | void loop() { |
e0854435 |
87 | int left_speed; |
88 | int right_speed; |
89 | |
4fb38d02 |
90 | // you'll need to adjust these based on your sonar sensor's behavior |
91 | int desired_right_ping_time = 800; |
92 | int desired_left_ping_time = 800; |
e0854435 |
93 | |
94 | unsigned int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin); |
95 | unsigned int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin); |
96 | |
ce941be7 |
97 | /* int left_led_value = (int)(actual_left_ping_time / 16.0); */ |
98 | /* int right_led_value = (int)(actual_right_ping_time / 16.0); */ |
8a033999 |
99 | |
ce941be7 |
100 | /* analogWrite(left_led_pin, left_led_value); */ |
101 | /* analogWrite(right_led_pin, right_led_value); */ |
8a033999 |
102 | |
ce941be7 |
103 | /* Serial.print("left led value = "); */ |
104 | /* Serial.print(left_led_value); */ |
105 | /* Serial.print(", right led value = "); */ |
106 | /* Serial.print(right_led_value); */ |
8a033999 |
107 | |
e0854435 |
108 | left_speed = actual_left_ping_time - desired_left_ping_time; |
109 | right_speed = actual_right_ping_time - desired_right_ping_time; |
110 | |
8a033999 |
111 | Serial.print(", left: ping = "); |
e0854435 |
112 | Serial.print(actual_left_ping_time); |
113 | Serial.print(" speed = "); |
114 | Serial.print(left_speed); |
115 | Serial.print(" right: ping = "); |
116 | Serial.print(actual_right_ping_time); |
117 | Serial.print(" speed = "); |
3a60191c |
118 | Serial.print(right_speed); |
119 | Serial.println(); |
e0854435 |
120 | |
3a60191c |
121 | go(left_speed, right_speed); } |
ce941be7 |
122 | |
123 | /* |
124 | This program is free software: you can redistribute it and/or modify |
125 | it under the terms of the GNU Affero General Public License as |
126 | published by the Free Software Foundation, either version 3 of the |
127 | License, or (at your option) any later version. |
128 | |
129 | This program is distributed in the hope that it will be useful, |
130 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
131 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
132 | GNU Affero General Public License for more details. |
133 | |
134 | You should have received a copy of the GNU Affero General Public License |
135 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
136 | */ |