e0854435 |
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 | |
8a033999 |
16 | // right side LED, white wire |
17 | int right_led_pin = 3; |
18 | |
e0854435 |
19 | // define which pins are connected to which components |
661ac21e |
20 | // blue wire |
8a033999 |
21 | int right_motor_forward_pin = 2; |
661ac21e |
22 | // orange wire |
34a3efe6 |
23 | int right_motor_backward_pin = 4; |
661ac21e |
24 | // white wire |
34a3efe6 |
25 | int right_motor_speed_pin = 5; |
e0854435 |
26 | |
661ac21e |
27 | // yellow wire |
5218c75f |
28 | int right_echo_pin = 6; |
661ac21e |
29 | // blue wire |
5218c75f |
30 | int right_trigger_pin = 7; |
e0854435 |
31 | |
661ac21e |
32 | // blue wire |
34a3efe6 |
33 | int left_motor_forward_pin = 8; |
661ac21e |
34 | // orange wire |
8a033999 |
35 | int left_motor_backward_pin = 13; |
661ac21e |
36 | // white wire |
34a3efe6 |
37 | int left_motor_speed_pin = 10; |
e0854435 |
38 | |
661ac21e |
39 | // yellow wire |
5218c75f |
40 | int left_echo_pin = 11; |
661ac21e |
41 | // blue wire |
5218c75f |
42 | int left_trigger_pin = 12; |
e0854435 |
43 | |
8a033999 |
44 | // left side LED, blue wire |
45 | int left_led_pin = 9; |
46 | |
e0854435 |
47 | void on(int pin){ |
48 | digitalWrite(pin, HIGH);} |
49 | |
50 | void off(int pin){ |
51 | digitalWrite(pin, LOW);} |
52 | |
53 | void set_motor(int speed_pin, |
54 | int forward_pin, |
55 | int backward_pin, |
56 | int speed){ |
57 | if(speed > 0){ |
58 | off(backward_pin); |
59 | on(forward_pin);} |
60 | else if(speed < 0){ |
61 | off(forward_pin); |
62 | on(backward_pin); |
63 | speed = -speed;} |
64 | else{ // speed is 0 |
65 | off(forward_pin); |
66 | off(backward_pin);} |
67 | // since speed has been set positive, no need to check if speed < -255. |
68 | if(speed > 255){ |
69 | speed = 255;} |
70 | analogWrite(speed_pin, speed);} |
71 | |
72 | void go(int left_motor_speed, int right_motor_speed){ |
73 | set_motor(left_motor_speed_pin, |
74 | left_motor_forward_pin, |
75 | left_motor_backward_pin, |
76 | left_motor_speed); |
77 | set_motor(right_motor_speed_pin, |
78 | right_motor_forward_pin, |
79 | right_motor_backward_pin, |
80 | right_motor_speed);} |
81 | |
82 | int ping(int trigger, int echo){ |
83 | int ping_time = 0; |
84 | // turn off trigger |
85 | off(trigger); |
86 | delayMicroseconds(2); |
87 | // turn on the trigger and leave it on long enough for the |
88 | // sonar sensor to notice |
89 | on(trigger); |
90 | delayMicroseconds(10); |
91 | off(trigger); |
92 | ping_time = pulseIn(echo, HIGH); |
93 | if(ping_time <= 0){ |
94 | ping_time = 3000;} |
95 | // sonar needs some time to recover before pinging again, |
96 | // so make sure it gets enough sleep right here. 50 milliseconds |
97 | delay(50); |
98 | return ping_time;} |
99 | |
e0854435 |
100 | void setup(){ |
101 | Serial.begin(9600); |
102 | |
103 | pinMode(right_motor_speed_pin, OUTPUT); |
104 | pinMode(right_motor_forward_pin, OUTPUT); |
105 | pinMode(right_motor_backward_pin, OUTPUT); |
106 | |
107 | pinMode(right_echo_pin, INPUT); |
108 | pinMode(right_trigger_pin, OUTPUT); |
109 | |
110 | pinMode(left_motor_speed_pin, OUTPUT); |
111 | pinMode(left_motor_forward_pin, OUTPUT); |
112 | pinMode(left_motor_backward_pin, OUTPUT); |
113 | |
114 | pinMode(left_echo_pin, INPUT); |
115 | pinMode(left_trigger_pin, OUTPUT); |
116 | |
117 | off(left_motor_speed_pin); |
118 | off(left_motor_forward_pin); |
119 | off(left_motor_backward_pin); |
120 | off(left_trigger_pin); |
121 | |
122 | off(right_motor_speed_pin); |
123 | off(right_motor_forward_pin); |
124 | off(right_motor_backward_pin); |
3a60191c |
125 | off(right_trigger_pin); } |
e0854435 |
126 | |
3a60191c |
127 | void loop() { |
e0854435 |
128 | int left_speed; |
129 | int right_speed; |
130 | |
4fb38d02 |
131 | // you'll need to adjust these based on your sonar sensor's behavior |
132 | int desired_right_ping_time = 800; |
133 | int desired_left_ping_time = 800; |
e0854435 |
134 | |
135 | unsigned int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin); |
136 | unsigned int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin); |
137 | |
8a033999 |
138 | int left_led_value = (int)(actual_left_ping_time / 16.0); |
139 | int right_led_value = (int)(actual_right_ping_time / 16.0); |
140 | |
141 | Serial.print("left led value = "); |
142 | Serial.print(left_led_value); |
143 | Serial.print(", right led value = "); |
144 | Serial.print(right_led_value); |
145 | |
146 | analogWrite(left_led_pin, left_led_value); |
147 | analogWrite(right_led_pin, right_led_value); |
148 | |
e0854435 |
149 | left_speed = actual_left_ping_time - desired_left_ping_time; |
150 | right_speed = actual_right_ping_time - desired_right_ping_time; |
151 | |
8a033999 |
152 | Serial.print(", left: ping = "); |
e0854435 |
153 | Serial.print(actual_left_ping_time); |
154 | Serial.print(" speed = "); |
155 | Serial.print(left_speed); |
156 | Serial.print(" right: ping = "); |
157 | Serial.print(actual_right_ping_time); |
158 | Serial.print(" speed = "); |
3a60191c |
159 | Serial.print(right_speed); |
160 | Serial.println(); |
e0854435 |
161 | |
3a60191c |
162 | go(left_speed, right_speed); } |