Index

auto-plow / b8b58d3

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

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

Latest Commit

{#}TimeHashSubjectAuthor#(+)(-)GPG?
5617 Feb 2019 16:38b8b58d3Good stuffjoshuas314014N

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

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