a77b88c5 |
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 | Motor. |
17 | Runs both motors back and forth. |
18 | n |
19 | This example code is in the public domain. |
20 | */ |
21 | |
a77b88c5 |
22 | int motor1_enable = 10; |
23 | int motor1a = 9; |
24 | int motor1b = 8; |
25 | int motor2_enable = 3; |
26 | int motor2a = 4; |
27 | int motor2b = 5; |
28 | |
29 | // ping sensors |
30 | int left_trigger = 12; |
31 | int left_echo = 11; |
32 | int right_trigger = 7; |
33 | int right_echo = 6; |
34 | |
35 | // the setup routine runs once when you press reset: |
36 | void setup() { |
37 | Serial.begin(9600); |
38 | |
39 | // initialize the digital pin as an output. |
a77b88c5 |
40 | pinMode(motor1_enable, OUTPUT); |
41 | pinMode(motor1a, OUTPUT); |
42 | pinMode(motor1b, OUTPUT); |
43 | pinMode(motor2_enable, OUTPUT); |
44 | pinMode(motor2a, OUTPUT); |
45 | pinMode(motor2b, OUTPUT); |
46 | |
47 | // ping sensors |
48 | pinMode(left_trigger, OUTPUT); |
49 | pinMode(left_echo, INPUT); |
50 | pinMode(right_trigger, OUTPUT); |
51 | pinMode(right_echo, INPUT); |
52 | |
a77b88c5 |
53 | digitalWrite(motor1_enable, LOW); |
54 | digitalWrite(motor1a, LOW); |
55 | digitalWrite(motor1b, LOW); |
56 | digitalWrite(motor2_enable, LOW); |
57 | digitalWrite(motor2a, LOW); |
58 | digitalWrite(motor2b, LOW); |
59 | } |
60 | |
61 | void |
62 | on(int pin) |
63 | { |
64 | digitalWrite (pin, HIGH); |
65 | } |
66 | |
67 | void |
68 | off(int pin) |
69 | { |
70 | digitalWrite (pin, LOW); |
71 | } |
72 | |
73 | int |
74 | ping(int trigger, int echo) |
75 | { |
76 | int ping_time; |
77 | on(trigger); |
78 | delayMicroseconds(12); |
79 | off(trigger); |
80 | ping_time = pulseIn (echo, HIGH); |
81 | if (ping_time <= 0) |
82 | ping_time = 3000; |
83 | return ping_time; |
84 | } |
85 | |
86 | void motorsRun(int left, int right, int ms_delay) { |
87 | // Set left motor direction: |
88 | if (left > 0) { |
89 | // Set left motor to go forward: |
90 | digitalWrite(motor1a, HIGH); |
91 | digitalWrite(motor1b, LOW); |
92 | } else { |
93 | // Set left motor to go backward: |
94 | digitalWrite(motor1a, LOW); |
95 | digitalWrite(motor1b, HIGH); |
96 | left = -left; // Make left a positive value: |
97 | } |
98 | analogWrite(motor1_enable, left); // Start motor in right direction |
99 | |
100 | // Set left motor direction: |
101 | if (right > 0) { |
102 | // Set right motor to go forward: |
103 | digitalWrite(motor2a, HIGH); |
104 | digitalWrite(motor2b, LOW); |
105 | } else { |
106 | // Set right motor to go backward: |
107 | digitalWrite(motor2a, LOW); |
108 | digitalWrite(motor2b, HIGH); |
109 | right = -right; // Make right a positive value: |
110 | } |
111 | analogWrite(motor2_enable, right); // Start motor in right direction |
112 | |
113 | delay(ms_delay); // Wait the specified amount of time |
114 | } |
115 | |
116 | // the loop routine runs over and over again forever: |
117 | void |
118 | loop() |
119 | { |
120 | int left_speed = 0; |
121 | int right_speed = 0; |
122 | |
123 | int left_ping = ping (left_trigger, left_echo); |
124 | int right_ping = ping (right_trigger, right_echo); |
125 | |
126 | /* |
127 | Serial.print ("left ping = "); |
128 | Serial.print (left_ping); |
129 | |
130 | Serial.print (" right ping = "); |
131 | Serial.println (right_ping); |
132 | */ |
133 | |
134 | if (left_ping < 400) |
135 | { |
136 | left_speed = 250; |
137 | } |
138 | |
139 | if (right_ping < 400) |
140 | { |
141 | right_speed = 250; |
142 | } |
143 | |
144 | if (left_speed == 0 && right_speed == 0) |
145 | { |
146 | // backup |
147 | motorsRun(-250, -250, 2000); |
148 | // turn around |
149 | motorsRun(-250, 250, 3200); |
150 | } |
151 | else |
152 | { |
153 | motorsRun(left_speed, right_speed, 0); |
154 | } |
155 | } |