Index

auto-plow / b88d028

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

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

Latest Commit

{#}TimeHashSubjectAuthor#(+)(-)GPG?
5512 Feb 2019 02:253630027Fix weird git conflictions, remove plow control codejoshuas3112174N

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

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