Index

auto-plow / b3fba95

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

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

Latest Commit

{#}TimeHashSubjectAuthor#(+)(-)GPG?
5126 Jan 2019 17:13b3fba95Full controljoshuas3114436N

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

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