From e7301db427e3c527bee14b5758bf50e3f4cabaad Mon Sep 17 00:00:00 2001 From: pgdalmeida Date: Tue, 12 May 2026 13:05:43 +0200 Subject: [PATCH] code re-structuring --- deploy.sh | 4 +- firmware/deploy_firmware.sh | 8 + firmware/src/config/Constants.h | 106 +++++---- firmware/src/hal/SPI_HAL.cpp | 8 +- firmware/src/main.cpp | 39 ++-- firmware/src/motor/Homing.cpp | 265 +++++++++++++--------- firmware/src/motor/Homing.h | 11 +- firmware/src/motor/MotorAxis.cpp | 136 +++++++++--- firmware/src/motor/MotorAxis.h | 142 +++++++----- firmware/src/motor/MotorConfig.h | 30 ++- firmware/src/motor/MotorDriver.cpp | 262 +++++++++++----------- firmware/src/motor/MotorDriver.h | 29 ++- firmware/src/serial/CommandParser.cpp | 306 ++++++++++++++------------ 13 files changed, 818 insertions(+), 528 deletions(-) mode change 100644 => 100755 deploy.sh create mode 100755 firmware/deploy_firmware.sh diff --git a/deploy.sh b/deploy.sh old mode 100644 new mode 100755 index e9d5c3e..fd37653 --- a/deploy.sh +++ b/deploy.sh @@ -1,8 +1,8 @@ #!/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 " # mkdir -p /home/ggs/projects/FireWatchTower_2axis/build && # cd /home/ggs/projects/FireWatchTower_2axis/build && # cmake .. && # make -j\$(nproc) -" \ No newline at end of file + diff --git a/firmware/deploy_firmware.sh b/firmware/deploy_firmware.sh new file mode 100755 index 0000000..939e73d --- /dev/null +++ b/firmware/deploy_firmware.sh @@ -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) + diff --git a/firmware/src/config/Constants.h b/firmware/src/config/Constants.h index ce82b05..8169d59 100644 --- a/firmware/src/config/Constants.h +++ b/firmware/src/config/Constants.h @@ -1,50 +1,80 @@ #ifndef 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 -#define ENC_DEVIATION_LIMIT 1500 -#define ENC_CONST 0x000C1F40 -#define ENCMODE_DECIMAL 0x400 +/* ------------------------------------------------------------------------- + Motor driver — shared by both axes + ------------------------------------------------------------------------- */ +#define ACCELERATION 20000 // counts/s² — used for A1, AMAX, DMAX, D1 (flat ramp profile) +#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 -#define YAW_GEAR_RATIO 739.5555f -#define YAW_GLOBAL_SCALER 80 -#define YAW_DEFAULT_XACTUAL 0x0007A120 -#define YAW_ENDSTOP_SPEED 25000 -#define YAW_DEFAULT_VMAX 50000 +/* ------------------------------------------------------------------------- + Yaw axis + ------------------------------------------------------------------------- */ +#define YAW_STEPS_PER_ROUND 177000 // encoder counts per full mechanical revolution +#define YAW_GEAR_RATIO 739.5555f +#define YAW_GLOBAL_SCALER 80 // scales peak coil current (0–255); lower = less current +#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 -#define PITCH_GEAR_RATIO 739.5555f -#define PITCH_GLOBAL_SCALER 50 -#define PITCH_DEFAULT_XACTUAL 500000 -#define PITCH_ENDSTOP_SPEED 150000 -#define PITCH_DEFAULT_VMAX 250000 +/* ------------------------------------------------------------------------- + Pitch axis + ------------------------------------------------------------------------- */ +#define PITCH_STEPS_PER_ROUND 500000 +#define PITCH_GEAR_RATIO 739.5555f +#define PITCH_GLOBAL_SCALER 50 // pitch motor runs at lower current than yaw +#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 -#define HOMING_XENC_MARGIN 1000 -#define HOMING_DELAY_MS 10 -#define HOMING_PITCH_INITIAL_DELAY 1000 -#define HOMING_YAW_TIMEOUT_MS 2000 +/* ------------------------------------------------------------------------- + Homing + ------------------------------------------------------------------------- */ +// After homing, this many encoder counts are kept as a margin inside each limit +// so normal operation never commands the axis right to the mechanical stop. +#define HOMING_XENC_MARGIN 1000 -/* Thermal */ -#define THERMAL_FAN_ON_TEMP 25 -#define THERMAL_FAN_GAIN 20 -#define THERMAL_FAN_MAX_PWM 255 -#define THERMAL_FAN_PULSE_US 100000 -#define THERMAL_FAN_FREQ_DIV 0.004 -#define THERMAL_LOOP_INTERVAL 250 +// If the measured travel between endstops is less than +// (steps_per_round - HOMING_MIN_ROUND_DIFF_OFFSET), homing is declared failed. +// The offset accounts for the margin on each side plus mechanical tolerance. +#define HOMING_MIN_ROUND_DIFF_OFFSET 6000 -/* Info print interval */ -#define INFO_PRINT_INTERVAL_MS 250 +#define HOMING_DELAY_MS 10 // settle time after an endstop fires before reading XLATCH +#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 diff --git a/firmware/src/hal/SPI_HAL.cpp b/firmware/src/hal/SPI_HAL.cpp index 661af8f..aa47633 100644 --- a/firmware/src/hal/SPI_HAL.cpp +++ b/firmware/src/hal/SPI_HAL.cpp @@ -8,9 +8,9 @@ void tmc5160_readWriteSPI(uint16_t icID, uint8_t *data, size_t dataLength) { uint8_t input[dataLength]; - if (icID == MotorYaw) + if (icID == AXIS_YAW) digitalWrite(PIN_TMC_CS_YAW, LOW); - if (icID == MotorPitch) + if (icID == AXIS_PITCH) digitalWrite(PIN_TMC_CS_PITCH, LOW); delayMicroseconds(1); @@ -20,9 +20,9 @@ void tmc5160_readWriteSPI(uint16_t icID, uint8_t *data, size_t dataLength) { } delayMicroseconds(1); - if (icID == MotorYaw) + if (icID == AXIS_YAW) digitalWrite(PIN_TMC_CS_YAW, HIGH); - if (icID == MotorPitch) + if (icID == AXIS_PITCH) digitalWrite(PIN_TMC_CS_PITCH, HIGH); } diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 805c1a8..303b9fa 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -5,33 +5,42 @@ #include "serial/CommandParser.h" #include "thermal/ThermalManager.h" -static unsigned long s_last_loop = 0; +static unsigned long s_last_print = 0; void setup() { Serial.begin(BAUD_RATE); - delay(50); + delay(50); // brief settle before sending — avoids garbled first bytes on some boards Serial.println(F("Starting...")); - init_motor_hardware(); - init_motor_params(); - thermal_init(); + motor_hw_init(); // configure SPI peripheral and CS/EN pins + motor_regs_init(); // write TMC5160 register configuration for both axes + thermal_init(); // start the DHT sensor - s_last_loop = millis(); + s_last_print = millis(); } void loop() { - motor_periodic_job(MotorYaw); - motor_periodic_job(MotorPitch); - endstop_routine(); + // Refresh cached position registers every iteration so all other code sees + // fresh values without triggering redundant SPI reads themselves. + 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()) { - read_command() + read_command(); } - if (millis() - s_last_loop > INFO_PRINT_INTERVAL_MS) { - s_last_loop = millis(); - print_motor_info(MotorPitch); - print_motor_info(MotorYaw); + // Print status and run the thermal fan controller on a slow timer. + // Thermal is piggy-backed here rather than its own timer because the DHT11 + // samples at ~1 Hz, so there is no benefit running it faster than the print interval. + 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(); } -} +} \ No newline at end of file diff --git a/firmware/src/motor/Homing.cpp b/firmware/src/motor/Homing.cpp index 9085263..919743c 100644 --- a/firmware/src/motor/Homing.cpp +++ b/firmware/src/motor/Homing.cpp @@ -5,129 +5,190 @@ #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) { - ax_yaw.set_xenc_to_xact(); - ax_yaw.reset_enc_dev_warn(); - ax_yaw.driver_status = GIMBAL_STATUS_ENC_INIT; - ax_yaw.set_speed(30000); - ax_yaw.set_SGT(60); - ax_yaw.move_endstop_right = true; - tmc5160_rotateMotor(MotorYaw, YAW_ENDSTOP_SPEED); +// --------------------------------------------------------------------------- +// do_homing_start() +// +// Prepares an axis for homing and starts it moving toward the right endstop. +// +// Yaw: relies on encoder deviation detection (stall against a mechanical stop). +// No physical switches — the deviation warning fires when the motor stalls. +// +// 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) { - if (ax_pitch.get_status_endswitch_l()) { - tmc5160_rotateMotor(MotorPitch, PITCH_ENDSTOP_SPEED); + else if (axis == AXIS_PITCH) { + // Back off if already on an endstop, then let the motor settle before + // 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); } - else if (ax_pitch.get_status_endswitch_r()) { - tmc5160_rotateMotor(MotorPitch, -PITCH_ENDSTOP_SPEED); + else if (ax_pitch.endswitch_r()) { + tmc5160_rotateMotor(AXIS_PITCH, -PITCH_ENDSTOP_SPEED); delay(HOMING_PITCH_INITIAL_DELAY); } - ax_pitch.set_xenc_to_xact(); - ax_pitch.reset_enc_dev_warn(); - ax_pitch.driver_status = GIMBAL_STATUS_ENC_INIT; - ax_pitch.set_speed(30000); - ax_pitch.set_SGT(60); - ax_pitch.move_endstop_right = true; - tmc5160_rotateMotor(MotorPitch, PITCH_ENDSTOP_SPEED); + ax_pitch.sync_encoder_to_actual(); + ax_pitch.clear_enc_deviation(); + ax_pitch.status = GIMBAL_STATUS_ENC_INIT; + ax_pitch.seek_right = true; + ax_pitch.set_speed(HOMING_PITCH_SPEED); + ax_pitch.set_stallguard_threshold(HOMING_SGT_THRESHOLD); + tmc5160_rotateMotor(AXIS_PITCH, PITCH_ENDSTOP_SPEED); } } -static void yaw_homing(void) { - if (ax_yaw.driver_status == GIMBAL_STATUS_ENC_INIT - && millis() - s_tick_move_yaw > HOMING_YAW_TIMEOUT_MS - && !ax_yaw.move_endstop_right) { - s_tick_move_yaw = millis(); - tmc5160_rotateMotor(MotorYaw, -YAW_ENDSTOP_SPEED); +// --------------------------------------------------------------------------- +// homing_complete() (internal) +// +// Called once both limits have been found. Validates the measured travel range, +// updates axis state, and commands a move to the centre of travel. +// +// 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) { - if (ax_yaw.get_dev_warn_status()) { - tmc5160_rotateMotor(MotorYaw, 0); + ax.status = GIMBAL_STATUS_INITIALIZED; + ax.act_steps_per_round = ax.enc_limit_r - ax.enc_limit_l; + + 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(); - Serial.println(ax_yaw.X_enc); - Serial.println(ax_yaw.X_act); - Serial.println(ax_yaw.X_err); - ax_yaw.encoderpos_r = ax_yaw.X_enc; - ax_yaw.set_xenc_to_xact(); - 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; + ax_yaw.enc_limit_r = ax_yaw.X_enc; + ax_yaw.sync_encoder_to_actual(); // reset step counter so left pass starts from a clean origin + ax_yaw.clear_enc_deviation(); + ax_yaw.seek_right = false; + s_yaw_homing_tick = millis(); // start the deceleration timeout - if (abs(ax_yaw.encoderpos_l - ax_yaw.encoderpos_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); - } + Serial.print(F("Yaw right limit: ")); Serial.println(ax_yaw.enc_limit_r); } + 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) { - if (ax_pitch.get_status_endswitch_r()) { +// --------------------------------------------------------------------------- +// pitch_homing_loop() (internal) +// +// 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(); - Serial.println(ax_pitch.X_enc); - Serial.println(ax_pitch.X_act); - Serial.println(ax_pitch.X_err); - delay(HOMING_DELAY_MS); - ax_pitch.encoderpos_r = ax_pitch.get_X_Latch(); - 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; + delay(HOMING_DELAY_MS); // brief pause so XLATCH settles before reading + ax_pitch.enc_limit_r = ax_pitch.get_x_latch(); + ax_pitch.clear_enc_deviation(); + ax_pitch.seek_right = false; + tmc5160_rotateMotor(AXIS_PITCH, -PITCH_ENDSTOP_SPEED); // reverse toward left switch - if (abs(ax_pitch.encoderpos_l - ax_pitch.encoderpos_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); - } + Serial.print(F("Pitch right limit: ")); Serial.println(ax_pitch.enc_limit_r); } + 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(); - pitch_homing(); +// --------------------------------------------------------------------------- +// homing_routine() — public, called every loop from main.cpp +// --------------------------------------------------------------------------- + +void homing_routine(void) { + yaw_homing_loop(); + pitch_homing_loop(); } diff --git a/firmware/src/motor/Homing.h b/firmware/src/motor/Homing.h index a9803f5..3713247 100644 --- a/firmware/src/motor/Homing.h +++ b/firmware/src/motor/Homing.h @@ -4,7 +4,14 @@ #include #include "motor/MotorConfig.h" -void do_endstop(uint16_t axis); -void endstop_routine(void); +// Kick off the homing sequence for one axis. +// 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 diff --git a/firmware/src/motor/MotorAxis.cpp b/firmware/src/motor/MotorAxis.cpp index 5b004c8..2297661 100644 --- a/firmware/src/motor/MotorAxis.cpp +++ b/firmware/src/motor/MotorAxis.cpp @@ -1,68 +1,134 @@ #include "motor/MotorAxis.h" #include "config/Constants.h" -MotorAxisParam ax_yaw(MotorYaw, YAW_STEPS_PER_ROUND, YAW_GEAR_RATIO, YAW_DEFAULT_VMAX); -MotorAxisParam ax_pitch(MotorPitch, PITCH_STEPS_PER_ROUND, PITCH_GEAR_RATIO, PITCH_DEFAULT_VMAX); +// One instance per physical axis. These are the only two MotorAxis objects in +// 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() { - return tmc5160_fieldRead(MOTOR_ID, TMC5160_STST_FIELD); +MotorAxis::MotorAxis(uint16_t _id, int32_t _steps_per_round, float _gear_ratio, int32_t _default_vmax) + : 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() { - return tmc5160_fieldRead(MOTOR_ID, TMC5160_STATUS_STOP_R_FIELD); +bool MotorAxis::endswitch_l() { + return tmc5160_fieldRead(id, TMC5160_STATUS_STOP_L_FIELD) != 0; } -uint32_t MotorAxisParam::get_X_Latch() { - return tmc5160_readRegister(MOTOR_ID, TMC5160_XLATCH); +bool MotorAxis::endswitch_r() { + return tmc5160_fieldRead(id, TMC5160_STATUS_STOP_R_FIELD) != 0; } -uint32_t MotorAxisParam::get_SGT_Status() { - return tmc5160_fieldRead(MOTOR_ID, TMC5160_EVENT_STOP_SG_FIELD); +// XLATCH captures XACTUAL at the moment an endstop input transitions. +// 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() { - return tmc5160_fieldRead(MOTOR_ID, TMC5160_DEVIATION_WARN_FIELD); +bool MotorAxis::stallguard_triggered() { + // 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() { - return tmc5160_readRegister(MOTOR_ID, TMC5160_ENC_DEVIATION); +bool MotorAxis::enc_deviation_triggered() { + // 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) { - tmc5160_fieldWrite(MOTOR_ID, TMC5160_SGT_FIELD, sgt); +int32_t MotorAxis::enc_deviation_value() { + 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() { - tmc5160_fieldWrite(MOTOR_ID, TMC5160_DEVIATION_WARN_FIELD, 1); +// SGT (stallguard threshold) controls how sensitive stall detection is. +// 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) { - tmc5160_fieldWrite(MOTOR_ID, TMC5160_ENC_DEVIATION_FIELD, limit); +void MotorAxis::set_enc_deviation_limit(int32_t limit) { + tmc5160_fieldWrite(id, TMC5160_ENC_DEVIATION_FIELD, limit); } -void MotorAxisParam::set_xenc_to_xact() { - tmc5160_writeRegister(MOTOR_ID, TMC5160_XACTUAL, X_enc); +// Divides the usable travel range (endstop-to-endstop minus margins) into +// 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) { - tmc5160_writeRegister(MOTOR_ID, TMC5160_VMAX, speed); +// Overwrites XACTUAL with the current encoder reading, snapping the step counter +// 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); - steps = turn_len / (s - 1); +// --------------------------------------------------------------------------- +// Latched flag clears +// --------------------------------------------------------------------------- + +// 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() { - X_enc = tmc5160_readRegister(MOTOR_ID, TMC5160_XENC); - X_act = tmc5160_readRegister(MOTOR_ID, TMC5160_XACTUAL); - X_err = X_act - X_enc; +void MotorAxis::clear_enc_deviation() { + tmc5160_fieldWrite(id, TMC5160_DEVIATION_WARN_FIELD, 1); } diff --git a/firmware/src/motor/MotorAxis.h b/firmware/src/motor/MotorAxis.h index 2484411..37e1259 100644 --- a/firmware/src/motor/MotorAxis.h +++ b/firmware/src/motor/MotorAxis.h @@ -3,66 +3,100 @@ #include #include "motor/MotorConfig.h" - #include "TMC5160.h" -struct MotorAxisParam { - const uint16_t MOTOR_ID; - const int32_t one_round_in_steps; - const float gear_red_var; - int32_t act_steps_per_round; - int32_t encoderpos_l; - int32_t encoderpos_r; - int32_t steps; - float c_hdg; - int32_t hdg_constant; - int32_t target; - int32_t X_act; - int32_t X_enc; - int32_t X_err; - int32_t v_max; - gimbal_status driver_status; - bool move_endstop_right; - bool new_pos; +// --------------------------------------------------------------------------- +// MotorAxis +// +// Owns all runtime state for one physical axis and wraps every TMC5160 +// register access behind a named method. No code outside this struct should +// call tmc5160_readRegister / tmc5160_fieldRead with motor-specific addresses. +// +// Method naming convention: +// is_*() / *_triggered() — reads a status bit; no side effects +// get_*() — reads a value register; no side effects +// set_*() — writes a configuration register +// clear_*() — resets a latched hardware flag (write-to-clear) +// sync_*() — aligns one hardware register to another +// update() — refreshes the local position cache from hardware +// --------------------------------------------------------------------------- - MotorAxisParam(uint16_t id, int32_t steps, float ratio, int32_t vmax) - : 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) - {} +struct MotorAxis { - uint32_t get_standstill_status(); - bool get_status_endswitch_l(); - bool get_status_endswitch_r(); - uint32_t get_X_Latch(); - uint32_t get_SGT_Status(); - uint32_t get_dev_warn_status(); - int32_t get_dev_value(); - void set_SGT(uint8_t sgt); - void reset_SGT(); - void reset_enc_dev_warn(); - void set_enc_dev_warn_limit(int32_t limit); - void set_xenc_to_xact(); - void set_speed(int32_t speed); - void set_step_len(int32_t s); + // ----------------------------------------------------------------------- + // Fixed parameters — set in the constructor, never changed at runtime + // ----------------------------------------------------------------------- + const uint16_t id; // TMC5160 chip ID passed to every API call (AXIS_YAW / AXIS_PITCH) + const int32_t steps_per_round; // nominal encoder counts per full mechanical revolution + const float gear_ratio; // gear reduction between motor shaft and output + const int32_t default_vmax; // speed applied on init and after reset + + // ----------------------------------------------------------------------- + // Homing results — written once by Homing.cpp, then treated as read-only + // ----------------------------------------------------------------------- + int32_t enc_limit_l; // encoder count where the left endstop was detected + int32_t enc_limit_r; // encoder count where the right endstop was detected + 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(); + + // --- 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; -extern MotorAxisParam ax_pitch; +// Global axis instances — defined in MotorAxis.cpp +extern MotorAxis ax_yaw; +extern MotorAxis ax_pitch; #endif diff --git a/firmware/src/motor/MotorConfig.h b/firmware/src/motor/MotorConfig.h index 217ae47..9e0986a 100644 --- a/firmware/src/motor/MotorConfig.h +++ b/firmware/src/motor/MotorConfig.h @@ -3,17 +3,29 @@ #include -#define MotorYaw 0x00 -#define MotorPitch 0x01 +// Axis IDs match the TMC5160 chip-select assignments in SPI_HAL.cpp. +// 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 { - GIMBAL_STATUS_ERROR = -2, - GIMBAL_STATUS_BOOT = -1, - GIMBAL_STATUS_RESET = 0, - GIMBAL_STATUS_ENC_INIT = 1, - GIMBAL_STATUS_INITIALIZED= 2, - GIMBAL_STATUS_AUTO = 3, - GIMBAL_STATUS_MANUAL = 4 + GIMBAL_STATUS_ERROR = -2, // homing failed or hardware fault + GIMBAL_STATUS_BOOT = -1, // initial power-on state, hardware not yet configured + GIMBAL_STATUS_RESET = 0, // registers written, motor enabled, ready to home + GIMBAL_STATUS_ENC_INIT = 1, // homing sequence in progress + GIMBAL_STATUS_INITIALIZED = 2, // homing complete, endstop limits known + GIMBAL_STATUS_AUTO = 3, // stepping through positions automatically + GIMBAL_STATUS_MANUAL = 4 // responding to direct move commands } gimbal_status; #endif diff --git a/firmware/src/motor/MotorDriver.cpp b/firmware/src/motor/MotorDriver.cpp index 835d47d..201845d 100644 --- a/firmware/src/motor/MotorDriver.cpp +++ b/firmware/src/motor/MotorDriver.cpp @@ -6,151 +6,167 @@ #include #include "TMC5160.h" -static void write_regs_yaw(void) { - tmc5160_writeRegister(MotorYaw, 0x00, 0x0000000C); - tmc5160_writeRegister(MotorYaw, 0x09, 0x00010606); - tmc5160_writeRegister(MotorYaw, 0x0A, 0x00080400); - tmc5160_writeRegister(MotorYaw, 0x0B, YAW_GLOBAL_SCALER); - tmc5160_writeRegister(MotorYaw, 0x10, 0x00071F08); - tmc5160_writeRegister(MotorYaw, 0x11, 0x00000008); - tmc5160_writeRegister(MotorYaw, 0x13, 0); - tmc5160_writeRegister(MotorYaw, 0x14, 0); - tmc5160_writeRegister(MotorYaw, 0x15, 0); - tmc5160_writeRegister(MotorYaw, 0x21, YAW_DEFAULT_XACTUAL); - tmc5160_writeRegister(MotorYaw, 0x24, ACCELERATION); - tmc5160_writeRegister(MotorYaw, 0x26, ACCELERATION); - tmc5160_writeRegister(MotorYaw, 0x25, 0x00000000); - tmc5160_writeRegister(MotorYaw, 0x27, 0); - tmc5160_writeRegister(MotorYaw, 0x28, ACCELERATION); - tmc5160_writeRegister(MotorYaw, 0x2A, ACCELERATION); - tmc5160_writeRegister(MotorYaw, 0x2B, 0x00000100); - tmc5160_writeRegister(MotorYaw, 0x2D, 500000); - tmc5160_writeRegister(MotorYaw, 0x34, 0x08A3); - tmc5160_writeRegister(MotorYaw, 0x35, 0xFFFF); - tmc5160_writeRegister(MotorYaw, 0x38, ENCMODE_DECIMAL); - tmc5160_writeRegister(MotorYaw, 0x39, 500000); - tmc5160_writeRegister(MotorYaw, 0x3A, ENC_CONST); - tmc5160_writeRegister(MotorYaw, 0x3D, ENC_DEVIATION_LIMIT); - tmc5160_writeRegister(MotorYaw, 0x6C, 0x00410043); - tmc5160_writeRegister(MotorYaw, 0x6D, 0x00000000); - tmc5160_writeRegister(MotorYaw, 0x70, 0x010C1E3C); - tmc5160_writeRegister(MotorYaw, TMC5160_RAMPMODE, TMC5160_MODE_HOLD); +// --------------------------------------------------------------------------- +// Register initialisation +// +// Each write is annotated with the TMC5160 register name so the intent is +// readable without cross-referencing the datasheet. Values that come from +// Constants.h are named; the rest are one-off configuration words that don't +// need a named constant because they are never used anywhere else. +// +// Both functions follow the same structure; only the axis ID and a handful +// of tuning values (GLOBALSCALER, XACTUAL) differ between yaw and pitch. +// --------------------------------------------------------------------------- + +void motor_regs_init_yaw(void) { + tmc5160_writeRegister(AXIS_YAW, 0x00, 0x0000000C); // GCONF: default global config + tmc5160_writeRegister(AXIS_YAW, 0x09, 0x00010606); // SHORTCONF: short-circuit protection timings + tmc5160_writeRegister(AXIS_YAW, 0x0A, 0x00080400); // DRVCONF: driver output configuration + tmc5160_writeRegister(AXIS_YAW, 0x0B, YAW_GLOBAL_SCALER); // GLOBALSCALER: scales peak motor current + tmc5160_writeRegister(AXIS_YAW, 0x10, 0x00071F08); // IHOLD_IRUN: hold current | run current | transition delay + tmc5160_writeRegister(AXIS_YAW, 0x11, 0x00000008); // TPOWERDOWN: delay before reducing to hold current after standstill + tmc5160_writeRegister(AXIS_YAW, 0x13, 0); // TPWMTHRS: upper velocity for stealthChop — 0 = always stealthChop + tmc5160_writeRegister(AXIS_YAW, 0x14, 0); // TCOOLTHRS: lower velocity for coolStep / stallGuard — 0 = disabled + tmc5160_writeRegister(AXIS_YAW, 0x15, 0); // THIGH: upper velocity threshold for high-speed mode — 0 = disabled + tmc5160_writeRegister(AXIS_YAW, 0x21, YAW_DEFAULT_XACTUAL); // XACTUAL: seed the internal position counter + tmc5160_writeRegister(AXIS_YAW, 0x24, ACCELERATION); // A1: first acceleration (flat ramp profile — same value as AMAX) + tmc5160_writeRegister(AXIS_YAW, 0x25, 0); // V1: velocity threshold between A1 and AMAX — 0 disables two-stage ramp + tmc5160_writeRegister(AXIS_YAW, 0x26, ACCELERATION); // AMAX: maximum acceleration + tmc5160_writeRegister(AXIS_YAW, 0x27, 0); // VMAX (RAMPMODE=0 only): not used in HOLD mode + tmc5160_writeRegister(AXIS_YAW, 0x28, ACCELERATION); // DMAX: maximum deceleration + 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) { - tmc5160_writeRegister(MotorPitch, 0x00, 0x0000000C); - tmc5160_writeRegister(MotorPitch, 0x09, 0x00010606); - tmc5160_writeRegister(MotorPitch, 0x0A, 0x00080400); - tmc5160_writeRegister(MotorPitch, 0x0B, PITCH_GLOBAL_SCALER); - tmc5160_writeRegister(MotorPitch, 0x10, 0x00071F08); - tmc5160_writeRegister(MotorPitch, 0x11, 0x00000008); - tmc5160_writeRegister(MotorPitch, 0x13, 0); - tmc5160_writeRegister(MotorPitch, 0x14, 0); - tmc5160_writeRegister(MotorPitch, 0x15, 0); - tmc5160_writeRegister(MotorPitch, 0x21, PITCH_DEFAULT_XACTUAL); - tmc5160_writeRegister(MotorPitch, 0x24, ACCELERATION); - tmc5160_writeRegister(MotorPitch, 0x26, ACCELERATION); - tmc5160_writeRegister(MotorPitch, 0x25, 0x00000000); - tmc5160_writeRegister(MotorPitch, 0x27, 0); - tmc5160_writeRegister(MotorPitch, 0x28, ACCELERATION); - tmc5160_writeRegister(MotorPitch, 0x2A, ACCELERATION); - tmc5160_writeRegister(MotorPitch, 0x2B, 0x00000100); - tmc5160_writeRegister(MotorPitch, 0x2D, 500000); - tmc5160_writeRegister(MotorPitch, 0x34, 0x08A3); - tmc5160_writeRegister(MotorPitch, 0x35, 0xFFFF); - tmc5160_writeRegister(MotorPitch, 0x38, ENCMODE_DECIMAL); - tmc5160_writeRegister(MotorPitch, 0x39, 500000); - tmc5160_writeRegister(MotorPitch, 0x3A, ENC_CONST); - tmc5160_writeRegister(MotorPitch, 0x3D, ENC_DEVIATION_LIMIT); - tmc5160_writeRegister(MotorPitch, 0x6C, 0x00410043); - tmc5160_writeRegister(MotorPitch, 0x6D, 0x00000000); - tmc5160_writeRegister(MotorPitch, 0x70, 0x010C1E3C); - tmc5160_writeRegister(MotorPitch, TMC5160_RAMPMODE, TMC5160_MODE_HOLD); +void motor_regs_init_pitch(void) { + tmc5160_writeRegister(AXIS_PITCH, 0x00, 0x0000000C); // GCONF + tmc5160_writeRegister(AXIS_PITCH, 0x09, 0x00010606); // SHORTCONF + tmc5160_writeRegister(AXIS_PITCH, 0x0A, 0x00080400); // DRVCONF + tmc5160_writeRegister(AXIS_PITCH, 0x0B, PITCH_GLOBAL_SCALER); // GLOBALSCALER + tmc5160_writeRegister(AXIS_PITCH, 0x10, 0x00071F08); // IHOLD_IRUN + tmc5160_writeRegister(AXIS_PITCH, 0x11, 0x00000008); // TPOWERDOWN + tmc5160_writeRegister(AXIS_PITCH, 0x13, 0); // TPWMTHRS — stealthChop threshold disabled + tmc5160_writeRegister(AXIS_PITCH, 0x14, 0); // TCOOLTHRS — coolStep threshold disabled + tmc5160_writeRegister(AXIS_PITCH, 0x15, 0); // THIGH — high-speed threshold disabled + tmc5160_writeRegister(AXIS_PITCH, 0x21, PITCH_DEFAULT_XACTUAL); // XACTUAL + tmc5160_writeRegister(AXIS_PITCH, 0x24, ACCELERATION); // A1 + tmc5160_writeRegister(AXIS_PITCH, 0x25, 0); // V1 — two-stage ramp disabled + tmc5160_writeRegister(AXIS_PITCH, 0x26, ACCELERATION); // AMAX + tmc5160_writeRegister(AXIS_PITCH, 0x27, 0); // VMAX (unused in HOLD mode) + tmc5160_writeRegister(AXIS_PITCH, 0x28, ACCELERATION); // DMAX + tmc5160_writeRegister(AXIS_PITCH, 0x2A, ACCELERATION); // D1 + tmc5160_writeRegister(AXIS_PITCH, 0x2B, 0x00000100); // VSTOP + tmc5160_writeRegister(AXIS_PITCH, 0x2D, 500000); // XTARGET + tmc5160_writeRegister(AXIS_PITCH, 0x34, 0x08A3); // SW_MODE + tmc5160_writeRegister(AXIS_PITCH, 0x35, 0xFFFF); // RAMP_STAT — clear stale flags + tmc5160_writeRegister(AXIS_PITCH, 0x38, ENCMODE_DECIMAL); // ENCMODE + tmc5160_writeRegister(AXIS_PITCH, 0x39, 500000); // X_ENC + tmc5160_writeRegister(AXIS_PITCH, 0x3A, ENC_CONST); // ENC_CONST + tmc5160_writeRegister(AXIS_PITCH, 0x3D, ENC_DEVIATION_LIMIT); // ENC_DEVIATION + tmc5160_writeRegister(AXIS_PITCH, 0x6C, 0x00410043); // CHOPCONF + tmc5160_writeRegister(AXIS_PITCH, 0x6D, 0x00000000); // COOLCONF — disabled + tmc5160_writeRegister(AXIS_PITCH, 0x70, 0x010C1E3C); // PWMCONF + tmc5160_writeRegister(AXIS_PITCH, TMC5160_RAMPMODE, TMC5160_MODE_HOLD); } -void init_motor_hardware(void) { - 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); +// --------------------------------------------------------------------------- +// Hardware initialisation — call once in setup(), before any register writes +// --------------------------------------------------------------------------- +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.setClockDivider(SPI_CLOCK_DIV); SPI.setDataMode(SPI_MODE3); 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(); } -void init_motor_params(void) { - write_regs_yaw(); - write_regs_pitch(); +void motor_regs_init(void) { + motor_regs_init_yaw(); + motor_regs_init_pitch(); } -void motor_power_on(uint16_t axis) { - if (axis == MotorPitch) { - digitalWrite(PIN_TMC_EN_PITCH, LOW); - } else if (axis == MotorYaw) { - digitalWrite(PIN_TMC_EN_YAW, LOW); - } +// --------------------------------------------------------------------------- +// Enable / disable +// +// The EN pin is active-low: LOW energises the coils (holding torque active), +// 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) { - if (axis == MotorPitch) { - digitalWrite(PIN_TMC_EN_PITCH, HIGH); - } else if (axis == MotorYaw) { - digitalWrite(PIN_TMC_EN_YAW, HIGH); - } +void motor_disable(uint16_t axis) { + if (axis == AXIS_YAW) digitalWrite(PIN_TMC_EN_YAW, HIGH); + else if (axis == AXIS_PITCH) digitalWrite(PIN_TMC_EN_PITCH, HIGH); } -void motor_periodic_job(uint16_t axis) { - if (axis == MotorYaw) { - ax_yaw.update(); - } else if (axis == MotorPitch) { - ax_pitch.update(); - } +// --------------------------------------------------------------------------- +// Loop jobs +// --------------------------------------------------------------------------- + +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) { - const char *spacer = ", "; - if (axis == MotorPitch) { +// Prints a comma-separated diagnostic line for one axis. +// +// 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(ax_pitch.X_enc); - Serial.print(spacer); - Serial.print(ax_pitch.X_act); - Serial.print(spacer); - Serial.print(ax_pitch.X_err); - Serial.print(spacer); - Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_DEVIATION_WARN_FIELD)); - Serial.print(spacer); - Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_DEVIATION_WARN_FIELD)); - Serial.print(spacer); - Serial.print(tmc5160_fieldRead(MotorPitch, TMC5160_STATUS_SG_FIELD)); - 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(ax_pitch.X_enc); Serial.print(sep); + Serial.print(ax_pitch.X_act); Serial.print(sep); + Serial.print(ax_pitch.X_err); Serial.print(sep); + Serial.print(ax_pitch.enc_deviation_triggered()); Serial.print(sep); + Serial.print(ax_pitch.stallguard_triggered()); Serial.print(sep); + Serial.print(tmc5160_fieldRead(AXIS_PITCH, TMC5160_STATUS_SG_FIELD)); Serial.print(sep); + Serial.print(tmc5160_fieldRead(AXIS_PITCH, TMC5160_SG_STOP_FIELD)); Serial.print(sep); + Serial.print(ax_pitch.endswitch_l()); Serial.print(sep); + Serial.println(ax_pitch.endswitch_r()); + + } else if (axis == AXIS_YAW) { Serial.print(F("YAW: ")); - Serial.print(ax_yaw.X_enc); - Serial.print(spacer); - Serial.print(ax_yaw.X_act); - Serial.print(spacer); - Serial.print(ax_yaw.X_err); - Serial.print(spacer); - Serial.print(tmc5160_fieldRead(MotorYaw, TMC5160_DEVIATION_WARN_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)); + Serial.print(ax_yaw.X_enc); Serial.print(sep); + Serial.print(ax_yaw.X_act); Serial.print(sep); + Serial.print(ax_yaw.X_err); Serial.print(sep); + Serial.print(ax_yaw.enc_deviation_triggered()); Serial.print(sep); + Serial.print(ax_yaw.stallguard_triggered()); Serial.print(sep); + Serial.print(tmc5160_fieldRead(AXIS_YAW, TMC5160_STATUS_SG_FIELD)); Serial.print(sep); + Serial.println(tmc5160_fieldRead(AXIS_YAW, TMC5160_SG_STOP_FIELD)); } } diff --git a/firmware/src/motor/MotorDriver.h b/firmware/src/motor/MotorDriver.h index c9ff316..0b75363 100644 --- a/firmware/src/motor/MotorDriver.h +++ b/firmware/src/motor/MotorDriver.h @@ -4,11 +4,28 @@ #include #include "motor/MotorConfig.h" -void init_motor_hardware(void); -void init_motor_params(void); -void motor_power_on(uint16_t axis); -void motor_power_off(uint16_t axis); -void motor_periodic_job(uint16_t axis); -void print_motor_info(uint16_t axis); +// One-time hardware setup: SPI peripheral + CS/EN pin directions. +// Call before any SPI communication. +void motor_hw_init(void); + +// Write the full TMC5160 register configuration to both axes. +// 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 diff --git a/firmware/src/serial/CommandParser.cpp b/firmware/src/serial/CommandParser.cpp index f518e03..8300e49 100644 --- a/firmware/src/serial/CommandParser.cpp +++ b/firmware/src/serial/CommandParser.cpp @@ -8,202 +8,232 @@ #include "TMC5160.h" -void read_command() { - // Command format: [y|p][d],,... - // Axis specifier: y=Yaw only, p=Pitch only - // Base specifier: d=decimal (default hex) +// --------------------------------------------------------------------------- +// Serial command protocol +// +// Format: [y|p] [d] , , ... +// +// 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] , +// ; overwrite XACTUAL register directly: ; +// 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 +// u enter AUTO stepping mode: u (must be > 2) +// h save heading calibration to EEPROM: h 0, +// 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 +// + set yaw right endstop to current encoder position +// - set yaw left endstop to current encoder position +// / set both yaw endstops manually: / , +// ? print current yaw endstop positions +// --------------------------------------------------------------------------- - // Get first character of recieved command - char c = Serial.read(); - - // Parse optional axis specifier: y (yaw) or p (pitch) - uint8_t target_yaw = 1; - uint8_t target_pitch = 1; - const __FlashStringHelper* axis_str; // string to hold the suffix for terminal messages - if (Serial.peek() == 'y') { - Serial.read(); - target_pitch = 0; - axis_str = F(" [YAW]"); - } - else if (Serial.peek() == 'p') { - Serial.read(); - target_yaw = 0; - axis_str = F(" [PITCH]") - } - else { - axis_str = F(" [YAW+PITCH]"); - } - - // Check if a "d" follows, for decimal values instead of hex - uint8_t base = 16; - if (Serial.peek() == 'd') { +// Peek at the next byte to check for an axis specifier ('y' or 'p'). +// 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) { + out_yaw = 1; + out_pitch = 1; + if (Serial.peek() == 'y') { Serial.read(); - base = 10; + out_pitch = 0; + return F(" [YAW]"); } + if (Serial.peek() == 'p') { + Serial.read(); + out_yaw = 0; + return F(" [PITCH]"); + } + return F(" [YAW + PITCH]"); +} - // 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 +// 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; + if (Serial.peek() == 'd') { Serial.read(); base = 10; } int32_t args[4] = {0}; - uint8_t argc = 0; - - char *token = strtok(buffer, ","); - while (token && argc < 4) { - args[argc++] = strtol(token, nullptr, base); - token = strtok(nullptr, ","); - } + parse_args(args, base); switch (c) { - - case 'r': // power cycle the motors - Serial.print(F("Reseting")); - Serial.println(axis_str); // targeted motor axis - 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; - } + case 'r': // re-init registers and re-enable — useful after a fault or power issue + Serial.print(F("Resetting")); Serial.println(axis_label); + 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; } break; - case 'q': // find endstop - Serial.print(F("Finding endstops")); - Serial.println(axis_str); // targeted motor axis - if (target_yaw) do_endstop(MotorYaw); - if (target_pitch) do_endstop(MotorPitch); + case 'q': // start homing — axis must be enabled first + Serial.print(F("Homing")); Serial.println(axis_label); + if (do_yaw) do_homing_start(AXIS_YAW); + if (do_pitch) do_homing_start(AXIS_PITCH); break; - case '!': // turn motors off - Serial.print(F("Turning motors off")); - Serial.println(axis_str); // targeted motor axis - if (target_yaw) motor_power_off(MotorPitch); - if (target_pitch) motor_power_off(MotorYaw); + case '!': // disable coils — motor can be moved by hand + Serial.print(F("Disabling motors")); Serial.println(axis_label); + if (do_yaw) motor_disable(AXIS_YAW); + if (do_pitch) motor_disable(AXIS_PITCH); break; - case '*': // turn motors on - Serial.print(F("Turning motors on")); - Serial.println(axis_str); // targeted motor axis - if (target_yaw) motor_power_on(MotorPitch); - if (target_pitch) motor_power_on(MotorYaw); + case '*': // enable coils — motor resists manual movement + Serial.print(F("Enabling motors")); Serial.println(axis_label); + if (do_yaw) motor_enable(AXIS_YAW); + if (do_pitch) motor_enable(AXIS_PITCH); break; - case 'm': // move to a specific position in yawl - Serial.print(F("Move ")); - Serial.print(args[0]); - Serial.print(F(" with ")); - Serial.println(args[1]); - tmc5160_moveTo(MotorYaw, args[0], args[1]); + case 'm': // absolute position move: args[0] = target, args[1] = speed + Serial.print(F("Moving to ")); Serial.print(args[0]); + Serial.print(F(" speed ")); Serial.print(args[1]); + Serial.println(axis_label); + if (do_yaw) tmc5160_moveTo(AXIS_YAW, args[0], args[1]); + if (do_pitch) tmc5160_moveTo(AXIS_PITCH, args[0], args[1]); break; - case ';': // change position homing (0x21: XACTUAL register) - tmc5160_writeRegister(MotorYaw, 0x21, args[0]); + case ';': // directly overwrite XACTUAL — shifts the coordinate origin without moving + 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; - case 's': // stop motors - tmc5160_rotateMotor(MotorPitch, 0); - tmc5160_rotateMotor(MotorYaw, 0); - Serial.println(F("Stop")); + case 's': // stop — set velocity to zero, motor decelerates on the ramp profile + Serial.print(F("Stopping")); Serial.println(axis_label); + if (do_yaw) tmc5160_rotateMotor(AXIS_YAW, 0); + if (do_pitch) tmc5160_rotateMotor(AXIS_PITCH, 0); break; - case 'b': - ax_yaw.reset_enc_dev_warn(); - ax_pitch.reset_enc_dev_warn(); + case 'b': // clear the latched deviation warning so the next deviation can be detected + Serial.print(F("Clearing encoder deviation flags")); Serial.println(axis_label); + if (do_yaw) ax_yaw.clear_enc_deviation(); + if (do_pitch) ax_pitch.clear_enc_deviation(); break; - case 't': - ax_yaw.set_xenc_to_xact(); - ax_pitch.set_xenc_to_xact(); + case 't': // snap the step counter to the encoder position, eliminating tracking error + Serial.print(F("Syncing encoder to actual")); Serial.println(axis_label); + 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; - case 'e': - ax_yaw.reset_SGT(); - ax_pitch.reset_SGT(); + case 'e': // clear stallguard latch on both axes (no axis specifier supported here) + ax_yaw.clear_stallguard(); + ax_pitch.clear_stallguard(); break; - case 'w': - ax_yaw.set_SGT(args[0]); - ax_pitch.set_SGT(args[0]); + case 'w': // adjust stallguard sensitivity — lower = more sensitive to stalls + ax_yaw.set_stallguard_threshold(args[0]); + ax_pitch.set_stallguard_threshold(args[0]); 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) { - ax_yaw.set_step_len(args[0]); - ax_yaw.target = ax_yaw.encoderpos_l + 2000; - ax_yaw.driver_status = GIMBAL_STATUS_AUTO; + ax_yaw.set_step_size(args[0]); + ax_yaw.target = ax_yaw.enc_limit_l + 2000; // start just inside the left limit + ax_yaw.status = GIMBAL_STATUS_AUTO; } break; - case 'h': - if (args[0] == MotorYaw) { - ax_yaw.c_hdg = (float)args[1]; - ax_yaw.hdg_constant = ax_yaw.X_enc; - - int ee = 0; - int32_t dist = ax_yaw.hdg_constant - ax_yaw.encoderpos_l; - EEPROM.put(ee, ax_yaw.c_hdg); - ee += sizeof(float); - EEPROM.put(ee, dist); + case 'h': // save heading calibration — args[0] = axis (0=yaw), args[1] = heading value + if (args[0] == AXIS_YAW) { + ax_yaw.heading = (float)args[1]; + ax_yaw.heading_origin = ax_yaw.X_enc; + // Store the offset from the left limit rather than the absolute encoder count, + // so the calibration remains valid after a re-home (which may shift the origin). + int32_t dist = ax_yaw.heading_origin - ax_yaw.enc_limit_l; + EEPROM.put(EEPROM_ADDR_HDG_FLOAT, ax_yaw.heading); + EEPROM.put(EEPROM_ADDR_HDG_DIST, dist); } break; - case 'y': { - if (args[0] == MotorYaw) { - int ee = 0; + case 'y': // restore heading calibration from EEPROM — args[0] = axis (0=yaw) + if (args[0] == AXIS_YAW) { int32_t dist = 0; - EEPROM.get(ee, ax_yaw.c_hdg); - ee += sizeof(float); - EEPROM.get(ee, dist); - ax_yaw.hdg_constant = ax_yaw.encoderpos_l + dist; - Serial.println(ax_yaw.c_hdg); - Serial.println(ax_yaw.hdg_constant); + EEPROM.get(EEPROM_ADDR_HDG_FLOAT, ax_yaw.heading); + EEPROM.get(EEPROM_ADDR_HDG_DIST, dist); + // Reconstruct heading_origin relative to the current left limit, + // making it valid even if the absolute encoder counts shifted after re-homing. + ax_yaw.heading_origin = ax_yaw.enc_limit_l + dist; + Serial.println(ax_yaw.heading); + Serial.println(ax_yaw.heading_origin); } break; - } - case 'd': - ax_yaw.driver_status = GIMBAL_STATUS_MANUAL; + case 'd': // enter MANUAL mode — disables AUTO stepping + ax_yaw.status = GIMBAL_STATUS_MANUAL; break; - case 'p': - ax_yaw.set_xenc_to_xact(); - ax_yaw.target += ax_yaw.steps; - ax_yaw.new_pos = true; + case 'p': // advance yaw by one step in AUTO mode + ax_yaw.sync_encoder_to_actual(); // zero tracking error before issuing the next target + ax_yaw.target += ax_yaw.step_size; + ax_yaw.new_pos = true; break; - case 'v': + case 'v': // override yaw v_max at runtime ax_yaw.v_max = args[0]; break; - case '+': - ax_yaw.encoderpos_r = ax_yaw.X_enc; + // Manual endstop overrides — useful for calibration or testing without homing: + case '+': // mark current yaw encoder position as the right limit + ax_yaw.enc_limit_r = ax_yaw.X_enc; break; - case '-': - ax_yaw.encoderpos_l = ax_yaw.X_enc; + case '-': // mark current yaw encoder position as the left limit + ax_yaw.enc_limit_l = ax_yaw.X_enc; break; - case '/': - ax_yaw.encoderpos_r = args[0]; - ax_yaw.encoderpos_l = args[1]; - Serial.println(ax_yaw.encoderpos_r); - Serial.println(ax_yaw.encoderpos_l); + case '/': // set both yaw limits explicitly: args[0] = right, args[1] = left + ax_yaw.enc_limit_r = args[0]; + ax_yaw.enc_limit_l = args[1]; + Serial.println(ax_yaw.enc_limit_r); + Serial.println(ax_yaw.enc_limit_l); break; - case '?': - Serial.println(ax_yaw.encoderpos_r); - Serial.println(ax_yaw.encoderpos_l); - Serial.println(ax_yaw.encoderpos_l - ax_yaw.encoderpos_r); + case '?': // query current yaw endstop positions + Serial.println(ax_yaw.enc_limit_r); + Serial.println(ax_yaw.enc_limit_l); + Serial.println(ax_yaw.enc_limit_l - ax_yaw.enc_limit_r); // negative = right > left (expected) break; default: Serial.println(F("Unknown command")); break; } -} +} \ No newline at end of file