Index

auto-plow / 6681ab8

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

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

Latest Commit

{#}TimeHashSubjectAuthor#(+)(-)GPG?
5227 Jan 2019 01:056681ab8Plow relaysjoshuas313530N

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

text/plain5528 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 Serial.println("Enabling switch pins");
30 pinMode(PIN_MOTOR_BRAKES, OUTPUT);
31 pinMode(PIN_LIGHTS, OUTPUT);
32
33 Serial.println("Enabling relay pins");
34 pinMode(PIN_RELAY_1, OUTPUT);
35 pinMode(PIN_RELAY_2, OUTPUT);
36 pinMode(PIN_RELAY_3, OUTPUT);
37 pinMode(PIN_RELAY_4, OUTPUT);
38 pinMode(PIN_RELAY_5, OUTPUT);
39 pinMode(PIN_RELAY_6, OUTPUT);
40
41 Serial.println("Enabling radio receiver pins");
42 pinMode(PIN_CH_1, INPUT);
43 pinMode(PIN_CH_2, INPUT);
44 pinMode(PIN_CH_3, INPUT);
45 pinMode(PIN_CH_4, INPUT);
46 pinMode(PIN_CH_5, INPUT);
47 pinMode(PIN_CH_6, INPUT);
48}
49
50// Calculate sensitivity threshold
51int left_joystick_threshold = (5 - LEFT_JOYSTICK_SENSITIVITY) * 10;
52int right_joystick_threshold = (5 - RIGHT_JOYSTICK_SENSITIVITY) * 10;
53
54int left_joystick_max = 64 + left_joystick_threshold;
55int left_joystick_min = 64 - left_joystick_threshold;
56
57int right_joystick_max = 64 + right_joystick_threshold;
58int right_joystick_min = 64 - right_joystick_threshold;
59
60
61// Repeatedly
62void loop() {
63 int start = millis();
64
65 // Read from radio receiver
66 int ch1 = pulseIn(PIN_CH_1, RADIO_TIMEOUT);
67 int ch2 = pulseIn(PIN_CH_2, RADIO_TIMEOUT);
68 int ch3 = pulseIn(PIN_CH_3, RADIO_TIMEOUT);
69 int ch4 = pulseIn(PIN_CH_4, RADIO_TIMEOUT);
70 int ch5 = pulseIn(PIN_CH_5, RADIO_TIMEOUT);
71 int ch6 = pulseIn(PIN_CH_6, RADIO_TIMEOUT);
72
73 // ch1: right joystick horizontal, 1099, 1978
74 // ch2: right joystick vertical, 1003, 1986
75 // ch3: left joystick vertical, 993, 1885
76 // ch4: left joystick horizontal, 1023, 1986
77 // ch5: switch d, 993, 1986
78 // ch6: switch a, 993, 1986
79
80 // Constrain radio information
81 if (ch1 < 1099) ch1 = 1099;
82 if (ch1 > 1978) ch1 = 1978;
83
84 if (ch2 < 1003) ch2 = 1003;
85 if (ch2 > 1986) ch2 = 1986;
86
87 if (ch3 < 993) ch3 = 993;
88 if (ch3 > 1885) ch3 = 1885;
89
90 if (ch4 < 1023) ch4 = 1023;
91 if (ch4 > 1986) ch4 = 1986;
92
93 if (ch5 < 993) ch5 = 993;
94 if (ch5 > 1986) ch5 = 1986;
95
96 if (ch6 < 993) ch6 = 993;
97 if (ch6 > 1986) ch6 = 1986;
98
99 // Map receiver information
100 int ch1_fixed = map(ch1, 1099, 1978, 1, 127);
101 int ch2_fixed = map(ch2, 1003, 1986, 1, 127);
102 int ch3_fixed = map(ch3, 993, 1885, 1, 127);
103 int ch4_fixed = map(ch4, 1023, 1986, 1, 127);
104 int ch5_fixed = (ch5 > 1500); // 1 or 0
105 int ch6_fixed = (ch6 > 1500); // 1 or 0
106
107 // Calculate tilt direction
108 int tilt = 0;
109 if (ch4_fixed < left_joystick_min) tilt = -1;
110 if (ch4_fixed > left_joystick_max) tilt = 1;
111
112 // Calculate lift direction
113 int lift = 0;
114 if (ch3_fixed < left_joystick_min) lift = -1;
115 if (ch3_fixed > left_joystick_max) lift = 1;
116
117 // Calculate motor directions
118 if (ch1_fixed < right_joystick_max && ch1_fixed > right_joystick_min) ch1_fixed = 64;
119 if (ch2_fixed < right_joystick_max && ch2_fixed > right_joystick_min) ch2_fixed = 64;
120
121 int motor_left = 64;
122 int motor_right = 64;
123
124 if (ch1_fixed < 64) {
125 motor_right = 64 + (64 - ch1_fixed);
126 motor_left = 64 - (64 - ch1_fixed);
127 } else if (ch1_fixed > 64) {
128 motor_left = 64 + (ch1_fixed - 64);
129 motor_right = 64 - (ch1_fixed - 64);
130 }
131
132 if (ch2_fixed > 64) {
133 motor_right += (ch2_fixed - 64);
134 motor_left += (ch2_fixed - 64);
135 } else if (ch2_fixed < 64) {
136 motor_right -= (64 - ch2_fixed);
137 motor_left -= (64 - ch2_fixed);
138 }
139
140 // Constrain motor directions
141 if (motor_right > 127) motor_right = 127;
142 if (motor_right < 1) motor_right = 1;
143 if (motor_left > 127) motor_left = 127;
144 if (motor_left < 1) motor_left = 1;
145
146 // Offset motor_right number for RoboClaw serial format
147 motor_right += 128;
148
149 #ifdef DEBUG
150 Serial.print("\e[1;1H\e[2J");
151 Serial.print("Left");
152 Serial.print("\t");
153 Serial.print("Right");
154 Serial.print("\t");
155 Serial.print("Lights");
156 Serial.print("\t");
157 Serial.print("Brakes + Motors");
158 Serial.print("\t\t");
159 Serial.print("Plow Tilt");
160 Serial.print("\t");
161 Serial.print("Plow Lift");
162 Serial.println();
163
164 Serial.print(motor_left);
165 Serial.print("\t");
166 Serial.print(motor_right);
167 Serial.print("\t");
168 Serial.print(ch6_fixed);
169 Serial.print("\t");
170 Serial.print(ch5_fixed);
171 Serial.print("\t\t\t");
172 Serial.print(tilt);
173 Serial.print("\t\t");
174 Serial.println(lift);
175 #endif
176
177 // Write motor information to RoboClaw
178 roboclaw.write((byte)motor_left);
179 roboclaw.write((byte)motor_right);
180
181 // Plow tilt (HIGH = LOW, LOW = HIGH)
182 if (tilt == -1) {
183
184 } else if (tilt == 1) {
185
186 } else {
187
188 }
189
190 // Plow lift
191 if (lift == -1) {
192
193 } else if (lift == 1) {
194
195 } else {
196
197 }
198
199 // Lights
200 if (ch6_fixed) {
201 digitalWrite(PIN_LIGHTS, HIGH);
202 } else {
203 digitalWrite(PIN_LIGHTS, LOW);
204 }
205
206 // Power to motor controller
207 if (ch5_fixed) {
208 digitalWrite(PIN_MOTOR_BRAKES, HIGH);
209 } else {
210 digitalWrite(PIN_MOTOR_BRAKES, LOW);
211 }
212
213 int end = millis();
214 #ifdef DEBUG
215 Serial.print("Cycle time: ");
216 Serial.println(end - start);
217 #endif
218}
219