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