GGS
/
FireWatchTower_2axis
Archived
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

code re-structuring

This commit is contained in:
pgdalmeida 2026-05-12 13:05:43 +02:00
parent 85bd9810b8
commit e7301db427
Signed by: pedro.almeida
GPG Key ID: D4A6C394DF13F1D7
13 changed files with 818 additions and 528 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
deploy.sh Normal file → Executable file
View File

@ -1,8 +1,8 @@
#!/bin/bash #!/bin/bash
rsync -av --exclude='build/' ~/code/FireWatchTower_2axis/ ggs@10.11.12.111:/home/ggs/projects/FireWatchTower_2axis rsync -av --exclude={'build/','.kilo/','.vscode/','.cache'} ~/code/FireWatchTower_2axis/ ggs@10.11.12.111:/home/ggs/projects/FireWatchTower_2axis
#ssh ggs@10.11.12.111 " #ssh ggs@10.11.12.111 "
# mkdir -p /home/ggs/projects/FireWatchTower_2axis/build && # mkdir -p /home/ggs/projects/FireWatchTower_2axis/build &&
# cd /home/ggs/projects/FireWatchTower_2axis/build && # cd /home/ggs/projects/FireWatchTower_2axis/build &&
# cmake .. && # cmake .. &&
# make -j\$(nproc) # make -j\$(nproc)
"

8
firmware/deploy_firmware.sh Executable file
View File

@ -0,0 +1,8 @@
#!/bin/bash
rsync -av --exclude={'build/','.kilo/','.vscode/','.cache'} ~/code/FireWatchTower_2axis/firmware/ ggs@10.11.12.111:/home/ggs/projects/FireWatchTower_2axis/firmware/
#ssh ggs@10.11.12.111 "
# mkdir -p /home/ggs/projects/FireWatchTower_2axis/build &&
# cd /home/ggs/projects/FireWatchTower_2axis/build &&
# cmake .. &&
# make -j\$(nproc)

106
firmware/src/config/Constants.h
View File

@ -1,50 +1,80 @@
#ifndef FTW_CONSTANTS_H #ifndef FTW_CONSTANTS_H
#define FTW_CONSTANTS_H #define FTW_CONSTANTS_H
/* Serial */ /* -------------------------------------------------------------------------
#define BAUD_RATE 115200 Serial
------------------------------------------------------------------------- */
#define BAUD_RATE 115200
/* SPI */ /* -------------------------------------------------------------------------
#define SPI_CLOCK_DIV SPI_CLOCK_DIV4 SPI
The TMC5160 supports up to ~4 MHz. DIV4 on a 16 MHz Arduino = 4 MHz.
------------------------------------------------------------------------- */
#define SPI_CLOCK_DIV SPI_CLOCK_DIV4
/* Motor driver */ /* -------------------------------------------------------------------------
#define ACCELERATION 20000 Motor driver — shared by both axes
#define ENC_DEVIATION_LIMIT 1500 ------------------------------------------------------------------------- */
#define ENC_CONST 0x000C1F40 #define ACCELERATION 20000 // counts/s² — used for A1, AMAX, DMAX, D1 (flat ramp profile)
#define ENCMODE_DECIMAL 0x400 #define ENC_DEVIATION_LIMIT 1500 // max allowed |XACTUAL - XENC| before DEVIATION_WARN fires
#define ENC_CONST 0x000C1F40 // encoder resolution constant (counts per electrical cycle)
#define ENCMODE_DECIMAL 0x400 // encoder counting mode: decimal (as opposed to binary)
/* Yaw axis */ /* -------------------------------------------------------------------------
#define YAW_STEPS_PER_ROUND 177000 Yaw axis
#define YAW_GEAR_RATIO 739.5555f ------------------------------------------------------------------------- */
#define YAW_GLOBAL_SCALER 80 #define YAW_STEPS_PER_ROUND 177000 // encoder counts per full mechanical revolution
#define YAW_DEFAULT_XACTUAL 0x0007A120 #define YAW_GEAR_RATIO 739.5555f
#define YAW_ENDSTOP_SPEED 25000 #define YAW_GLOBAL_SCALER 80 // scales peak coil current (0–255); lower = less current
#define YAW_DEFAULT_VMAX 50000 #define YAW_DEFAULT_XACTUAL 0x0007A120 // initial value written to XACTUAL on reset
#define YAW_ENDSTOP_SPEED 25000 // velocity used when driving toward a limit during homing
#define YAW_DEFAULT_VMAX 50000 // normal operating speed
/* Pitch axis */ /* -------------------------------------------------------------------------
#define PITCH_STEPS_PER_ROUND 500000 Pitch axis
#define PITCH_GEAR_RATIO 739.5555f ------------------------------------------------------------------------- */
#define PITCH_GLOBAL_SCALER 50 #define PITCH_STEPS_PER_ROUND 500000
#define PITCH_DEFAULT_XACTUAL 500000 #define PITCH_GEAR_RATIO 739.5555f
#define PITCH_ENDSTOP_SPEED 150000 #define PITCH_GLOBAL_SCALER 50 // pitch motor runs at lower current than yaw
#define PITCH_DEFAULT_VMAX 250000 #define PITCH_DEFAULT_XACTUAL 500000
#define PITCH_ENDSTOP_SPEED 150000 // pitch can home faster because it uses physical switches
#define PITCH_DEFAULT_VMAX 250000
/* Homing */ /* -------------------------------------------------------------------------
#define HOMING_MIN_ROUND_DIFF_OFFSET 6000 Homing
#define HOMING_XENC_MARGIN 1000 ------------------------------------------------------------------------- */
#define HOMING_DELAY_MS 10 // After homing, this many encoder counts are kept as a margin inside each limit
#define HOMING_PITCH_INITIAL_DELAY 1000 // so normal operation never commands the axis right to the mechanical stop.
#define HOMING_YAW_TIMEOUT_MS 2000 #define HOMING_XENC_MARGIN 1000
/* Thermal */ // If the measured travel between endstops is less than
#define THERMAL_FAN_ON_TEMP 25 // (steps_per_round - HOMING_MIN_ROUND_DIFF_OFFSET), homing is declared failed.
#define THERMAL_FAN_GAIN 20 // The offset accounts for the margin on each side plus mechanical tolerance.
#define THERMAL_FAN_MAX_PWM 255 #define HOMING_MIN_ROUND_DIFF_OFFSET 6000
#define THERMAL_FAN_PULSE_US 100000
#define THERMAL_FAN_FREQ_DIV 0.004
#define THERMAL_LOOP_INTERVAL 250
/* Info print interval */ #define HOMING_DELAY_MS 10 // settle time after an endstop fires before reading XLATCH
#define INFO_PRINT_INTERVAL_MS 250 #define HOMING_PITCH_INITIAL_DELAY 1000 // time to back off a switch before starting the pitch sequence
#define HOMING_YAW_TIMEOUT_MS 2000 // pause between yaw right-pass and left-pass (deceleration time)
#define HOMING_SGT_THRESHOLD 60 // stallGuard threshold during homing (high = only triggers on hard stop)
#define HOMING_YAW_SPEED 30000 // speed used during the yaw homing passes
#define HOMING_PITCH_SPEED 30000 // speed used during the pitch homing passes
#define HOMING_YAW_CENTER_SPEED 35000 // speed for the post-homing move to centre (yaw)
#define HOMING_PITCH_CENTER_SPEED 150000 // speed for the post-homing move to centre (pitch)
/* -------------------------------------------------------------------------
Thermal
------------------------------------------------------------------------- */
#define THERMAL_FAN_ON_TEMP 25 // °C threshold below which the fan stays off
#define THERMAL_FAN_GAIN 20 // PWM counts per °C above threshold
#define THERMAL_FAN_MAX_PWM 255 // PWM ceiling (full speed)
#define THERMAL_FAN_PULSE_US 100000 // timeout for pulseIn() on the fan tachometer signal
#define THERMAL_FAN_FREQ_DIV 0.004f // unused — reserved for frequency calculation
#define THERMAL_LOOP_INTERVAL 250 // ms between thermal samples (DHT11 max ~1 Hz)
/* -------------------------------------------------------------------------
Main loop
------------------------------------------------------------------------- */
#define INFO_PRINT_INTERVAL_MS 250 // ms between Serial status prints
#endif #endif

8
firmware/src/hal/SPI_HAL.cpp
View File

@ -8,9 +8,9 @@
void tmc5160_readWriteSPI(uint16_t icID, uint8_t *data, size_t dataLength) { void tmc5160_readWriteSPI(uint16_t icID, uint8_t *data, size_t dataLength) {
uint8_t input[dataLength]; uint8_t input[dataLength];
if (icID == MotorYaw) if (icID == AXIS_YAW)
digitalWrite(PIN_TMC_CS_YAW, LOW); digitalWrite(PIN_TMC_CS_YAW, LOW);
if (icID == MotorPitch) if (icID == AXIS_PITCH)
digitalWrite(PIN_TMC_CS_PITCH, LOW); digitalWrite(PIN_TMC_CS_PITCH, LOW);
delayMicroseconds(1); delayMicroseconds(1);
@ -20,9 +20,9 @@ void tmc5160_readWriteSPI(uint16_t icID, uint8_t *data, size_t dataLength) {
} }
delayMicroseconds(1); delayMicroseconds(1);
if (icID == MotorYaw) if (icID == AXIS_YAW)
digitalWrite(PIN_TMC_CS_YAW, HIGH); digitalWrite(PIN_TMC_CS_YAW, HIGH);
if (icID == MotorPitch) if (icID == AXIS_PITCH)
digitalWrite(PIN_TMC_CS_PITCH, HIGH); digitalWrite(PIN_TMC_CS_PITCH, HIGH);
} }

37
firmware/src/main.cpp
View File

@ -5,33 +5,42 @@
#include "serial/CommandParser.h" #include "serial/CommandParser.h"
#include "thermal/ThermalManager.h" #include "thermal/ThermalManager.h"
static unsigned long s_last_loop = 0; static unsigned long s_last_print = 0;
void setup() { void setup() {
Serial.begin(BAUD_RATE); Serial.begin(BAUD_RATE);
delay(50); delay(50); // brief settle before sending — avoids garbled first bytes on some boards
Serial.println(F("Starting...")); Serial.println(F("Starting..."));
init_motor_hardware(); motor_hw_init(); // configure SPI peripheral and CS/EN pins
init_motor_params(); motor_regs_init(); // write TMC5160 register configuration for both axes
thermal_init(); thermal_init(); // start the DHT sensor
s_last_loop = millis(); s_last_print = millis();
} }
void loop() { void loop() {
motor_periodic_job(MotorYaw); // Refresh cached position registers every iteration so all other code sees
motor_periodic_job(MotorPitch); // fresh values without triggering redundant SPI reads themselves.
endstop_routine(); motor_update(AXIS_YAW);
motor_update(AXIS_PITCH);
// Run the homing state machines. These are non-blocking: they check for
// events (endstop triggers, deviation flags) and only act when something
// has changed, so calling them every loop is cheap when idle.
homing_routine();
if (Serial.available()) { if (Serial.available()) {
read_command() read_command();
} }
if (millis() - s_last_loop > INFO_PRINT_INTERVAL_MS) { // Print status and run the thermal fan controller on a slow timer.
s_last_loop = millis(); // Thermal is piggy-backed here rather than its own timer because the DHT11
print_motor_info(MotorPitch); // samples at ~1 Hz, so there is no benefit running it faster than the print interval.
print_motor_info(MotorYaw); if (millis() - s_last_print > INFO_PRINT_INTERVAL_MS) {
s_last_print = millis();
// motor_print_info(AXIS_PITCH);
// motor_print_info(AXIS_YAW);
thermal_loop(); thermal_loop();
} }
} }

265
firmware/src/motor/Homing.cpp
View File

@ -5,129 +5,190 @@
#include "TMC5160.h" #include "TMC5160.h"
static unsigned long s_tick_move_yaw = 0; // Timestamp used to insert a pause before yaw starts moving left.
// The pause lets the motor fully decelerate after hitting the right limit
// before reversing direction.
static unsigned long s_yaw_homing_tick = 0;
void do_endstop(uint16_t axis) { // ---------------------------------------------------------------------------
if (axis == MotorYaw) { // do_homing_start()
ax_yaw.set_xenc_to_xact(); //
ax_yaw.reset_enc_dev_warn(); // Prepares an axis for homing and starts it moving toward the right endstop.
ax_yaw.driver_status = GIMBAL_STATUS_ENC_INIT; //
ax_yaw.set_speed(30000); // Yaw: relies on encoder deviation detection (stall against a mechanical stop).
ax_yaw.set_SGT(60); // No physical switches — the deviation warning fires when the motor stalls.
ax_yaw.move_endstop_right = true; //
tmc5160_rotateMotor(MotorYaw, YAW_ENDSTOP_SPEED); // Pitch: relies on hardware endstop switches. Gets an extra safety step: if the
// axis is already resting on a switch at startup, it reverses clear before
// the sequence begins so the first limit captured is clean.
// ---------------------------------------------------------------------------
void do_homing_start(uint16_t axis) {
if (axis == AXIS_YAW) {
ax_yaw.sync_encoder_to_actual(); // align encoder to step counter before moving
ax_yaw.clear_enc_deviation(); // clear any stale deviation flag
ax_yaw.status = GIMBAL_STATUS_ENC_INIT;
ax_yaw.seek_right = true;
ax_yaw.set_speed(HOMING_YAW_SPEED);
ax_yaw.set_stallguard_threshold(HOMING_SGT_THRESHOLD); // high threshold = only triggers on hard stop
tmc5160_rotateMotor(AXIS_YAW, YAW_ENDSTOP_SPEED);
} }
else if (axis == MotorPitch) { else if (axis == AXIS_PITCH) {
if (ax_pitch.get_status_endswitch_l()) { // Back off if already on an endstop, then let the motor settle before
tmc5160_rotateMotor(MotorPitch, PITCH_ENDSTOP_SPEED); // syncing the encoder — otherwise the captured position would be wrong.
if (ax_pitch.endswitch_l()) {
tmc5160_rotateMotor(AXIS_PITCH, PITCH_ENDSTOP_SPEED);
delay(HOMING_PITCH_INITIAL_DELAY); delay(HOMING_PITCH_INITIAL_DELAY);
} }
else if (ax_pitch.get_status_endswitch_r()) { else if (ax_pitch.endswitch_r()) {
tmc5160_rotateMotor(MotorPitch, -PITCH_ENDSTOP_SPEED); tmc5160_rotateMotor(AXIS_PITCH, -PITCH_ENDSTOP_SPEED);
delay(HOMING_PITCH_INITIAL_DELAY); delay(HOMING_PITCH_INITIAL_DELAY);
} }
ax_pitch.set_xenc_to_xact(); ax_pitch.sync_encoder_to_actual();
ax_pitch.reset_enc_dev_warn(); ax_pitch.clear_enc_deviation();
ax_pitch.driver_status = GIMBAL_STATUS_ENC_INIT; ax_pitch.status = GIMBAL_STATUS_ENC_INIT;
ax_pitch.set_speed(30000); ax_pitch.seek_right = true;
ax_pitch.set_SGT(60); ax_pitch.set_speed(HOMING_PITCH_SPEED);
ax_pitch.move_endstop_right = true; ax_pitch.set_stallguard_threshold(HOMING_SGT_THRESHOLD);
tmc5160_rotateMotor(MotorPitch, PITCH_ENDSTOP_SPEED); tmc5160_rotateMotor(AXIS_PITCH, PITCH_ENDSTOP_SPEED);
} }
} }
static void yaw_homing(void) { // ---------------------------------------------------------------------------
if (ax_yaw.driver_status == GIMBAL_STATUS_ENC_INIT // homing_complete() (internal)
&& millis() - s_tick_move_yaw > HOMING_YAW_TIMEOUT_MS //
&& !ax_yaw.move_endstop_right) { // Called once both limits have been found. Validates the measured travel range,
s_tick_move_yaw = millis(); // updates axis state, and commands a move to the centre of travel.
tmc5160_rotateMotor(MotorYaw, -YAW_ENDSTOP_SPEED); //
// The travel check catches cases where the motor didn't reach a real limit —
// e.g. it stalled mid-range due to mechanical obstruction or too-low current.
// ---------------------------------------------------------------------------
static void homing_complete(MotorAxis &ax, int32_t center_speed) {
int32_t travel = abs(ax.enc_limit_l - ax.enc_limit_r);
if (travel < (ax.steps_per_round - HOMING_MIN_ROUND_DIFF_OFFSET)) {
// Travel is shorter than expected — the limits are probably wrong.
Serial.print(F("Homing failed — travel too short: "));
Serial.println(travel);
ax.status = GIMBAL_STATUS_ERROR;
return;
} }
if (ax_yaw.driver_status == GIMBAL_STATUS_ENC_INIT && ax_yaw.move_endstop_right) { ax.status = GIMBAL_STATUS_INITIALIZED;
if (ax_yaw.get_dev_warn_status()) { ax.act_steps_per_round = ax.enc_limit_r - ax.enc_limit_l;
tmc5160_rotateMotor(MotorYaw, 0);
Serial.print(F("Homing OK — L/R/Diff: "));
Serial.print(ax.enc_limit_l); Serial.print(F(" / "));
Serial.print(ax.enc_limit_r); Serial.print(F(" / "));
Serial.println(travel);
// Move to centre so the axis starts operation in a known, safe position.
int32_t center = (ax.enc_limit_l + ax.enc_limit_r) / 2;
tmc5160_moveTo(ax.id, center, center_speed);
}
// ---------------------------------------------------------------------------
// yaw_homing_loop() (internal)
//
// Yaw homing state machine — call every loop while status == ENC_INIT.
//
// Phase 1 (seek_right = true):
// Motor moves right. When the stall event fires (encoder deviation triggered),
// record enc_limit_r, stop, clear the flag, then start the left pass.
//
// Phase 2 (seek_right = false):
// Wait HOMING_YAW_TIMEOUT_MS before starting left (lets motor decelerate),
// then move left. When deviation fires again, record enc_limit_l and call
// homing_complete().
// ---------------------------------------------------------------------------
static void yaw_homing_loop(void) {
if (ax_yaw.status != GIMBAL_STATUS_ENC_INIT) return;
if (ax_yaw.seek_right) {
if (ax_yaw.enc_deviation_triggered()) {
tmc5160_rotateMotor(AXIS_YAW, 0);
ax_yaw.update(); ax_yaw.update();
Serial.println(ax_yaw.X_enc); ax_yaw.enc_limit_r = ax_yaw.X_enc;
Serial.println(ax_yaw.X_act); ax_yaw.sync_encoder_to_actual(); // reset step counter so left pass starts from a clean origin
Serial.println(ax_yaw.X_err); ax_yaw.clear_enc_deviation();
ax_yaw.encoderpos_r = ax_yaw.X_enc; ax_yaw.seek_right = false;
ax_yaw.set_xenc_to_xact(); s_yaw_homing_tick = millis(); // start the deceleration timeout
ax_yaw.reset_enc_dev_warn();
ax_yaw.move_endstop_right = false;
s_tick_move_yaw = millis();
}
}
else if (ax_yaw.driver_status == GIMBAL_STATUS_ENC_INIT && !ax_yaw.move_endstop_right) {
if (ax_yaw.get_dev_warn_status()) {
tmc5160_rotateMotor(MotorYaw, 0);
ax_yaw.update();
ax_yaw.encoderpos_l = ax_yaw.X_enc;
ax_yaw.set_xenc_to_xact();
ax_yaw.reset_enc_dev_warn();
ax_yaw.move_endstop_right = true;
if (abs(ax_yaw.encoderpos_l - ax_yaw.encoderpos_r) Serial.print(F("Yaw right limit: ")); Serial.println(ax_yaw.enc_limit_r);
< (ax_yaw.one_round_in_steps - HOMING_MIN_ROUND_DIFF_OFFSET)) {
Serial.println(F("Endstop drive failed"));
ax_yaw.driver_status = GIMBAL_STATUS_ERROR;
}
else {
ax_yaw.driver_status = GIMBAL_STATUS_INITIALIZED;
ax_yaw.act_steps_per_round = ax_yaw.encoderpos_r - ax_yaw.encoderpos_l;
Serial.println(F("YAW ENDs L/R/Diff"));
Serial.println(ax_yaw.encoderpos_l);
Serial.println(ax_yaw.encoderpos_r);
Serial.println(abs(ax_yaw.encoderpos_l - ax_yaw.encoderpos_r));
tmc5160_moveTo(MotorYaw,
(ax_yaw.encoderpos_l + ax_yaw.encoderpos_r) / 2, 35000);
}
} }
return; // still moving right — nothing else to check this iteration
}
// Left pass: wait for the timeout before commanding motion, then wait for
// another deviation event. The timeout check re-fires every iteration until
// it triggers, at which point the motor starts moving.
if (millis() - s_yaw_homing_tick > HOMING_YAW_TIMEOUT_MS) {
s_yaw_homing_tick = millis(); // re-arm so the motor keeps moving if called again
tmc5160_rotateMotor(AXIS_YAW, -YAW_ENDSTOP_SPEED);
}
if (ax_yaw.enc_deviation_triggered()) {
tmc5160_rotateMotor(AXIS_YAW, 0);
ax_yaw.update();
ax_yaw.enc_limit_l = ax_yaw.X_enc;
ax_yaw.sync_encoder_to_actual();
ax_yaw.clear_enc_deviation();
ax_yaw.seek_right = true; // reset for any future re-homing
Serial.print(F("Yaw left limit: ")); Serial.println(ax_yaw.enc_limit_l);
homing_complete(ax_yaw, HOMING_YAW_CENTER_SPEED);
} }
} }
static void pitch_homing(void) { // ---------------------------------------------------------------------------
if (ax_pitch.driver_status == GIMBAL_STATUS_ENC_INIT && ax_pitch.move_endstop_right) { // pitch_homing_loop() (internal)
if (ax_pitch.get_status_endswitch_r()) { //
// Pitch homing state machine — call every loop while status == ENC_INIT.
//
// Pitch uses physical endstop switches rather than stallGuard. The TMC5160
// latches XACTUAL into XLATCH the instant a switch activates, giving a more
// accurate position than reading XACTUAL a few loop iterations later.
//
// Phase 1 (seek_right = true): move right, wait for right switch → enc_limit_r
// Phase 2 (seek_right = false): move left, wait for left switch → enc_limit_l
// ---------------------------------------------------------------------------
static void pitch_homing_loop(void) {
if (ax_pitch.status != GIMBAL_STATUS_ENC_INIT) return;
if (ax_pitch.seek_right) {
if (ax_pitch.endswitch_r()) {
ax_pitch.update(); ax_pitch.update();
Serial.println(ax_pitch.X_enc); delay(HOMING_DELAY_MS); // brief pause so XLATCH settles before reading
Serial.println(ax_pitch.X_act); ax_pitch.enc_limit_r = ax_pitch.get_x_latch();
Serial.println(ax_pitch.X_err); ax_pitch.clear_enc_deviation();
delay(HOMING_DELAY_MS); ax_pitch.seek_right = false;
ax_pitch.encoderpos_r = ax_pitch.get_X_Latch(); tmc5160_rotateMotor(AXIS_PITCH, -PITCH_ENDSTOP_SPEED); // reverse toward left switch
ax_pitch.reset_enc_dev_warn();
ax_pitch.move_endstop_right = false;
tmc5160_rotateMotor(MotorPitch, -PITCH_ENDSTOP_SPEED);
}
}
else if (ax_pitch.driver_status == GIMBAL_STATUS_ENC_INIT && !ax_pitch.move_endstop_right) {
if (ax_pitch.get_status_endswitch_l()) {
ax_pitch.update();
delay(HOMING_DELAY_MS);
ax_pitch.encoderpos_l = ax_pitch.get_X_Latch();
ax_pitch.reset_enc_dev_warn();
ax_pitch.move_endstop_right = true;
if (abs(ax_pitch.encoderpos_l - ax_pitch.encoderpos_r) Serial.print(F("Pitch right limit: ")); Serial.println(ax_pitch.enc_limit_r);
< (ax_pitch.one_round_in_steps - HOMING_MIN_ROUND_DIFF_OFFSET)) {
Serial.println(F("Endstop drive failed"));
ax_pitch.driver_status = GIMBAL_STATUS_ERROR;
}
else {
ax_pitch.driver_status = GIMBAL_STATUS_INITIALIZED;
ax_pitch.act_steps_per_round = ax_pitch.encoderpos_r - ax_pitch.encoderpos_l;
Serial.println(F("Pitch ENDs L/R/Diff"));
Serial.println(ax_pitch.encoderpos_l);
Serial.println(ax_pitch.encoderpos_r);
Serial.println(abs(ax_pitch.encoderpos_l - ax_pitch.encoderpos_r));
tmc5160_moveTo(MotorPitch,
(ax_pitch.encoderpos_l + ax_pitch.encoderpos_r) / 2, 150000);
}
} }
return;
}
if (ax_pitch.endswitch_l()) {
ax_pitch.update();
delay(HOMING_DELAY_MS);
ax_pitch.enc_limit_l = ax_pitch.get_x_latch();
ax_pitch.clear_enc_deviation();
ax_pitch.seek_right = true;
Serial.print(F("Pitch left limit: ")); Serial.println(ax_pitch.enc_limit_l);
homing_complete(ax_pitch, HOMING_PITCH_CENTER_SPEED);
} }
} }
void endstop_routine(void) { // ---------------------------------------------------------------------------
yaw_homing(); // homing_routine() — public, called every loop from main.cpp
pitch_homing(); // ---------------------------------------------------------------------------
void homing_routine(void) {
yaw_homing_loop();
pitch_homing_loop();
} }

11
firmware/src/motor/Homing.h
View File

@ -4,7 +4,14 @@
#include <Arduino.h> #include <Arduino.h>
#include "motor/MotorConfig.h" #include "motor/MotorConfig.h"
void do_endstop(uint16_t axis); // Kick off the homing sequence for one axis.
void endstop_routine(void); // Call once (e.g. from the 'q' command or after a reset).
// The axis must be enabled before calling this.
void do_homing_start(uint16_t axis);
// Non-blocking homing state machine tick.
// Must be called every loop iteration while status == GIMBAL_STATUS_ENC_INIT.
// Exits immediately (no-op) for axes that are not currently homing.
void homing_routine(void);
#endif #endif

136
firmware/src/motor/MotorAxis.cpp
View File

@ -1,68 +1,134 @@
#include "motor/MotorAxis.h" #include "motor/MotorAxis.h"
#include "config/Constants.h" #include "config/Constants.h"
MotorAxisParam ax_yaw(MotorYaw, YAW_STEPS_PER_ROUND, YAW_GEAR_RATIO, YAW_DEFAULT_VMAX); // One instance per physical axis. These are the only two MotorAxis objects in
MotorAxisParam ax_pitch(MotorPitch, PITCH_STEPS_PER_ROUND, PITCH_GEAR_RATIO, PITCH_DEFAULT_VMAX); // the system; everything else holds a reference or uses the extern declarations.
MotorAxis ax_yaw (AXIS_YAW, YAW_STEPS_PER_ROUND, YAW_GEAR_RATIO, YAW_DEFAULT_VMAX);
MotorAxis ax_pitch(AXIS_PITCH, PITCH_STEPS_PER_ROUND, PITCH_GEAR_RATIO, PITCH_DEFAULT_VMAX);
uint32_t MotorAxisParam::get_standstill_status() { MotorAxis::MotorAxis(uint16_t _id, int32_t _steps_per_round, float _gear_ratio, int32_t _default_vmax)
return tmc5160_fieldRead(MOTOR_ID, TMC5160_STST_FIELD); : id(_id),
steps_per_round(_steps_per_round),
gear_ratio(_gear_ratio),
default_vmax(_default_vmax),
enc_limit_l(0),
enc_limit_r(0),
act_steps_per_round(0),
step_size(0),
target(0),
v_max(_default_vmax), // start at the configured default; can be overridden at runtime
seek_right(false),
new_pos(false),
heading(0.0f),
heading_origin(0),
X_enc(0),
X_act(0),
X_err(0),
status(GIMBAL_STATUS_BOOT)
{}
// ---------------------------------------------------------------------------
// Position cache
// ---------------------------------------------------------------------------
// Reads the three position-related registers over SPI and caches them locally.
// Called once per loop so the rest of the code can read X_enc / X_act / X_err
// without each triggering its own SPI transaction.
void MotorAxis::update() {
X_enc = tmc5160_readRegister(id, TMC5160_XENC); // external encoder counter
X_act = tmc5160_readRegister(id, TMC5160_XACTUAL); // internal step counter
X_err = X_act - X_enc; // positive = stepped ahead of encoder
} }
bool MotorAxisParam::get_status_endswitch_l() { // ---------------------------------------------------------------------------
return tmc5160_fieldRead(MOTOR_ID, TMC5160_STATUS_STOP_L_FIELD); // Status reads
// ---------------------------------------------------------------------------
bool MotorAxis::is_standstill() {
// STST bit is set by the driver when velocity reaches zero and the motor is settled.
return tmc5160_fieldRead(id, TMC5160_STST_FIELD) != 0;
} }
bool MotorAxisParam::get_status_endswitch_r() { bool MotorAxis::endswitch_l() {
return tmc5160_fieldRead(MOTOR_ID, TMC5160_STATUS_STOP_R_FIELD); return tmc5160_fieldRead(id, TMC5160_STATUS_STOP_L_FIELD) != 0;
} }
uint32_t MotorAxisParam::get_X_Latch() { bool MotorAxis::endswitch_r() {
return tmc5160_readRegister(MOTOR_ID, TMC5160_XLATCH); return tmc5160_fieldRead(id, TMC5160_STATUS_STOP_R_FIELD) != 0;
} }
uint32_t MotorAxisParam::get_SGT_Status() { // XLATCH captures XACTUAL at the moment an endstop input transitions.
return tmc5160_fieldRead(MOTOR_ID, TMC5160_EVENT_STOP_SG_FIELD); // Reading it after an endstop fires gives the exact position of the limit,
// more reliable than reading XACTUAL a few loop iterations later.
int32_t MotorAxis::get_x_latch() {
return tmc5160_readRegister(id, TMC5160_XLATCH);
} }
uint32_t MotorAxisParam::get_dev_warn_status() { bool MotorAxis::stallguard_triggered() {
return tmc5160_fieldRead(MOTOR_ID, TMC5160_DEVIATION_WARN_FIELD); // EVENT_STOP_SG is a latched flag — it stays set until explicitly cleared.
// Used by yaw homing as a virtual endstop: the motor stalls against a
// mechanical stop, the driver detects the current spike, and sets this flag.
return tmc5160_fieldRead(id, TMC5160_EVENT_STOP_SG_FIELD) != 0;
} }
int32_t MotorAxisParam::get_dev_value() { bool MotorAxis::enc_deviation_triggered() {
return tmc5160_readRegister(MOTOR_ID, TMC5160_ENC_DEVIATION); // DEVIATION_WARN is set when |XACTUAL - XENC| exceeds ENC_DEVIATION.
// A large deviation means the encoder and step counter have diverged —
// typically because the motor stalled or skipped steps.
return tmc5160_fieldRead(id, TMC5160_DEVIATION_WARN_FIELD) != 0;
} }
void MotorAxisParam::set_SGT(uint8_t sgt) { int32_t MotorAxis::enc_deviation_value() {
tmc5160_fieldWrite(MOTOR_ID, TMC5160_SGT_FIELD, sgt); return tmc5160_readRegister(id, TMC5160_ENC_DEVIATION);
} }
void MotorAxisParam::reset_SGT() { // ---------------------------------------------------------------------------
tmc5160_fieldWrite(MOTOR_ID, TMC5160_EVENT_STOP_SG_FIELD, 1); // Configuration writes
// ---------------------------------------------------------------------------
void MotorAxis::set_speed(int32_t speed) {
tmc5160_writeRegister(id, TMC5160_VMAX, speed);
} }
void MotorAxisParam::reset_enc_dev_warn() { // SGT (stallguard threshold) controls how sensitive stall detection is.
tmc5160_fieldWrite(MOTOR_ID, TMC5160_DEVIATION_WARN_FIELD, 1); // Range: -64 to +63. Lower values = triggers more easily (more sensitive).
// During homing a high value (e.g. 60) is used so it only fires on a hard stop.
void MotorAxis::set_stallguard_threshold(uint8_t sgt) {
tmc5160_fieldWrite(id, TMC5160_SGT_FIELD, sgt);
} }
void MotorAxisParam::set_enc_dev_warn_limit(int32_t limit) { void MotorAxis::set_enc_deviation_limit(int32_t limit) {
tmc5160_fieldWrite(MOTOR_ID, TMC5160_ENC_DEVIATION_FIELD, limit); tmc5160_fieldWrite(id, TMC5160_ENC_DEVIATION_FIELD, limit);
} }
void MotorAxisParam::set_xenc_to_xact() { // Divides the usable travel range (endstop-to-endstop minus margins) into
tmc5160_writeRegister(MOTOR_ID, TMC5160_XACTUAL, X_enc); // num_steps equal intervals and stores the interval size in step_size.
// The margins prevent the motor from being commanded right to the physical limits.
void MotorAxis::set_step_size(int32_t num_steps) {
int32_t travel = (enc_limit_r - HOMING_XENC_MARGIN)
- (enc_limit_l + HOMING_XENC_MARGIN);
step_size = travel / (num_steps - 1); // (n-1) gaps between n positions
} }
void MotorAxisParam::set_speed(int32_t speed) { // Overwrites XACTUAL with the current encoder reading, snapping the step counter
tmc5160_writeRegister(MOTOR_ID, TMC5160_VMAX, speed); // to the encoder without physically moving the motor. This eliminates any
// accumulated tracking error before starting a new motion segment.
void MotorAxis::sync_encoder_to_actual() {
tmc5160_writeRegister(id, TMC5160_XACTUAL, X_enc);
} }
void MotorAxisParam::set_step_len(int32_t s) { // ---------------------------------------------------------------------------
int32_t turn_len = (encoderpos_r - HOMING_XENC_MARGIN) - (encoderpos_l + HOMING_XENC_MARGIN); // Latched flag clears
steps = turn_len / (s - 1); // ---------------------------------------------------------------------------
// These flags are write-to-clear: writing 1 resets the latch so the next
// event can be detected. Clearing before starting a new homing pass or motion
// segment prevents a stale flag from immediately triggering the handler.
void MotorAxis::clear_stallguard() {
tmc5160_fieldWrite(id, TMC5160_EVENT_STOP_SG_FIELD, 1);
} }
void MotorAxisParam::update() { void MotorAxis::clear_enc_deviation() {
X_enc = tmc5160_readRegister(MOTOR_ID, TMC5160_XENC); tmc5160_fieldWrite(id, TMC5160_DEVIATION_WARN_FIELD, 1);
X_act = tmc5160_readRegister(MOTOR_ID, TMC5160_XACTUAL);
X_err = X_act - X_enc;
} }

142
firmware/src/motor/MotorAxis.h
View File

@ -3,66 +3,100 @@
#include <Arduino.h> #include <Arduino.h>
#include "motor/MotorConfig.h" #include "motor/MotorConfig.h"
#include "TMC5160.h" #include "TMC5160.h"
struct MotorAxisParam { // ---------------------------------------------------------------------------
const uint16_t MOTOR_ID; // MotorAxis
const int32_t one_round_in_steps; //
const float gear_red_var; // Owns all runtime state for one physical axis and wraps every TMC5160
int32_t act_steps_per_round; // register access behind a named method. No code outside this struct should
int32_t encoderpos_l; // call tmc5160_readRegister / tmc5160_fieldRead with motor-specific addresses.
int32_t encoderpos_r; //
int32_t steps; // Method naming convention:
float c_hdg; // is_*() / *_triggered() — reads a status bit; no side effects
int32_t hdg_constant; // get_*() — reads a value register; no side effects
int32_t target; // set_*() — writes a configuration register
int32_t X_act; // clear_*() — resets a latched hardware flag (write-to-clear)
int32_t X_enc; // sync_*() — aligns one hardware register to another
int32_t X_err; // update() — refreshes the local position cache from hardware
int32_t v_max; // ---------------------------------------------------------------------------
gimbal_status driver_status;
bool move_endstop_right;
bool new_pos;
MotorAxisParam(uint16_t id, int32_t steps, float ratio, int32_t vmax) struct MotorAxis {
: MOTOR_ID(id),
one_round_in_steps(steps),
gear_red_var(ratio),
act_steps_per_round(0),
encoderpos_l(0),
encoderpos_r(0),
steps(0),
c_hdg(0.0f),
hdg_constant(0),
target(0),
X_act(0),
X_enc(0),
X_err(0),
v_max(vmax),
driver_status(GIMBAL_STATUS_BOOT),
move_endstop_right(false),
new_pos(false)
{}
uint32_t get_standstill_status(); // -----------------------------------------------------------------------
bool get_status_endswitch_l(); // Fixed parameters — set in the constructor, never changed at runtime
bool get_status_endswitch_r(); // -----------------------------------------------------------------------
uint32_t get_X_Latch(); const uint16_t id; // TMC5160 chip ID passed to every API call (AXIS_YAW / AXIS_PITCH)
uint32_t get_SGT_Status(); const int32_t steps_per_round; // nominal encoder counts per full mechanical revolution
uint32_t get_dev_warn_status(); const float gear_ratio; // gear reduction between motor shaft and output
int32_t get_dev_value(); const int32_t default_vmax; // speed applied on init and after reset
void set_SGT(uint8_t sgt);
void reset_SGT(); // -----------------------------------------------------------------------
void reset_enc_dev_warn(); // Homing results — written once by Homing.cpp, then treated as read-only
void set_enc_dev_warn_limit(int32_t limit); // -----------------------------------------------------------------------
void set_xenc_to_xact(); int32_t enc_limit_l; // encoder count where the left endstop was detected
void set_speed(int32_t speed); int32_t enc_limit_r; // encoder count where the right endstop was detected
void set_step_len(int32_t s); int32_t act_steps_per_round; // measured encoder travel between the two limits
// -----------------------------------------------------------------------
// Motion state
// -----------------------------------------------------------------------
int32_t step_size; // encoder counts per step in AUTO mode (set by set_step_size())
int32_t target; // next target encoder position, consumed by the motion controller
int32_t v_max; // current speed cap; can be changed at runtime via 'v' command
bool seek_right; // homing direction: true = moving toward right endstop
bool new_pos; // flag: a new target has been written and not yet acted on
// -----------------------------------------------------------------------
// Heading calibration — persisted to EEPROM via 'h'/'y' commands
// -----------------------------------------------------------------------
float heading; // calibrated heading value at heading_origin (degrees or application units)
int32_t heading_origin; // encoder count corresponding to the calibrated heading
// -----------------------------------------------------------------------
// Live position cache — refreshed every loop by update()
// -----------------------------------------------------------------------
int32_t X_enc; // XENC: encoder position (feedback from the external encoder)
int32_t X_act; // XACTUAL: internal step counter (what the driver thinks the position is)
int32_t X_err; // X_act - X_enc: tracking error between step counter and encoder
// -----------------------------------------------------------------------
// Axis lifecycle state
// -----------------------------------------------------------------------
gimbal_status status;
// Constructor
MotorAxis(uint16_t id, int32_t steps_per_round, float gear_ratio, int32_t default_vmax);
// Refresh X_enc, X_act, X_err from hardware (call once per loop)
void update(); void update();
// --- Status reads (no side effects) ---
bool is_standstill(); // true when the motor is not moving (STST bit)
bool endswitch_l(); // true when the left hardware endstop input is active
bool endswitch_r(); // true when the right hardware endstop input is active
int32_t get_x_latch(); // XLATCH: position captured at the moment an endstop fired
bool stallguard_triggered(); // true if a stall event has been latched (EVENT_STOP_SG)
bool enc_deviation_triggered(); // true if encoder vs step-counter difference exceeded the limit
int32_t enc_deviation_value(); // raw encoder deviation count from ENC_DEVIATION register
// --- Configuration writes ---
void set_speed(int32_t speed); // write VMAX
void set_stallguard_threshold(uint8_t sgt); // write SGT field (sensitivity: lower = more sensitive)
void set_enc_deviation_limit(int32_t limit); // write ENC_DEVIATION threshold
void set_step_size(int32_t num_steps); // divide the homed travel range into num_steps equal steps
// Overwrite XACTUAL with the current encoder position, eliminating accumulated tracking error.
// Call this before starting a new motion segment to avoid a step-count jump.
void sync_encoder_to_actual();
// --- Latched flag clears (write-to-clear) ---
void clear_stallguard(); // clear EVENT_STOP_SG so the next stall can be detected
void clear_enc_deviation(); // clear DEVIATION_WARN so the next deviation can be detected
}; };
extern MotorAxisParam ax_yaw; // Global axis instances — defined in MotorAxis.cpp
extern MotorAxisParam ax_pitch; extern MotorAxis ax_yaw;
extern MotorAxis ax_pitch;
#endif #endif

30
firmware/src/motor/MotorConfig.h
View File

@ -3,17 +3,29 @@
#include <stdint.h> #include <stdint.h>
#define MotorYaw 0x00 // Axis IDs match the TMC5160 chip-select assignments in SPI_HAL.cpp.
#define MotorPitch 0x01 // They are also used as array indices where applicable, so keep them 0-based.
#define AXIS_YAW 0x00
#define AXIS_PITCH 0x01
#define AXIS_COUNT 2
// EEPROM layout for heading calibration.
// Defined here so save (case 'h') and load (case 'y') in CommandParser always
// agree on offsets — nothing should hardcode byte addresses independently.
#define EEPROM_ADDR_HDG_FLOAT 0 // float: calibrated heading value
#define EEPROM_ADDR_HDG_DIST (EEPROM_ADDR_HDG_FLOAT + sizeof(float)) // int32_t: encoder distance from left limit
// Axis lifecycle states.
// The state machine progresses: BOOT → RESET → ENC_INIT → INITIALIZED → AUTO/MANUAL
// ERROR is a terminal state; re-init via the 'r' command resets back to RESET.
typedef enum { typedef enum {
GIMBAL_STATUS_ERROR = -2, GIMBAL_STATUS_ERROR = -2, // homing failed or hardware fault
GIMBAL_STATUS_BOOT = -1, GIMBAL_STATUS_BOOT = -1, // initial power-on state, hardware not yet configured
GIMBAL_STATUS_RESET = 0, GIMBAL_STATUS_RESET = 0, // registers written, motor enabled, ready to home
GIMBAL_STATUS_ENC_INIT = 1, GIMBAL_STATUS_ENC_INIT = 1, // homing sequence in progress
GIMBAL_STATUS_INITIALIZED= 2, GIMBAL_STATUS_INITIALIZED = 2, // homing complete, endstop limits known
GIMBAL_STATUS_AUTO = 3, GIMBAL_STATUS_AUTO = 3, // stepping through positions automatically
GIMBAL_STATUS_MANUAL = 4 GIMBAL_STATUS_MANUAL = 4 // responding to direct move commands
} gimbal_status; } gimbal_status;
#endif #endif

262
firmware/src/motor/MotorDriver.cpp
View File

@ -6,151 +6,167 @@
#include <SPI.h> #include <SPI.h>
#include "TMC5160.h" #include "TMC5160.h"
static void write_regs_yaw(void) { // ---------------------------------------------------------------------------
tmc5160_writeRegister(MotorYaw, 0x00, 0x0000000C); // Register initialisation
tmc5160_writeRegister(MotorYaw, 0x09, 0x00010606); //
tmc5160_writeRegister(MotorYaw, 0x0A, 0x00080400); // Each write is annotated with the TMC5160 register name so the intent is
tmc5160_writeRegister(MotorYaw, 0x0B, YAW_GLOBAL_SCALER); // readable without cross-referencing the datasheet. Values that come from
tmc5160_writeRegister(MotorYaw, 0x10, 0x00071F08); // Constants.h are named; the rest are one-off configuration words that don't
tmc5160_writeRegister(MotorYaw, 0x11, 0x00000008); // need a named constant because they are never used anywhere else.
tmc5160_writeRegister(MotorYaw, 0x13, 0); //
tmc5160_writeRegister(MotorYaw, 0x14, 0); // Both functions follow the same structure; only the axis ID and a handful
tmc5160_writeRegister(MotorYaw, 0x15, 0); // of tuning values (GLOBALSCALER, XACTUAL) differ between yaw and pitch.
tmc5160_writeRegister(MotorYaw, 0x21, YAW_DEFAULT_XACTUAL); // ---------------------------------------------------------------------------
tmc5160_writeRegister(MotorYaw, 0x24, ACCELERATION);
tmc5160_writeRegister(MotorYaw, 0x26, ACCELERATION); void motor_regs_init_yaw(void) {
tmc5160_writeRegister(MotorYaw, 0x25, 0x00000000); tmc5160_writeRegister(AXIS_YAW, 0x00, 0x0000000C); // GCONF: default global config
tmc5160_writeRegister(MotorYaw, 0x27, 0); tmc5160_writeRegister(AXIS_YAW, 0x09, 0x00010606); // SHORTCONF: short-circuit protection timings
tmc5160_writeRegister(MotorYaw, 0x28, ACCELERATION); tmc5160_writeRegister(AXIS_YAW, 0x0A, 0x00080400); // DRVCONF: driver output configuration
tmc5160_writeRegister(MotorYaw, 0x2A, ACCELERATION); tmc5160_writeRegister(AXIS_YAW, 0x0B, YAW_GLOBAL_SCALER); // GLOBALSCALER: scales peak motor current
tmc5160_writeRegister(MotorYaw, 0x2B, 0x00000100); tmc5160_writeRegister(AXIS_YAW, 0x10, 0x00071F08); // IHOLD_IRUN: hold current | run current | transition delay
tmc5160_writeRegister(MotorYaw, 0x2D, 500000); tmc5160_writeRegister(AXIS_YAW, 0x11, 0x00000008); // TPOWERDOWN: delay before reducing to hold current after standstill
tmc5160_writeRegister(MotorYaw, 0x34, 0x08A3); tmc5160_writeRegister(AXIS_YAW, 0x13, 0); // TPWMTHRS: upper velocity for stealthChop — 0 = always stealthChop
tmc5160_writeRegister(MotorYaw, 0x35, 0xFFFF); tmc5160_writeRegister(AXIS_YAW, 0x14, 0); // TCOOLTHRS: lower velocity for coolStep / stallGuard — 0 = disabled
tmc5160_writeRegister(MotorYaw, 0x38, ENCMODE_DECIMAL); tmc5160_writeRegister(AXIS_YAW, 0x15, 0); // THIGH: upper velocity threshold for high-speed mode — 0 = disabled
tmc5160_writeRegister(MotorYaw, 0x39, 500000); tmc5160_writeRegister(AXIS_YAW, 0x21, YAW_DEFAULT_XACTUAL); // XACTUAL: seed the internal position counter
tmc5160_writeRegister(MotorYaw, 0x3A, ENC_CONST); tmc5160_writeRegister(AXIS_YAW, 0x24, ACCELERATION); // A1: first acceleration (flat ramp profile — same value as AMAX)
tmc5160_writeRegister(MotorYaw, 0x3D, ENC_DEVIATION_LIMIT); tmc5160_writeRegister(AXIS_YAW, 0x25, 0); // V1: velocity threshold between A1 and AMAX — 0 disables two-stage ramp
tmc5160_writeRegister(MotorYaw, 0x6C, 0x00410043); tmc5160_writeRegister(AXIS_YAW, 0x26, ACCELERATION); // AMAX: maximum acceleration
tmc5160_writeRegister(MotorYaw, 0x6D, 0x00000000); tmc5160_writeRegister(AXIS_YAW, 0x27, 0); // VMAX (RAMPMODE=0 only): not used in HOLD mode
tmc5160_writeRegister(MotorYaw, 0x70, 0x010C1E3C); tmc5160_writeRegister(AXIS_YAW, 0x28, ACCELERATION); // DMAX: maximum deceleration
tmc5160_writeRegister(MotorYaw, TMC5160_RAMPMODE, TMC5160_MODE_HOLD); tmc5160_writeRegister(AXIS_YAW, 0x2A, ACCELERATION); // D1: final deceleration
tmc5160_writeRegister(AXIS_YAW, 0x2B, 0x00000100); // VSTOP: minimum velocity before the ramp stops (must be > 0)
tmc5160_writeRegister(AXIS_YAW, 0x2D, 500000); // XTARGET: initial position target
tmc5160_writeRegister(AXIS_YAW, 0x34, 0x08A3); // SW_MODE: endstop input polarity and latch behaviour
tmc5160_writeRegister(AXIS_YAW, 0x35, 0xFFFF); // RAMP_STAT: write all-ones to clear any stale status flags
tmc5160_writeRegister(AXIS_YAW, 0x38, ENCMODE_DECIMAL); // ENCMODE: encoder counting mode (decimal / N-channel)
tmc5160_writeRegister(AXIS_YAW, 0x39, 500000); // X_ENC: seed the encoder counter to match XACTUAL
tmc5160_writeRegister(AXIS_YAW, 0x3A, ENC_CONST); // ENC_CONST: encoder counts per full electrical cycle
tmc5160_writeRegister(AXIS_YAW, 0x3D, ENC_DEVIATION_LIMIT); // ENC_DEVIATION: max allowed |XACTUAL - XENC| before DEVIATION_WARN fires
tmc5160_writeRegister(AXIS_YAW, 0x6C, 0x00410043); // CHOPCONF: chopper configuration (stealthChop, microstep resolution)
tmc5160_writeRegister(AXIS_YAW, 0x6D, 0x00000000); // COOLCONF: coolStep configuration — all zeros = disabled
tmc5160_writeRegister(AXIS_YAW, 0x70, 0x010C1E3C); // PWMCONF: stealthChop PWM parameters
tmc5160_writeRegister(AXIS_YAW, TMC5160_RAMPMODE, TMC5160_MODE_HOLD); // start in HOLD mode (no autonomous motion)
} }
static void write_regs_pitch(void) { void motor_regs_init_pitch(void) {
tmc5160_writeRegister(MotorPitch, 0x00, 0x0000000C); tmc5160_writeRegister(AXIS_PITCH, 0x00, 0x0000000C); // GCONF
tmc5160_writeRegister(MotorPitch, 0x09, 0x00010606); tmc5160_writeRegister(AXIS_PITCH, 0x09, 0x00010606); // SHORTCONF
tmc5160_writeRegister(MotorPitch, 0x0A, 0x00080400); tmc5160_writeRegister(AXIS_PITCH, 0x0A, 0x00080400); // DRVCONF
tmc5160_writeRegister(MotorPitch, 0x0B, PITCH_GLOBAL_SCALER); tmc5160_writeRegister(AXIS_PITCH, 0x0B, PITCH_GLOBAL_SCALER); // GLOBALSCALER
tmc5160_writeRegister(MotorPitch, 0x10, 0x00071F08); tmc5160_writeRegister(AXIS_PITCH, 0x10, 0x00071F08); // IHOLD_IRUN
tmc5160_writeRegister(MotorPitch, 0x11, 0x00000008); tmc5160_writeRegister(AXIS_PITCH, 0x11, 0x00000008); // TPOWERDOWN
tmc5160_writeRegister(MotorPitch, 0x13, 0); tmc5160_writeRegister(AXIS_PITCH, 0x13, 0); // TPWMTHRS — stealthChop threshold disabled
tmc5160_writeRegister(MotorPitch, 0x14, 0); tmc5160_writeRegister(AXIS_PITCH, 0x14, 0); // TCOOLTHRS — coolStep threshold disabled
tmc5160_writeRegister(MotorPitch, 0x15, 0); tmc5160_writeRegister(AXIS_PITCH, 0x15, 0); // THIGH — high-speed threshold disabled
tmc5160_writeRegister(MotorPitch, 0x21, PITCH_DEFAULT_XACTUAL); tmc5160_writeRegister(AXIS_PITCH, 0x21, PITCH_DEFAULT_XACTUAL); // XACTUAL
tmc5160_writeRegister(MotorPitch, 0x24, ACCELERATION); tmc5160_writeRegister(AXIS_PITCH, 0x24, ACCELERATION); // A1
tmc5160_writeRegister(MotorPitch, 0x26, ACCELERATION); tmc5160_writeRegister(AXIS_PITCH, 0x25, 0); // V1 — two-stage ramp disabled
tmc5160_writeRegister(MotorPitch, 0x25, 0x00000000); tmc5160_writeRegister(AXIS_PITCH, 0x26, ACCELERATION); // AMAX
tmc5160_writeRegister(MotorPitch, 0x27, 0); tmc5160_writeRegister(AXIS_PITCH, 0x27, 0); // VMAX (unused in HOLD mode)
tmc5160_writeRegister(MotorPitch, 0x28, ACCELERATION); tmc5160_writeRegister(AXIS_PITCH, 0x28, ACCELERATION); // DMAX
tmc5160_writeRegister(MotorPitch, 0x2A, ACCELERATION); tmc5160_writeRegister(AXIS_PITCH, 0x2A, ACCELERATION); // D1
tmc5160_writeRegister(MotorPitch, 0x2B, 0x00000100); tmc5160_writeRegister(AXIS_PITCH, 0x2B, 0x00000100); // VSTOP
tmc5160_writeRegister(MotorPitch, 0x2D, 500000); tmc5160_writeRegister(AXIS_PITCH, 0x2D, 500000); // XTARGET
tmc5160_writeRegister(MotorPitch, 0x34, 0x08A3); tmc5160_writeRegister(AXIS_PITCH, 0x34, 0x08A3); // SW_MODE
tmc5160_writeRegister(MotorPitch, 0x35, 0xFFFF); tmc5160_writeRegister(AXIS_PITCH, 0x35, 0xFFFF); // RAMP_STAT — clear stale flags
tmc5160_writeRegister(MotorPitch, 0x38, ENCMODE_DECIMAL); tmc5160_writeRegister(AXIS_PITCH, 0x38, ENCMODE_DECIMAL); // ENCMODE
tmc5160_writeRegister(MotorPitch, 0x39, 500000); tmc5160_writeRegister(AXIS_PITCH, 0x39, 500000); // X_ENC
tmc5160_writeRegister(MotorPitch, 0x3A, ENC_CONST); tmc5160_writeRegister(AXIS_PITCH, 0x3A, ENC_CONST); // ENC_CONST
tmc5160_writeRegister(MotorPitch, 0x3D, ENC_DEVIATION_LIMIT); tmc5160_writeRegister(AXIS_PITCH, 0x3D, ENC_DEVIATION_LIMIT); // ENC_DEVIATION
tmc5160_writeRegister(MotorPitch, 0x6C, 0x00410043); tmc5160_writeRegister(AXIS_PITCH, 0x6C, 0x00410043); // CHOPCONF
tmc5160_writeRegister(MotorPitch, 0x6D, 0x00000000); tmc5160_writeRegister(AXIS_PITCH, 0x6D, 0x00000000); // COOLCONF — disabled
tmc5160_writeRegister(MotorPitch, 0x70, 0x010C1E3C); tmc5160_writeRegister(AXIS_PITCH, 0x70, 0x010C1E3C); // PWMCONF
tmc5160_writeRegister(MotorPitch, TMC5160_RAMPMODE, TMC5160_MODE_HOLD); tmc5160_writeRegister(AXIS_PITCH, TMC5160_RAMPMODE, TMC5160_MODE_HOLD);
} }
void init_motor_hardware(void) { // ---------------------------------------------------------------------------
pinMode(PIN_TMC_CS_YAW, OUTPUT); // Hardware initialisation — call once in setup(), before any register writes
digitalWrite(PIN_TMC_CS_YAW, HIGH); // ---------------------------------------------------------------------------
pinMode(PIN_TMC_EN_YAW, OUTPUT);
digitalWrite(PIN_TMC_EN_YAW, HIGH);
pinMode(PIN_TMC_CS_PITCH, OUTPUT);
digitalWrite(PIN_TMC_CS_PITCH, HIGH);
pinMode(PIN_TMC_EN_PITCH, OUTPUT);
digitalWrite(PIN_TMC_EN_PITCH, HIGH);
void motor_hw_init(void) {
// CS and EN default HIGH: chip deselected, driver coils off.
// EN must be HIGH before registers are written — energising the motor
// before the driver is configured can cause unexpected movement.
pinMode(PIN_TMC_CS_YAW, OUTPUT); digitalWrite(PIN_TMC_CS_YAW, HIGH);
pinMode(PIN_TMC_EN_YAW, OUTPUT); digitalWrite(PIN_TMC_EN_YAW, HIGH);
pinMode(PIN_TMC_CS_PITCH, OUTPUT); digitalWrite(PIN_TMC_CS_PITCH, HIGH);
pinMode(PIN_TMC_EN_PITCH, OUTPUT); digitalWrite(PIN_TMC_EN_PITCH, HIGH);
// TMC5160 datasheet requires SPI Mode 3 (CPOL=1, CPHA=1), MSB first.
SPI.setBitOrder(MSBFIRST); SPI.setBitOrder(MSBFIRST);
SPI.setClockDivider(SPI_CLOCK_DIV); SPI.setClockDivider(SPI_CLOCK_DIV);
SPI.setDataMode(SPI_MODE3); SPI.setDataMode(SPI_MODE3);
SPI.begin(); SPI.begin();
delay(50); delay(50); // allow supply rails and oscillator to stabilise before first SPI transaction
// Initialise the TMC-API's internal register shadow cache.
// The library caches written values so it can do read-modify-write field operations
// without a full register read; the cache must be cleared before first use.
tmc5160_initCache(); tmc5160_initCache();
} }
void init_motor_params(void) { void motor_regs_init(void) {
write_regs_yaw(); motor_regs_init_yaw();
write_regs_pitch(); motor_regs_init_pitch();
} }
void motor_power_on(uint16_t axis) { // ---------------------------------------------------------------------------
if (axis == MotorPitch) { // Enable / disable
digitalWrite(PIN_TMC_EN_PITCH, LOW); //
} else if (axis == MotorYaw) { // The EN pin is active-low: LOW energises the coils (holding torque active),
digitalWrite(PIN_TMC_EN_YAW, LOW); // HIGH de-energises them (motor free-wheels, no position holding).
} // ---------------------------------------------------------------------------
void motor_enable(uint16_t axis) {
if (axis == AXIS_YAW) digitalWrite(PIN_TMC_EN_YAW, LOW);
else if (axis == AXIS_PITCH) digitalWrite(PIN_TMC_EN_PITCH, LOW);
} }
void motor_power_off(uint16_t axis) { void motor_disable(uint16_t axis) {
if (axis == MotorPitch) { if (axis == AXIS_YAW) digitalWrite(PIN_TMC_EN_YAW, HIGH);
digitalWrite(PIN_TMC_EN_PITCH, HIGH); else if (axis == AXIS_PITCH) digitalWrite(PIN_TMC_EN_PITCH, HIGH);
} else if (axis == MotorYaw) {
digitalWrite(PIN_TMC_EN_YAW, HIGH);
}
} }
void motor_periodic_job(uint16_t axis) { // ---------------------------------------------------------------------------
if (axis == MotorYaw) { // Loop jobs
ax_yaw.update(); // ---------------------------------------------------------------------------
} else if (axis == MotorPitch) {
ax_pitch.update(); void motor_update(uint16_t axis) {
} if (axis == AXIS_YAW) ax_yaw.update();
else if (axis == AXIS_PITCH) ax_pitch.update();
} }
void print_motor_info(uint16_t axis) { // Prints a comma-separated diagnostic line for one axis.
const char *spacer = ", "; //
if (axis == MotorPitch) { // Yaw fields: X_enc, X_act, X_err, dev_warn, sg_event, sg_status, sg_stop
// Pitch fields: (same as yaw) + endswitch_l, endswitch_r
//
// Pitch gets the extra endstop columns because it uses physical switches;
// yaw uses encoder deviation for homing so those fields are less useful there.
void motor_print_info(uint16_t axis) {
const char *sep = ", ";
if (axis == AXIS_PITCH) {
Serial.print(F("PITCH: ")); Serial.print(F("PITCH: "));
Serial.print(ax_pitch.X_enc); Serial.print(ax_pitch.X_enc); Serial.print(sep);
Serial.print(spacer); Serial.print(ax_pitch.X_act); Serial.print(sep);
Serial.print(ax_pitch.X_act); Serial.print(ax_pitch.X_err); Serial.print(sep);
Serial.print(spacer); Serial.print(ax_pitch.enc_deviation_triggered()); Serial.print(sep);
Serial.print(ax_pitch.X_err); Serial.print(ax_pitch.stallguard_triggered()); Serial.print(sep);
Serial.print(spacer); Serial.print(tmc5160_fieldRead(AXIS_PITCH, TMC5160_STATUS_SG_FIELD)); Serial.print(sep);
Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_DEVIATION_WARN_FIELD)); Serial.print(tmc5160_fieldRead(AXIS_PITCH, TMC5160_SG_STOP_FIELD)); Serial.print(sep);
Serial.print(spacer); Serial.print(ax_pitch.endswitch_l()); Serial.print(sep);
Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_DEVIATION_WARN_FIELD)); Serial.println(ax_pitch.endswitch_r());
Serial.print(spacer);
Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_STATUS_SG_FIELD)); } else if (axis == AXIS_YAW) {
Serial.print(spacer);
Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_SG_STOP_FIELD));
Serial.print(spacer);
Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_STATUS_STOP_L_FIELD));
Serial.print(spacer);
Serial.println(tmc5160_fieldRead(MotorPitch, TMC5160_STATUS_STOP_R_FIELD));
} else if (axis == MotorYaw) {
Serial.print(F("YAW: ")); Serial.print(F("YAW: "));
Serial.print(ax_yaw.X_enc); Serial.print(ax_yaw.X_enc); Serial.print(sep);
Serial.print(spacer); Serial.print(ax_yaw.X_act); Serial.print(sep);
Serial.print(ax_yaw.X_act); Serial.print(ax_yaw.X_err); Serial.print(sep);
Serial.print(spacer); Serial.print(ax_yaw.enc_deviation_triggered()); Serial.print(sep);
Serial.print(ax_yaw.X_err); Serial.print(ax_yaw.stallguard_triggered()); Serial.print(sep);
Serial.print(spacer); Serial.print(tmc5160_fieldRead(AXIS_YAW, TMC5160_STATUS_SG_FIELD)); Serial.print(sep);
Serial.print(tmc5160_fieldRead(MotorYaw, TMC5160_DEVIATION_WARN_FIELD)); Serial.println(tmc5160_fieldRead(AXIS_YAW, TMC5160_SG_STOP_FIELD));
Serial.print(spacer);
Serial.print(tmc5160_fieldRead(MotorYaw, TMC5160_DEVIATION_WARN_FIELD));
Serial.print(spacer);
Serial.print(tmc5160_fieldRead(MotorYaw, TMC5160_STATUS_SG_FIELD));
Serial.print(spacer);
Serial.println(tmc5160_fieldRead(MotorYaw, TMC5160_SG_STOP_FIELD));
} }
} }

29
firmware/src/motor/MotorDriver.h
View File

@ -4,11 +4,28 @@
#include <Arduino.h> #include <Arduino.h>
#include "motor/MotorConfig.h" #include "motor/MotorConfig.h"
void init_motor_hardware(void); // One-time hardware setup: SPI peripheral + CS/EN pin directions.
void init_motor_params(void); // Call before any SPI communication.
void motor_power_on(uint16_t axis); void motor_hw_init(void);
void motor_power_off(uint16_t axis);
void motor_periodic_job(uint16_t axis); // Write the full TMC5160 register configuration to both axes.
void print_motor_info(uint16_t axis); // Safe to call again at runtime (via 'r' command) to recover from a fault.
void motor_regs_init(void);
// Per-axis register init — exposed individually so CommandParser can reset
// one axis without disturbing the other.
void motor_regs_init_yaw(void);
void motor_regs_init_pitch(void);
// Enable / disable motor coils via the EN pin (active-low).
// Disable = coils off, motor free-wheels, no holding torque.
void motor_enable(uint16_t axis);
void motor_disable(uint16_t axis);
// Refresh cached position state from hardware. Call once per loop iteration.
void motor_update(uint16_t axis);
// Print a comma-separated status line to Serial. Call on a slow timer (~250 ms).
void motor_print_info(uint16_t axis);
#endif #endif

296
firmware/src/serial/CommandParser.cpp
View File

@ -8,198 +8,228 @@
#include "TMC5160.h" #include "TMC5160.h"
void read_command() { // ---------------------------------------------------------------------------
// Command format: <cmd>[y|p][d]<arg1>,<arg2>,... // Serial command protocol
// Axis specifier: y=Yaw only, p=Pitch only //
// Base specifier: d=decimal (default hex) // Format: <cmd> [y|p] [d] <arg1> , <arg2> , ...
//
// Axis specifier (optional, default = both):
// y — yaw only
// p — pitch only
//
// Base specifier (optional, default = hex):
// d — interpret numeric arguments as decimal
//
// Command table:
// r re-init registers and enable coils (safe recovery from any fault)
// q start homing sequence
// ! disable motor coils (free-wheel)
// * enable motor coils (apply holding torque)
// m move to absolute encoder position: m[y|p][d] <pos>, <speed>
// ; overwrite XACTUAL register directly: ; <value>
// s stop (set velocity to 0)
// b clear encoder deviation warning flag
// t sync encoder to actual position (zero out tracking error)
// e clear stallguard event flag (both axes)
// w set stallguard threshold: w <sgt>
// u enter AUTO stepping mode: u <num_steps> (must be > 2)
// h save heading calibration to EEPROM: h 0, <heading_value>
// y load heading calibration from EEPROM: y 0
// d enter MANUAL mode (disables AUTO stepping on yaw)
// p advance yaw by one step (AUTO mode)
// v set yaw v_max: v <speed>
// + set yaw right endstop to current encoder position
// - set yaw left endstop to current encoder position
// / set both yaw endstops manually: / <right>, <left>
// ? print current yaw endstop positions
// ---------------------------------------------------------------------------
// Get first character of recieved command // Peek at the next byte to check for an axis specifier ('y' or 'p').
char c = Serial.read(); // Consumes the byte if found. Returns a Flash string label for Serial output.
static const __FlashStringHelper* parse_axis(uint8_t &out_yaw, uint8_t &out_pitch) {
// Parse optional axis specifier: y (yaw) or p (pitch) out_yaw = 1;
uint8_t target_yaw = 1; out_pitch = 1;
uint8_t target_pitch = 1;
const __FlashStringHelper* axis_str; // string to hold the suffix for terminal messages
if (Serial.peek() == 'y') { if (Serial.peek() == 'y') {
Serial.read(); Serial.read();
target_pitch = 0; out_pitch = 0;
axis_str = F(" [YAW]"); return F(" [YAW]");
} }
else if (Serial.peek() == 'p') { if (Serial.peek() == 'p') {
Serial.read(); Serial.read();
target_yaw = 0; out_yaw = 0;
axis_str = F(" [PITCH]") return F(" [PITCH]");
}
else {
axis_str = F(" [YAW+PITCH]");
} }
return F(" [YAW + PITCH]");
}
// Check if a "d" follows, for decimal values instead of hex // Read the rest of the command line (up to newline) and tokenise on commas.
// Up to 4 arguments are parsed; extras are silently ignored.
// Returns the number of arguments found.
static uint8_t parse_args(int32_t args[4], uint8_t base) {
static char buf[32]; // static to avoid stack allocation on every call
size_t len = Serial.readBytesUntil('\n', buf, sizeof(buf) - 1);
buf[len] = '\0';
uint8_t argc = 0;
char *tok = strtok(buf, ",");
while (tok && argc < 4) {
args[argc++] = strtol(tok, nullptr, base);
tok = strtok(nullptr, ",");
}
return argc;
}
void read_command() {
char c = Serial.read(); // command character — must be read before peek/parse_axis consume it
uint8_t do_yaw, do_pitch;
const __FlashStringHelper *axis_label = parse_axis(do_yaw, do_pitch);
// 'd' immediately after the axis specifier switches argument parsing to decimal.
// Default is hex, which is convenient for register values and encoder positions.
uint8_t base = 16; uint8_t base = 16;
if (Serial.peek() == 'd') { if (Serial.peek() == 'd') { Serial.read(); base = 10; }
Serial.read();
base = 10;
}
// Create a stactic buffer for reading the rest of the command
static char buffer[32];
size_t len = Serial.readBytesUntil('\n', buffer, sizeof(buffer) - 1);
buffer[len] = '\0'; // Add termination character to string
int32_t args[4] = {0}; int32_t args[4] = {0};
uint8_t argc = 0; parse_args(args, base);
char *token = strtok(buffer, ",");
while (token && argc < 4) {
args[argc++] = strtol(token, nullptr, base);
token = strtok(nullptr, ",");
}
switch (c) { switch (c) {
case 'r': // power cycle the motors case 'r': // re-init registers and re-enable — useful after a fault or power issue
Serial.print(F("Reseting")); Serial.print(F("Resetting")); Serial.println(axis_label);
Serial.println(axis_str); // targeted motor axis if (do_yaw) { motor_regs_init_yaw(); motor_enable(AXIS_YAW); ax_yaw.status = GIMBAL_STATUS_RESET; }
if (do_pitch) { motor_regs_init_pitch(); motor_enable(AXIS_PITCH); ax_pitch.status = GIMBAL_STATUS_RESET; }
if (target_yaw) {
write_regs_yaw();
motor_power_on(MotorYaw);
ax_yaw.driver_status = GIMBAL_STATUS_RESET;
}
if (target_pitch) {
write_regs_pitch();
motor_power_on(MotorPitch);
ax_pitch.driver_status = GIMBAL_STATUS_RESET;
}
break; break;
case 'q': // find endstop case 'q': // start homing — axis must be enabled first
Serial.print(F("Finding endstops")); Serial.print(F("Homing")); Serial.println(axis_label);
Serial.println(axis_str); // targeted motor axis if (do_yaw) do_homing_start(AXIS_YAW);
if (target_yaw) do_endstop(MotorYaw); if (do_pitch) do_homing_start(AXIS_PITCH);
if (target_pitch) do_endstop(MotorPitch);
break; break;
case '!': // turn motors off case '!': // disable coils — motor can be moved by hand
Serial.print(F("Turning motors off")); Serial.print(F("Disabling motors")); Serial.println(axis_label);
Serial.println(axis_str); // targeted motor axis if (do_yaw) motor_disable(AXIS_YAW);
if (target_yaw) motor_power_off(MotorPitch); if (do_pitch) motor_disable(AXIS_PITCH);
if (target_pitch) motor_power_off(MotorYaw);
break; break;
case '*': // turn motors on case '*': // enable coils — motor resists manual movement
Serial.print(F("Turning motors on")); Serial.print(F("Enabling motors")); Serial.println(axis_label);
Serial.println(axis_str); // targeted motor axis if (do_yaw) motor_enable(AXIS_YAW);
if (target_yaw) motor_power_on(MotorPitch); if (do_pitch) motor_enable(AXIS_PITCH);
if (target_pitch) motor_power_on(MotorYaw);
break; break;
case 'm': // move to a specific position in yawl case 'm': // absolute position move: args[0] = target, args[1] = speed
Serial.print(F("Move ")); Serial.print(F("Moving to ")); Serial.print(args[0]);
Serial.print(args[0]); Serial.print(F(" speed ")); Serial.print(args[1]);
Serial.print(F(" with ")); Serial.println(axis_label);
Serial.println(args[1]); if (do_yaw) tmc5160_moveTo(AXIS_YAW, args[0], args[1]);
tmc5160_moveTo(MotorYaw, args[0], args[1]); if (do_pitch) tmc5160_moveTo(AXIS_PITCH, args[0], args[1]);
break; break;
case ';': // change position homing (0x21: XACTUAL register) case ';': // directly overwrite XACTUAL — shifts the coordinate origin without moving
tmc5160_writeRegister(MotorYaw, 0x21, args[0]); Serial.print(F("Setting XACTUAL to ")); Serial.print(args[0]); Serial.println(axis_label);
if (do_yaw) tmc5160_writeRegister(AXIS_YAW, TMC5160_XACTUAL, args[0]);
if (do_pitch) tmc5160_writeRegister(AXIS_PITCH, TMC5160_XACTUAL, args[0]);
break; break;
case 's': // stop motors case 's': // stop — set velocity to zero, motor decelerates on the ramp profile
tmc5160_rotateMotor(MotorPitch, 0); Serial.print(F("Stopping")); Serial.println(axis_label);
tmc5160_rotateMotor(MotorYaw, 0); if (do_yaw) tmc5160_rotateMotor(AXIS_YAW, 0);
Serial.println(F("Stop")); if (do_pitch) tmc5160_rotateMotor(AXIS_PITCH, 0);
break; break;
case 'b': case 'b': // clear the latched deviation warning so the next deviation can be detected
ax_yaw.reset_enc_dev_warn(); Serial.print(F("Clearing encoder deviation flags")); Serial.println(axis_label);
ax_pitch.reset_enc_dev_warn(); if (do_yaw) ax_yaw.clear_enc_deviation();
if (do_pitch) ax_pitch.clear_enc_deviation();
break; break;
case 't': case 't': // snap the step counter to the encoder position, eliminating tracking error
ax_yaw.set_xenc_to_xact(); Serial.print(F("Syncing encoder to actual")); Serial.println(axis_label);
ax_pitch.set_xenc_to_xact(); if (do_yaw) { ax_yaw.clear_enc_deviation(); ax_yaw.sync_encoder_to_actual(); }
if (do_pitch) { ax_pitch.clear_enc_deviation(); ax_pitch.sync_encoder_to_actual(); }
break; break;
case 'e': case 'e': // clear stallguard latch on both axes (no axis specifier supported here)
ax_yaw.reset_SGT(); ax_yaw.clear_stallguard();
ax_pitch.reset_SGT(); ax_pitch.clear_stallguard();
break; break;
case 'w': case 'w': // adjust stallguard sensitivity — lower = more sensitive to stalls
ax_yaw.set_SGT(args[0]); ax_yaw.set_stallguard_threshold(args[0]);
ax_pitch.set_SGT(args[0]); ax_pitch.set_stallguard_threshold(args[0]);
break; break;
case 'u': case 'u': // enter AUTO mode and configure step count (yaw only)
// Requires args[0] > 2 because set_step_size() divides by (n-1);
// anything <= 1 would produce zero or negative step sizes.
if (args[0] > 2) { if (args[0] > 2) {
ax_yaw.set_step_len(args[0]); ax_yaw.set_step_size(args[0]);
ax_yaw.target = ax_yaw.encoderpos_l + 2000; ax_yaw.target = ax_yaw.enc_limit_l + 2000; // start just inside the left limit
ax_yaw.driver_status = GIMBAL_STATUS_AUTO; ax_yaw.status = GIMBAL_STATUS_AUTO;
} }
break; break;
case 'h': case 'h': // save heading calibration — args[0] = axis (0=yaw), args[1] = heading value
if (args[0] == MotorYaw) { if (args[0] == AXIS_YAW) {
ax_yaw.c_hdg = (float)args[1]; ax_yaw.heading = (float)args[1];
ax_yaw.hdg_constant = ax_yaw.X_enc; ax_yaw.heading_origin = ax_yaw.X_enc;
// Store the offset from the left limit rather than the absolute encoder count,
int ee = 0; // so the calibration remains valid after a re-home (which may shift the origin).
int32_t dist = ax_yaw.hdg_constant - ax_yaw.encoderpos_l; int32_t dist = ax_yaw.heading_origin - ax_yaw.enc_limit_l;
EEPROM.put(ee, ax_yaw.c_hdg); EEPROM.put(EEPROM_ADDR_HDG_FLOAT, ax_yaw.heading);
ee += sizeof(float); EEPROM.put(EEPROM_ADDR_HDG_DIST, dist);
EEPROM.put(ee, dist);
} }
break; break;
case 'y': { case 'y': // restore heading calibration from EEPROM — args[0] = axis (0=yaw)
if (args[0] == MotorYaw) { if (args[0] == AXIS_YAW) {
int ee = 0;
int32_t dist = 0; int32_t dist = 0;
EEPROM.get(ee, ax_yaw.c_hdg); EEPROM.get(EEPROM_ADDR_HDG_FLOAT, ax_yaw.heading);
ee += sizeof(float); EEPROM.get(EEPROM_ADDR_HDG_DIST, dist);
EEPROM.get(ee, dist); // Reconstruct heading_origin relative to the current left limit,
ax_yaw.hdg_constant = ax_yaw.encoderpos_l + dist; // making it valid even if the absolute encoder counts shifted after re-homing.
Serial.println(ax_yaw.c_hdg); ax_yaw.heading_origin = ax_yaw.enc_limit_l + dist;
Serial.println(ax_yaw.hdg_constant); Serial.println(ax_yaw.heading);
Serial.println(ax_yaw.heading_origin);
} }
break; break;
}
case 'd': case 'd': // enter MANUAL mode — disables AUTO stepping
ax_yaw.driver_status = GIMBAL_STATUS_MANUAL; ax_yaw.status = GIMBAL_STATUS_MANUAL;
break; break;
case 'p': case 'p': // advance yaw by one step in AUTO mode
ax_yaw.set_xenc_to_xact(); ax_yaw.sync_encoder_to_actual(); // zero tracking error before issuing the next target
ax_yaw.target += ax_yaw.steps; ax_yaw.target += ax_yaw.step_size;
ax_yaw.new_pos = true; ax_yaw.new_pos = true;
break; break;
case 'v': case 'v': // override yaw v_max at runtime
ax_yaw.v_max = args[0]; ax_yaw.v_max = args[0];
break; break;
case '+': // Manual endstop overrides — useful for calibration or testing without homing:
ax_yaw.encoderpos_r = ax_yaw.X_enc; case '+': // mark current yaw encoder position as the right limit
ax_yaw.enc_limit_r = ax_yaw.X_enc;
break; break;
case '-': case '-': // mark current yaw encoder position as the left limit
ax_yaw.encoderpos_l = ax_yaw.X_enc; ax_yaw.enc_limit_l = ax_yaw.X_enc;
break; break;
case '/': case '/': // set both yaw limits explicitly: args[0] = right, args[1] = left
ax_yaw.encoderpos_r = args[0]; ax_yaw.enc_limit_r = args[0];
ax_yaw.encoderpos_l = args[1]; ax_yaw.enc_limit_l = args[1];
Serial.println(ax_yaw.encoderpos_r); Serial.println(ax_yaw.enc_limit_r);
Serial.println(ax_yaw.encoderpos_l); Serial.println(ax_yaw.enc_limit_l);
break; break;
case '?': case '?': // query current yaw endstop positions
Serial.println(ax_yaw.encoderpos_r); Serial.println(ax_yaw.enc_limit_r);
Serial.println(ax_yaw.encoderpos_l); Serial.println(ax_yaw.enc_limit_l);
Serial.println(ax_yaw.encoderpos_l - ax_yaw.encoderpos_r); Serial.println(ax_yaw.enc_limit_l - ax_yaw.enc_limit_r); // negative = right > left (expected)
break; break;
default: default: