Index

auto-plow / 52ee505

A wheelchair motor-propelled battery-powered ESP32-driven remote control snow plow.

Download | Refs | Commit Log | Files | README | LICENSE

Latest Commit

{#}TimeHashSubjectAuthor#(+)(-)GPG?
5412 Feb 2019 02:066100814Updatejoshuas311100N

Blob @ auto-plow / src / ard1 / ard1.cpp

text/plain9207 bytesdownload raw
1// Copyright (c) Joshua 'joshuas3' Stockin 2019
2
3#define DEBUG 1
4
5#include "Arduino.h"
6#include "ard1.h"
7#include "serial.h"
8#include "reset.h"
9
10#include "SoftwareSerial.h"
11SoftwareSerial roboclaw(ROBOCLAW_RX, ROBOCLAW_TX);
12
13// One time
14void setup() {
15 pinMode(PIN_RESET, INPUT);
16 digitalWrite(PIN_RESET, LOW);
17 serial();
18
19 Serial.println("ARD1: ARD1 serial communications active");
20
21 Serial.println("ARD1: Sending power to RoboClaw logic side");
22 pinMode(ROBOCLAW_LOGIC, OUTPUT);
23 digitalWrite(ROBOCLAW_LOGIC, HIGH);
24
25 Serial.print("ARD1: Beginning RoboClaw serial at baud rate ");
26 Serial.println(ROBOCLAW_BAUD);
27 roboclaw.begin(ROBOCLAW_BAUD);
28
29<<<<<<< HEAD
30 Serial.println("Enabling switch pins");
31 pinMode(PIN_MOTOR_BRAKES, OUTPUT);
32 pinMode(PIN_LIGHTS, OUTPUT);
33
34 Serial.println("Enabling relay pins");
35 pinMode(PIN_RELAY_1, OUTPUT);
36 pinMode(PIN_RELAY_2, OUTPUT);
37 pinMode(PIN_RELAY_3, OUTPUT);
38 pinMode(PIN_RELAY_4, OUTPUT);
39 pinMode(PIN_RELAY_5, OUTPUT);
40 pinMode(PIN_RELAY_6, OUTPUT);
41
42 Serial.println("Enabling radio receiver pins");
43=======
44 pinMode(PIN_MOTOR_BRAKES, OUTPUT);
45 pinMode(PIN_LIGHTS, OUTPUT);
46
47 pinMode(PIN_PLOW_LIFT_UP, OUTPUT);
48 pinMode(PIN_PLOW_LIFT_DOWN, OUTPUT);
49 pinMode(PIN_PLOW_TILT_LEFT, OUTPUT);
50 pinMode(PIN_PLOW_TILT_RIGHT, OUTPUT);
51
52>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
53 pinMode(PIN_CH_1, INPUT);
54 pinMode(PIN_CH_2, INPUT);
55 pinMode(PIN_CH_3, INPUT);
56 pinMode(PIN_CH_4, INPUT);
57 pinMode(PIN_CH_5, INPUT);
58 pinMode(PIN_CH_6, INPUT);
59}
60
61<<<<<<< HEAD
62// Calculate sensitivity threshold
63int left_joystick_threshold = (5 - LEFT_JOYSTICK_SENSITIVITY) * 10;
64int right_joystick_threshold = (5 - RIGHT_JOYSTICK_SENSITIVITY) * 10;
65
66int left_joystick_max = 64 + left_joystick_threshold;
67int left_joystick_min = 64 - left_joystick_threshold;
68
69int right_joystick_max = 64 + right_joystick_threshold;
70int right_joystick_min = 64 - right_joystick_threshold;
71
72=======
73int controller_threshold = (5 - CONTROLLER_SENSITIVITY) * 10;
74int joystick_max = 64 + controller_threshold;
75int joystick_min = 64 - controller_threshold;
76int brakes = 1;
77>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
78
79// Repeatedly
80void loop() {
81 int start = millis();
82
83<<<<<<< HEAD
84 // Read from radio receiver
85 int ch1 = pulseIn(PIN_CH_1, RADIO_TIMEOUT);
86 int ch2 = pulseIn(PIN_CH_2, RADIO_TIMEOUT);
87 int ch3 = pulseIn(PIN_CH_3, RADIO_TIMEOUT);
88 int ch4 = pulseIn(PIN_CH_4, RADIO_TIMEOUT);
89 int ch5 = pulseIn(PIN_CH_5, RADIO_TIMEOUT);
90 int ch6 = pulseIn(PIN_CH_6, RADIO_TIMEOUT);
91=======
92 int ch1 = pulseIn(PIN_CH_1, 1000);
93 int ch2 = pulseIn(PIN_CH_2, 1000);
94 int ch3 = pulseIn(PIN_CH_3, 1000);
95 int ch4 = pulseIn(PIN_CH_4, 1000);
96 int ch5 = pulseIn(PIN_CH_5, 1000);
97 int ch6 = pulseIn(PIN_CH_6, 1000);
98>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
99
100 // ch1: right joystick horizontal, 1099, 1978
101 // ch2: right joystick vertical, 1003, 1986
102 // ch3: left joystick vertical, 993, 1885
103 // ch4: left joystick horizontal, 1023, 1986
104 // ch5: switch d, 993, 1986
105 // ch6: switch a, 993, 1986
106
107 // Constrain radio information
108 if (ch1 < 1099) ch1 = 1099;
109 if (ch1 > 1978) ch1 = 1978;
110
111 if (ch2 < 1003) ch2 = 1003;
112 if (ch2 > 1986) ch2 = 1986;
113
114 if (ch3 < 993) ch3 = 993;
115 if (ch3 > 1885) ch3 = 1885;
116
117 if (ch4 < 1023) ch4 = 1023;
118 if (ch4 > 1986) ch4 = 1986;
119
120 if (ch5 < 993) ch5 = 993;
121 if (ch5 > 1986) ch5 = 1986;
122
123 if (ch6 < 993) ch6 = 993;
124 if (ch6 > 1986) ch6 = 1986;
125
126 // Map receiver information
127 int ch1_fixed = map(ch1, 1099, 1978, 1, 127);
128 int ch2_fixed = map(ch2, 1003, 1986, 1, 127);
129 int ch3_fixed = map(ch3, 993, 1885, 1, 127);
130 int ch4_fixed = map(ch4, 1023, 1986, 1, 127);
131<<<<<<< HEAD
132 int ch5_fixed = (ch5 > 1500); // 1 or 0
133 int ch6_fixed = (ch6 > 1500); // 1 or 0
134
135 // Calculate tilt direction
136 int tilt = 0;
137 if (ch4_fixed < left_joystick_min) tilt = -1;
138 if (ch4_fixed > left_joystick_max) tilt = 1;
139
140 // Calculate lift direction
141 int lift = 0;
142 if (ch3_fixed < left_joystick_min) lift = -1;
143 if (ch3_fixed > left_joystick_max) lift = 1;
144
145 // Calculate motor directions
146 if (ch1_fixed < right_joystick_max && ch1_fixed > right_joystick_min) ch1_fixed = 64;
147 if (ch2_fixed < right_joystick_max && ch2_fixed > right_joystick_min) ch2_fixed = 64;
148=======
149 int ch5_fixed = (ch5 > 1500);
150 int ch6_fixed = (ch6 > 1500);
151
152 int tilt = 0;
153 if (ch4_fixed < joystick_min) tilt = -1;
154 if (ch4_fixed > joystick_max) tilt = 1;
155
156 int lift = 0;
157 if (ch3_fixed < joystick_min) lift = -1;
158 if (ch3_fixed > joystick_max) lift = 1;
159
160 if (ch1_fixed < joystick_max && ch1_fixed > joystick_min) ch1_fixed = 64;
161 if (ch2_fixed < joystick_max && ch2_fixed > joystick_min) ch2_fixed = 64;
162>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
163
164 int motor_left = 64;
165 int motor_right = 64;
166
167 if (ch1_fixed < 64) {
168 motor_right = 64 + (64 - ch1_fixed);
169 motor_left = 64 - (64 - ch1_fixed);
170 } else if (ch1_fixed > 64) {
171 motor_left = 64 + (ch1_fixed - 64);
172 motor_right = 64 - (ch1_fixed - 64);
173 }
174
175 if (ch2_fixed > 64) {
176 motor_right += (ch2_fixed - 64);
177 motor_left += (ch2_fixed - 64);
178 } else if (ch2_fixed < 64) {
179 motor_right -= (64 - ch2_fixed);
180 motor_left -= (64 - ch2_fixed);
181 }
182
183<<<<<<< HEAD
184 // Constrain motor directions
185=======
186>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
187 if (motor_right > 127) motor_right = 127;
188 if (motor_right < 1) motor_right = 1;
189 if (motor_left > 127) motor_left = 127;
190 if (motor_left < 1) motor_left = 1;
191
192<<<<<<< HEAD
193 // Offset motor_right number for RoboClaw serial format
194 motor_right += 128;
195
196 #ifdef DEBUG
197=======
198 motor_right += 128;
199
200 int end = millis();
201
202>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
203 Serial.print("\e[1;1H\e[2J");
204 Serial.print("Left");
205 Serial.print("\t");
206 Serial.print("Right");
207<<<<<<< HEAD
208 Serial.print("\t");
209 Serial.print("Lights");
210 Serial.print("\t");
211 Serial.print("Brakes + Motors");
212 Serial.print("\t\t");
213 Serial.print("Plow Tilt");
214 Serial.print("\t");
215 Serial.print("Plow Lift");
216 Serial.println();
217
218 Serial.print(motor_left);
219 Serial.print("\t");
220 Serial.print(motor_right);
221 Serial.print("\t");
222 Serial.print(ch6_fixed);
223 Serial.print("\t");
224 Serial.print(ch5_fixed);
225 Serial.print("\t\t\t");
226 Serial.print(tilt);
227 Serial.print("\t\t");
228 Serial.println(lift);
229 #endif
230
231 // Write motor information to RoboClaw
232 roboclaw.write((byte)motor_left);
233 roboclaw.write((byte)motor_right);
234
235 // Plow tilt (HIGH = LOW, LOW = HIGH)
236 if (tilt == -1) {
237 digitalWrite(PIN_RELAY_4, HIGH);
238 digitalWrite(PIN_RELAY_5, LOW);
239 digitalWrite(PIN_RELAY_6, LOW);
240 } else if (tilt == 1) {
241 digitalWrite(PIN_RELAY_5, HIGH);
242 digitalWrite(PIN_RELAY_6, HIGH);
243 digitalWrite(PIN_RELAY_4, LOW);
244 } else {
245 digitalWrite(PIN_RELAY_4, HIGH);
246 digitalWrite(PIN_RELAY_5, HIGH);
247 digitalWrite(PIN_RELAY_6, HIGH);
248 }
249
250 // Plow lift
251 if (lift == -1) {
252 digitalWrite(PIN_RELAY_1, HIGH);
253 digitalWrite(PIN_RELAY_2, LOW);
254 digitalWrite(PIN_RELAY_3, LOW);
255 } else if (lift == 1) {
256 digitalWrite(PIN_RELAY_2, HIGH);
257 digitalWrite(PIN_RELAY_3, HIGH);
258 digitalWrite(PIN_RELAY_1, LOW);
259 } else {
260 digitalWrite(PIN_RELAY_1, HIGH);
261 digitalWrite(PIN_RELAY_2, HIGH);
262 digitalWrite(PIN_RELAY_3, HIGH);
263 }
264
265 // Lights
266=======
267 Serial.print("\t");
268 Serial.print("Lights");
269 Serial.print("\t");
270 Serial.print("Brakes + Motors");
271 Serial.print("\t\t");
272 Serial.print("Plow Tilt");
273 Serial.print("\t");
274 Serial.print("Plow Lift");
275 Serial.println();
276
277 Serial.print(motor_left);
278 Serial.print("\t");
279 Serial.print(motor_right);
280 Serial.print("\t");
281 Serial.print(ch6_fixed);
282 Serial.print("\t");
283 Serial.print(ch5_fixed);
284 Serial.print("\t\t\t");
285 Serial.print(tilt);
286 Serial.print("\t\t");
287 Serial.println(lift);
288
289 Serial.print("Cycle time: ");
290 Serial.println(end - start);
291
292 roboclaw.write((byte)motor_left);
293 roboclaw.write((byte)motor_right);
294
295 if (tilt == -1) {
296 digitalWrite(PIN_PLOW_TILT_RIGHT, LOW);
297 digitalWrite(PIN_PLOW_TILT_LEFT, HIGH);
298 } else if (tilt == 0) {
299 digitalWrite(PIN_PLOW_TILT_RIGHT, LOW);
300 digitalWrite(PIN_PLOW_TILT_LEFT, LOW);
301 } else if (tilt == 1) {
302 digitalWrite(PIN_PLOW_TILT_LEFT, LOW);
303 digitalWrite(PIN_PLOW_TILT_RIGHT, HIGH);
304 }
305 if (lift == -1) {
306 digitalWrite(PIN_PLOW_LIFT_UP, LOW);
307 digitalWrite(PIN_PLOW_LIFT_DOWN, HIGH);
308 } else if (lift == 0) {
309 digitalWrite(PIN_PLOW_LIFT_UP, LOW);
310 digitalWrite(PIN_PLOW_LIFT_DOWN, LOW);
311 } else if (tilt == 1) {
312 digitalWrite(PIN_PLOW_LIFT_DOWN, LOW);
313 digitalWrite(PIN_PLOW_LIFT_UP, HIGH);
314 }
315>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
316 if (ch6_fixed) {
317 digitalWrite(PIN_LIGHTS, HIGH);
318 } else {
319 digitalWrite(PIN_LIGHTS, LOW);
320 }
321<<<<<<< HEAD
322
323 // Power to motor controller
324=======
325>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
326 if (ch5_fixed) {
327 digitalWrite(PIN_MOTOR_BRAKES, HIGH);
328 } else {
329 digitalWrite(PIN_MOTOR_BRAKES, LOW);
330 }
331<<<<<<< HEAD
332
333 int end = millis();
334 #ifdef DEBUG
335 Serial.print("Cycle time: ");
336 Serial.println(end - start);
337 #endif
338=======
339>>>>>>> a85aa7c2aa323a809d7357d17b4ce3a517dd965f
340}
341