239 lines
9.6 KiB
C++
239 lines
9.6 KiB
C++
#include <EEPROM.h>
|
|
#include <string.h>
|
|
#include "serial/CommandParser.h"
|
|
#include "motor/MotorAxis.h"
|
|
#include "motor/MotorDriver.h"
|
|
#include "motor/Homing.h"
|
|
#include "config/Constants.h"
|
|
|
|
#include "TMC5160.h"
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Serial command protocol
|
|
//
|
|
// 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
|
|
// ---------------------------------------------------------------------------
|
|
|
|
// 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();
|
|
out_pitch = 0;
|
|
return F(" [YAW]");
|
|
}
|
|
if (Serial.peek() == 'p') {
|
|
Serial.read();
|
|
out_yaw = 0;
|
|
return F(" [PITCH]");
|
|
}
|
|
return F(" [YAW + PITCH]");
|
|
}
|
|
|
|
// 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};
|
|
parse_args(args, base);
|
|
|
|
switch (c) {
|
|
|
|
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': // 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 '!': // 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 '*': // 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': // 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 ';': // 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 — 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': // 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': // 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': // clear stallguard latch on both axes (no axis specifier supported here)
|
|
ax_yaw.clear_stallguard();
|
|
ax_pitch.clear_stallguard();
|
|
break;
|
|
|
|
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': // 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_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': // 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': // restore heading calibration from EEPROM — args[0] = axis (0=yaw)
|
|
if (args[0] == AXIS_YAW) {
|
|
int32_t dist = 0;
|
|
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': // enter MANUAL mode — disables AUTO stepping
|
|
ax_yaw.status = GIMBAL_STATUS_MANUAL;
|
|
break;
|
|
|
|
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': // override yaw v_max at runtime
|
|
ax_yaw.v_max = args[0];
|
|
break;
|
|
|
|
// 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 '-': // mark current yaw encoder position as the left limit
|
|
ax_yaw.enc_limit_l = ax_yaw.X_enc;
|
|
break;
|
|
|
|
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 '?': // 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;
|
|
}
|
|
} |