If I comment out the line in Loop() that calls:
updateRoboClawDuty(); //send roboclaw serial every 100ms
then the Stepper works and turns fine and smooth accel/deccel. but it's jittery with the roboclaw updates.
Debugging - turning that on makes it really bad, of course, as it's printing all kinds of things (roboclaw has to be disconnected for debugging) But in normal operation I set DEBUG to 0
/*This is version 2.5, made for the RoboClaw Solo30,
Packet Serial @ 115200 BAUD
It also has the AS5600 connected via I2C
DEBUG 1 give serial output, but you must disconnect the Roboclaw
TODO:
1. Sharp Turn Mode - don't start motor until the stepper is done
It doesn't work well until load of thrust.
2. Find the smoothest stepper option for turning in normal & auto modes
*/
#include <EEPROM.h>
#include <math.h>
// Debugging switches and macros
#define DEBUG 0 // Switch debug output on and off by 1 or 0
#if DEBUG
#define PRINTS(s) \
{ Serial.print(F(s)); }
#define PRINT(s, v) \
{ \
Serial.print(F(s)); \
Serial.print(v); \
}
#else
#define PRINTS(s)
#define PRINT(s, v)
#endif
#include "RoboClaw.h"
RoboClaw roboclaw(&Serial, 10000);
#include <Wire.h>
#include "AS5600.h"
#define GEARBOX_RATIO 5.1818
#define SENSOR_TICKS_PER_REV 4096.0
#define TICKS_PER_OUTPUT_REV (SENSOR_TICKS_PER_REV * GEARBOX_RATIO)
#define EEPROM_ADDR 0
AS5600 as5600;
long zeroCumulative = 0; // Cumulative position at zero
#include <Arduino.h>
#include <AccelStepper.h>
// Define Stepper pin connections
const int dirPin = 9; //Dir Pin
const int stepPin = 8; //Step Pin
const int stepperEN = A3; //Enable Pin
const int sleepPin = 7; //sleep and reset pin both
const int MS1pin = 4;
const int MS2pin = 5;
const int MS3pin = 6;
#define motorInterfaceType 1 // Define motor interface type
// Creates an instance
AccelStepper myStepper(motorInterfaceType, stepPin, dirPin);
const int LEDpin = 10; //LED on power switch
const int bttnPOWERPin = 12; //Power Switch
const int bttnUPpin = 11; //SW1 (momentary on rocker switch)
const int bttnDOWNpin = 13; //SW2 (momentary on rocker switch)
const int Xpin = A0; //analog joystick pin Thrust
const int Ypin = A1; //analog joystick pin Rotation
const int BatPin = A2; //monitor battery pin
const int microsteps = 4; // confirmed by observation
const float stepsPerRevolution = 200.0 * microsteps * 5.1818;
const float stepsPerDegree = stepsPerRevolution / 360.0;
bool systemOn = false; //set to true when power button is turned on
bool as5600_OK = false; //set to true when the AS5600 is working
int duty = 0; // Duty value for RoboClaw +/-32767
bool trollingMotorSynced = false;
int trollingMotorAngle = 0; //current angle of trolling motor
float batteryAhUsed = 0.0;
unsigned long lastCurrentSampleTime = 0;
//int maxSteps = 200; //259 * 4 is 90 degrees
unsigned long t; //counter for serial print
int Xdebounce = 100; //debounce value for turning
int Ydebounce = 20; //debounce value for thrust
int Y_forwardTurnThreshold = 100; //min forward val for “moving forward” steering
bool isSteering = false; //true when steering in AutoPilot mode
int Xreading; //reading of X axis
int Yreading; //reading of Y axis
int minThrust = 0; //limit the min thrust value to this
int maxThrust = 32767; //limit the max thrust value to this
int X_cen = 512; //center reading of X - axis on joystick
int X_right = 233; //joystick pushed RIGHT gives lower value
int X_left = 792; //joystick pushed LEFT gives higher value
int Y_cen = 510; //center reading of Y - axis on joystick
int Y_up = 791; //joystick pushed UP gives higher value
int Y_down = 225; //joystick pushed DOWN gives lower value
enum LedState {
LED_BLINK,
LED_TRIPLE_BLINK,
LED_RAPID_BLINK,
LED_ON,
LED_OFF
};
LedState LEDstate = LED_OFF;
enum DriveMode {
Normal,
Turning,
AutoPilot,
AngleChange
};
DriveMode driveMode = Normal; //start out in normal mode
#include <Bounce2.h>
Bounce2::Button bttnPOWER = Bounce2::Button();
Bounce2::Button bttnUP = Bounce2::Button();
Bounce2::Button bttnDOWN = Bounce2::Button();
void setup() {
Serial.begin(115200);
roboclaw.begin(115200);
delay(1000); // Give RoboClaw a moment to start
pinMode(LEDpin, OUTPUT);
pinMode(stepperEN, OUTPUT);
digitalWrite(stepperEN, HIGH); // DISABLE stepper driver (idle)
pinMode(sleepPin, OUTPUT);
digitalWrite(sleepPin, HIGH); // Enable driver by default
pinMode(MS1pin, OUTPUT);
pinMode(MS2pin, OUTPUT);
pinMode(MS3pin, OUTPUT);
// Set for 1/4 microstepping (A4988/DRV8825 standard)
digitalWrite(MS1pin, LOW);
digitalWrite(MS2pin, HIGH);
digitalWrite(MS3pin, LOW);
myStepper.setMaxSpeed(2000);
myStepper.setAcceleration(3000);
myStepper.setSpeed(3000);
PRINTS("\nFloat Tube Controller Version 2");
bttnPOWER.attach(bttnPOWERPin, INPUT_PULLUP); // Attach with INPUT_PULLUP mode
bttnPOWER.interval(200); //debounce interval
bttnPOWER.setPressedState(LOW);
bttnUP.attach(bttnUPpin, INPUT_PULLUP); // Attach with INPUT_PULLUP mode
bttnUP.interval(200); //debounce interval
bttnUP.setPressedState(LOW);
bttnDOWN.attach(bttnDOWNpin, INPUT_PULLUP); // Attach with INPUT_PULLUP mode
bttnDOWN.interval(200); //debounce interval
bttnDOWN.setPressedState(LOW);
Wire.begin();
init_AS5600(); //initiate the AS5600
PRINT("\nAS5600 connected: ", as5600.isConnected());
}
void loop() {
checkButtons();
updateLED();
updateJoystick();
updateRoboClawDuty(); //send roboclaw serial every 100ms
as5600.getCumulativePosition(); //keep the AS5600 updated
if (systemOn) {
myStepper.run(); //update constantly
if (driveMode != AngleChange) {
syncStepperToAS5600(); //update as needed
}
if (duty != 0 || myStepper.distanceToGo() != 0 || isSteering) {
digitalWrite(stepperEN, LOW); // ENABLE stepper driver
} else {
digitalWrite(stepperEN, HIGH); // DISABLE stepper driver (idle)
}
switch (driveMode) {
case Normal:
normalModeThrust();
handleFineSteeringNormalThrust();
break;
case Turning:
handleSharpTurn();
break;
case AutoPilot:
autoModeThrust();
handleFineSteeringInAutoPilot();
break;
case AngleChange:
handleAngleChange(); //this mode is used to re zero the angle
break;
}
} else { //system is off
digitalWrite(stepperEN, HIGH); //disable
duty = 0; //set motor speed
}
}
void checkButtons() {
bttnPOWER.update();
bttnUP.update();
bttnDOWN.update();
// --- POWER button logic: handle ON/OFF ---
if (bttnPOWER.pressed()) { // just turned on
LEDstate = LED_ON;
PRINTS("\npower turned on");
systemOn = true;
duty = 0;
driveMode = Normal;
digitalWrite(stepperEN, HIGH); // DISABLE stepper driver (idle)
digitalWrite(LEDpin, HIGH);
#if DEBUG
init_AS5600(); //so we don't have to keep unplugging it
#endif
}
if (bttnPOWER.released()) { // just turned off
LEDstate = LED_OFF;
PRINTS("\npower turned off");
systemOn = false;
driveMode = Normal;
duty = 0;
digitalWrite(stepperEN, HIGH); // DISABLE stepper driver (idle)
}
// --- UP button toggles AutoPilot ---
if (bttnUP.pressed() && systemOn) {
if (driveMode == AutoPilot) {
LEDstate = LED_ON;
PRINTS("\ndrive mode: Normal");
driveMode = Normal;
} else {
LEDstate = LED_BLINK;
PRINTS("\ndrive mode: AutoPilot");
driveMode = AutoPilot;
}
duty = 0;
}
// --- DOWN button single press: AngleChange mode ---
if (bttnDOWN.pressed() && systemOn) {
LEDstate = LED_RAPID_BLINK;
driveMode = AngleChange;
PRINTS("\nDOWN pressed");
}
}
void normalModeThrust() {
LEDstate = LED_ON; // On for normal mode
int offset = abs(Yreading - Y_cen);
if (offset < Ydebounce) {
// Deadband: joystick is near the center
duty = 0; //set motor speed
} else if (Yreading > Y_cen) {
// Forward thrust (up direction)
if (Yreading > Y_up) Yreading = Y_up; //otherwise an int can't handle the larger size
duty = map(Yreading, Y_cen + Ydebounce, Y_up, minThrust, maxThrust);
duty = constrain(duty, minThrust, maxThrust);
} else {
// Reverse thrust
if (Yreading < Y_down) Yreading = Y_down; //stay inbounds for an int
duty = map(Yreading, Y_cen - Ydebounce, Y_down, minThrust, maxThrust);
duty = constrain(duty, minThrust, maxThrust);
duty = -duty; //reverse
}
static int prevDuty;
if (prevDuty != duty) {
//PRINT("\nSpeed: ", duty);
}
prevDuty = duty;
// Sharp turn entry logic is unchanged
int xOffset = Xreading - X_cen;
bool stickSideways = abs(xOffset) > ((X_left - X_cen) * 4 / 10); //about half way
bool stickCenteredY = abs(Yreading - Y_cen) < Y_forwardTurnThreshold; // not forward/back
if (duty == 0 && stickCenteredY && stickSideways) {
PRINTS("entering turn mode");
driveMode = Turning; // Enter sharp turn mode
}
}
void autoModeThrust() {
static unsigned long prevThrustMillis = 0;
unsigned long now = millis();
const unsigned long accelInterval = 10; // ms for all modes
const int fullStep = 103;
const int partialStep = 62;
int accel = 0;
int accelStep = 1;
if (Yreading > Y_up - 20) {
accel = 1;
accelStep = fullStep;
} else if (Yreading > Y_cen + 50) {
accel = 1;
accelStep = partialStep;
} else if (Yreading < Y_down + 20) {
duty = 0;
accel = 0;
} else if (Yreading < Y_cen - 50) {
accel = -1;
accelStep = partialStep; // adjust as desired for decel
} else {
accel = 0;
}
if (accel != 0 && (now - prevThrustMillis > accelInterval)) {
prevThrustMillis = now;
duty += accel * accelStep;
if (accel > 0 && duty < 2000) duty = 2000;
}
duty = constrain(duty, 0, maxThrust);
}
void handleFineSteeringNormalThrust() {
// Only allow small left/right steering when moving forward strongly
if ((Yreading - Y_cen) > Y_forwardTurnThreshold && duty > 0) {
int xOffset = Xreading - X_cen;
if (abs(xOffset) > Xdebounce) {
int turnAngle = map(xOffset, X_right - X_cen, X_left - X_cen, -60, 60); //
myStepper.moveTo(turnAngle * stepsPerDegree);
} else {
myStepper.moveTo(0); // Go straight if joystick is centered
}
}
}
void handleFineSteeringInAutoPilot() {
int xOffset = Xreading - X_cen;
static int state = 0; // -1 = right, 0 = center, 1 = left
int halfwayThreshold = (X_left - X_cen) / 2; // halfway left/right from center
if (xOffset > halfwayThreshold) {
// More than halfway to the left
if (state != 1) {
myStepper.moveTo(30 * stepsPerDegree);
isSteering = true;
state = 1;
}
} else if (xOffset < -halfwayThreshold) {
// More than halfway to the right
if (state != -1) {
myStepper.moveTo(-30 * stepsPerDegree);
isSteering = true;
state = -1;
}
} else if (abs(xOffset) < Xdebounce) {
// Centered (with hysteresis)
if (state != 0) {
myStepper.moveTo(0);
isSteering = false;
state = 0;
}
}
// Otherwise, do nothing (hysteresis region)
}
void handleSharpTurn() {
int halfwayleft = X_cen + ((X_left - X_cen) * 4) / 10; //Left is a higher value
int halfwayright = X_cen + ((X_right - X_cen) * 4) / 10; //Right is a lower value
//int halfwayleft = (X_cen + X_left) / 2; //Left is a higher value
//int halfwayright = (X_cen + X_right) / 2; //Right is a lower value
int sharpTurnMaxSpeed = 32767;
int sharpTurnMinSpeed = 2000;
if (Xreading > halfwayleft) {
// Left sharp turn
myStepper.moveTo(65 * stepsPerDegree);
// Map Xreading from halfway left to max left for thrust
if (Xreading > X_left) Xreading = X_left; //keep in bounds
duty = map(Xreading, halfwayleft, X_left, sharpTurnMinSpeed, sharpTurnMaxSpeed);
duty = constrain(duty, sharpTurnMinSpeed, sharpTurnMaxSpeed);
} else if (Xreading < halfwayright) {
// Right sharp turn
myStepper.moveTo(-65 * stepsPerDegree);
// Map Xreading from halfway right to max right for thrust
if (Xreading < X_right) Xreading = X_right; //keep in bounds
duty = map(Xreading, halfwayright, X_right, sharpTurnMinSpeed, sharpTurnMaxSpeed);
duty = constrain(duty, sharpTurnMinSpeed, sharpTurnMaxSpeed);
} else {
myStepper.moveTo(0); //Joystick back near center
duty = 0;
}
// Exit Turning mode only when stick is centered AND turn is complete
if (Xreading < halfwayleft && Xreading > halfwayright && myStepper.distanceToGo() == 0) {
driveMode = Normal;
}
}
void handleAngleChange() { //this mode is used to re zero the angle
// Forward/backward controls thrust just like autoModeThrust
autoModeThrust();
// Joystick X controls stepper speed and direction
int xOffset = Xreading - X_cen;
int jogDeadband = Xdebounce;
float maxJogSpeed = 300.0;
float speed = 0.0;
if (abs(xOffset) > jogDeadband) {
speed = map(xOffset, X_right - X_cen, X_left - X_cen, -maxJogSpeed, maxJogSpeed);
} else {
speed = 0.0;
}
myStepper.setSpeed(speed);
myStepper.runSpeed();
// --- Only allow a DOWN press *after* a release since entering this mode ---
static bool downReady = false;
if (bttnDOWN.released()) {
downReady = true;
}
if (downReady && bttnDOWN.pressed()) {
setAS5600Zero();
#if DEBUG
PRINT("\nAS5600 zero set. New zero value: ", zeroCumulative);
#endif
driveMode = Normal;
downReady = false; // block until next release
}
}
void updateRoboClawDuty() {
static unsigned long lastUpdate = 0;
unsigned long now = millis();
#if DEBUG
if (now - lastUpdate >= 1000) { // 1 second interval in debug
lastUpdate = now;
//PRINT("\nDEBUG Duty: ", duty);
}
#else
if (now - lastUpdate >= 100) { // 100 ms interval normally
lastUpdate = now;
roboclaw.DutyM1(0x80, duty);
}
#endif
}
void updateJoystick() {
Xreading = analogRead(Xpin); //analog reading
delayMicroseconds(10);
Xreading = analogRead(Xpin); //2nd analog reading is better
if (Xreading < 100 || Xreading > 900) error(); //connector failure??
Yreading = analogRead(Ypin); //analog reading
delayMicroseconds(10);
Yreading = analogRead(Ypin); //2nd analog reading is better
if (Yreading < 100 || Yreading > 900) error(); //connector failure??
//PRINT("\nY:", Yreading);
//PRINT(" X:", Xreading);
}
void init_AS5600() {
as5600.begin();
as5600_OK = as5600.isConnected();
// 1. Read saved zero from EEPROM
long savedZeroCumulative = 0;
uint16_t savedZeroRawAngle = 0;
EEPROM.get(EEPROM_ADDR, savedZeroCumulative);
EEPROM.get(EEPROM_ADDR + 10, savedZeroRawAngle);
// 2. Read current AS5600 raw angle
uint16_t currentRaw = as5600.rawAngle();
// 3. Calculate shortest signed difference in ticks (wraparound safe)
int16_t tickDiff = ((int16_t)savedZeroRawAngle - (int16_t)currentRaw + 6144) % 4096 - 2048;
float stepsPerTick = stepsPerRevolution / TICKS_PER_OUTPUT_REV;
long stepOffset = lround(tickDiff * stepsPerTick);
// 4. Enable stepper driver so it can move
digitalWrite(stepperEN, LOW);
// 5. Set current stepper position (so our move command below works correctly)
myStepper.setCurrentPosition(0); // Assume we're at step 0 now
// 6. Move stepper so that the sensor should reach the EEPROM-saved angle
long targetStep = stepOffset;
myStepper.moveTo(targetStep);
while (myStepper.distanceToGo() != 0) {
myStepper.run();
}
// 7. Set stepper software zero at the end
myStepper.setCurrentPosition(0);
// 8. Save the current position as new zero (for cumulative use)
setAS5600Zero();
}
void syncStepperToAS5600() {
if (!as5600_OK) return;
if (duty == 0) {
trollingMotorSynced = false; // Reset sync flag
return;
}
if (!trollingMotorSynced) {
long currentCumulative = as5600.getCumulativePosition();
long difference = currentCumulative - zeroCumulative;
float stepsPerTick = stepsPerRevolution / TICKS_PER_OUTPUT_REV;
float stepsToZero = lround(difference * stepsPerTick);
PRINTS("\nSYNC DEBUG ----");
PRINT("\nSETPOINT: ", zeroCumulative);
PRINT("\nCURRENT: ", currentCumulative);
PRINT("\ndifference: ", difference);
PRINT("\nstepsPerTick: ", stepsPerTick);
PRINT("\nstepsToZero: ", stepsToZero);
PRINT("\nStepper before set: ", myStepper.currentPosition());
myStepper.setCurrentPosition(stepsToZero);
myStepper.moveTo(0);
PRINT("\nStepper after set: ", myStepper.currentPosition());
PRINT("\nStepper target: ", myStepper.targetPosition());
digitalWrite(stepperEN, LOW);
trollingMotorSynced = true;
}
}
int getOutputAngle() {
long cumulativePos = as5600.getCumulativePosition();
float outputAngle = ((cumulativePos - zeroCumulative) / TICKS_PER_OUTPUT_REV) * 360.0;
// Wrap to -180..+180
if (outputAngle > 180.0) outputAngle -= 360.0;
if (outputAngle < -180.0) outputAngle += 360.0;
int angleInt = (int)round(outputAngle);
return angleInt;
}
void setAS5600Zero() {
if (!as5600_OK) return; // if sensor is not available, exit
zeroCumulative = as5600.getCumulativePosition();
uint16_t zeroRawAngle = as5600.rawAngle();
EEPROM.put(EEPROM_ADDR, zeroCumulative);
EEPROM.put(EEPROM_ADDR + 10, zeroRawAngle);
PRINTS("\nAS5600 zero set.");
PRINT(" Zero cumulative: ", zeroCumulative);
PRINT(" | Zero raw angle: ", zeroRawAngle);
}
void error() { // the reading was way out of range (connector failure??)
duty = 0;
digitalWrite(stepperEN, HIGH); // DISABLE stepper driver (idle)
PRINTS("Error on joystick reading. Out of range...");
while (1) {
updateRoboClawDuty();
int i0 = analogRead(A0);
int i1 = analogRead(A1);
PRINT("\nReading on A0: ", i0);
PRINT(" Reading on A1: ", i1);
// If both readings are valid, exit the error loop
if (i0 >= 100 && i0 <= 900 && i1 >= 100 && i1 <= 900) {
PRINTS("\nJoystick readings OK. Exiting error state.\n");
break;
}
delay(200);
}
}
void updateLED() {
static unsigned long lastToggle = 0;
static bool ledOn = false;
const unsigned long onTime = 200;
const unsigned long offTime = 5000;
switch (LEDstate) {
case LED_ON:
digitalWrite(LEDpin, HIGH);
ledOn = true;
break;
case LED_OFF:
digitalWrite(LEDpin, LOW);
ledOn = false;
break;
case LED_BLINK:
{
unsigned long now = millis();
unsigned long interval = ledOn ? onTime : offTime;
if (now - lastToggle >= interval) {
ledOn = !ledOn;
digitalWrite(LEDpin, ledOn ? HIGH : LOW);
lastToggle = now;
}
break;
}
case LED_RAPID_BLINK:
{
unsigned long now = millis();
unsigned long interval = 200; // Rapid: 200ms ON/OFF
if (now - lastToggle >= interval) {
ledOn = !ledOn;
digitalWrite(LEDpin, ledOn ? HIGH : LOW);
lastToggle = now;
}
break;
}
// no default
}
}
void monitorAhUsage() {
static unsigned long lastPoll = 0;
static unsigned long lastCurrentSampleTime = 0;
const unsigned long pollInterval = 100; // ms (5 Hz)
unsigned long now = millis();
if (duty == 0) {
lastCurrentSampleTime = now;
return;
}
if (now - lastPoll < pollInterval) return;
lastPoll = now;
int16_t m1Current, m2Current;
bool status = roboclaw.ReadCurrents(0x80, m1Current, m2Current);
if (status) {
float amps = m1Current * 0.01;
if (lastCurrentSampleTime != 0) {
float hours = (now - lastCurrentSampleTime) / 3600000.0;
batteryAhUsed += amps * hours;
}
lastCurrentSampleTime = now;
}
}
float readVoltage() {
float R1 = 29980.0; // 30K resistor
float R2 = 7500.0; // 7.5K resistor
float VREF = 5.0; //system voltage
int reading = analogRead(A5);
delayMicroseconds(10);
reading = analogRead(A5);
float vout = (reading * VREF) / 1023.0;
float vin = vout / (R2 / (R1 + R2));
return vin;
}