/*
ESC Control For Tracked R/C Vehicles
Mitch Regnier <[email protected]>
Dec 2024
Arduino PWM Pins - 3, 5, 6, 9, 10, 11 (hardware PWM out, software PWM can be used on any pin)
For r/c systems, pulse width in microseconds determines position or speed:
1000 microseconds = 0 degrees or full clockwise or full reverse
1500 microseconds = 90 degrees or full centered or zero throttle
2000 microseconds = 180 degrees or full counterclowise or full forward
This program uses the Servo.h library to control the "servos" which are electronic speed controllers (ESCs) in this case.
The standard servo.write() function is used instead servo.writeMicroseconds() since servo.write() defaults to
microseconds outside of the 0 - 180 range.
pulseIn() timeout is set to 100 microseconds. For r/c systems PWM pulse frequency is around 50hz or 50 pulses per 1000 microseconds
or 1 pulse every 20 microseconds. At 100 microseconds, pulseIn() will wait for what should be 5 pulses before timing out.
The switch() function is used to program all ESC control as a state machine.
There is a great introduction to state machine programming in the following manual (chapter 6):
https://velocio.net/wp-content/uploads/2016/01/vBuilder-Manual.pdf
State machine programming is super useful in all logic programming.
This program controls the ESCs as follows:
If throttle is zero, then move tracks in opposite directions to rotate vehicle using steering input.
If throttle isn't zero, then move tracks in direction of throttle and turn by stopping track in the turning direction.
For this program, the tracked vehicle has 9 states:
0 - stopped
1 - stopped, rotating left
2 - stopped, rotating right
3 - moving forward, straight
4 - moving forward, turning left
5 - moving forward, turning right
6 - moving in reverse, straight
7 - moving in reverse, turning left
8 - moving in reverse, turning right
*/
//Libraries
#include <Servo.h>
//Variables
Servo escLeft, escRight; //add servo objects for each ESC
int steeringPin=A2, throttlePin=A1; //pin inputs from receiver channels with pin assignment (potentiometers in this case)
int steering=1500, throttle=1500; //acutual position of steering and throttle set to 1500 microsec which is neutral throttle and steering
//unsigned long steeringIn=1500, throttleIn=1500; //will be used for reading pulses using pusleIn() from receiver in reality
int escLeftPin=6, escRightPin=5; //pin outputs to ESCs with pin assignments
int steeringPosition; //for defining the overall position of the throttle input (left= -1 / center= 0 / right= 1)
int throttlePosition; //for defining the overall position of the thorttle input (reverse= -1 / stop= 0 / forward = 1)
int state; //defines the state in which the machine is in
//Setup commands (runs once at the beginning)
void setup() {
Serial.begin(9600); //setup serial communications to read values from arduino
escLeft.attach(escLeftPin,1000,2000); //attach ESC to designated pin with min value of 1000 microsec and max value of 2000 microsec
escRight.attach(escRightPin,1000,2000);
}
//Main Program (runs in a loop)
void loop() {
steering = analogRead(steeringPin); // reads the value of the potentiometer (value between 0 and 1023)
throttle = analogRead(throttlePin);
//steering = pulseIn(steeringPin,HIGH,100); // reads the value of pulses from receiver
//throttle = pulseIn(throttlePin,HIgh,100);
steering = map(steering, 0, 1023, 1000, 2000); // scales potentiometer value to ESC range (value between 1000 and 2000)
throttle = map(throttle, 0, 1023, 1000, 2000); // shouldn't be needed when using pulseIn() unless calibration required
// establishing steering position
if (steering <= 1450) {steeringPosition = -1;} // left
if (steering > 1450 && steering < 1550) {steeringPosition = 0;} // center
if (steering >= 1550) {steeringPosition = 1;} // right
// establishing thorttle position
if (throttle <= 1450) {throttlePosition = -1;} // reverse
if (throttle > 1450 && throttle < 1550) {throttlePosition = 0;} // stopped
if (throttle >= 1550) {throttlePosition = 1;} // forward
//debugging
//Serial.println(steeringPosition);
//Serial.println(throttlePosition);
//delay(500);
// Machine state definitions for switch() function
if (throttlePosition == 0 && steeringPosition == 0) {state = 0;} // state 0 - stopped
if (throttlePosition == 0 && steeringPosition == -1) {state = 1;} // state 1 - stopped, rotating left
if (throttlePosition == 0 && steeringPosition == 1) {state = 2;} // state 2 - stopped, rotating right
if (throttlePosition == 1 && steeringPosition == 0) {state = 3;} // state 3 - moving forward, straight
if (throttlePosition == 1 && steeringPosition == -1) {state = 4;} // state 4 - moving forward, turning left
if (throttlePosition == 1 && steeringPosition == 1) {state = 5;} // state 5 - moving forward, turning right
if (throttlePosition == -1 && steeringPosition == 0) {state = 6;} // state 6 - moving in reverse, straight
if (throttlePosition == -1 && steeringPosition == -1) {state = 7;} // state 7 - moving in reverse, turning left
if (throttlePosition == -1 && steeringPosition == 1) {state = 8;} // state 8 - moving in reverse, turning right
// ESC outputs for each state
switch (state) {
case 0:
escLeft.write(1500); // stopped
escRight.write(1500); // stopped
break;
case 1:
escLeft.write(steering); // moving in reverse at rate of steering position
escRight.write(1500+(1500-steering)); // moving forward at rate of steering position (opposite of escLeft)
break;
case 2:
escLeft.write(steering); // moving forward at rate of steering position
escRight.write(1500-(steering-1500)); // moving in reverse at rate of steering position (opposite of escLeft)
break;
case 3:
escLeft.write(throttle); // both go forward at rate of throttle
escRight.write(throttle);
break;
case 4:
escLeft.write(1500); // stopped
escRight.write(throttle); // moving forward at rate of throttle
break;
case 5:
escLeft.write(throttle); // moving forward at rate of throttle
escRight.write(1500); // stopped
break;
case 6:
escLeft.write(throttle); // both go in reverse at rate of throttle
escRight.write(throttle);
break;
case 7:
escLeft.write(1500); // stopped
escRight.write(throttle); // moving in reverse at rate of throttle
break;
case 8:
escLeft.write(throttle); // moving in reverse at rate of throttle
escRight.write(1500); // stopped
break;
default:
escLeft.write(1500); // stopped
escRight.write(1500); // stopped
break;
}
}