Smoother arms, and swap head tilt/turn ports

- ARM_STEP_DEG 8 -> 3 and a dedicated ARM_MIN_GAP_MS of 60, so the arm
  tracks the trigger instead of jumping between targets. Speed and spans
  left as they were.
- hub1 port B is the tilt motor, C is the turn motor - swapped from what
  the first build assumed.
- Zero the arm encoders once per boot rather than on every reconnect. The
  hub keeps its preset, and re-zeroing mid-session would redefine zero at
  whatever position the arms were in when the link dropped.
This commit is contained in:
2026-09-11 12:34:56 +10:00
parent 343544510f
commit 0c149272bd
+27 -19
View File
@@ -26,10 +26,10 @@
* ARMS ARE POSITION CONTROLLED: * ARMS ARE POSITION CONTROLLED:
* The triggers set an ANGLE, not a power level. Trigger released holds the * The triggers set an ANGLE, not a power level. Trigger released holds the
* arm at 0, fully depressed holds it at ARM_SPAN_*. The encoders are zeroed * arm at 0, fully depressed holds it at ARM_SPAN_*. The encoders are zeroed
* on connect, so BOTH ARMS MUST BE DOWN when the hub connects. * once, on the first hub1 connect after boot, so BOTH ARMS MUST BE DOWN at
* Measured travel was about 275 motor degrees each way; the spans below are * that moment. They are deliberately NOT re-zeroed on a reconnect - the hub
* set short of that deliberately, so backlash cannot drive the motor into * keeps its encoder preset, and re-zeroing mid-session would redefine zero
* its end stop while holding. * at whatever position the arms happened to be in.
* *
* FILE ORDER MATTERS: * FILE ORDER MATTERS:
* The Arduino IDE injects generated function prototypes immediately before * The Arduino IDE injects generated function prototypes immediately before
@@ -48,8 +48,8 @@
// //
// hub 0 - lower body hub 1 - upper body // hub 0 - lower body hub 1 - upper body
// A right track A left arm // A right track A left arm
// B left track B head turn // B left track B head tilt
// C (free) C head tilt // C (free) C head turn
// D body lift D right arm // D body lift D right arm
static const char *HUB0_ADDR = "90:84:2b:61:f2:d7"; static const char *HUB0_ADDR = "90:84:2b:61:f2:d7";
static const char *HUB1_ADDR = "90:84:2b:61:e6:8c"; static const char *HUB1_ADDR = "90:84:2b:61:e6:8c";
@@ -80,14 +80,20 @@ static const int DIR_HEAD_TURN = 1;
// Arm travel in MOTOR degrees, measured with tools/arm_calibrate. // Arm travel in MOTOR degrees, measured with tools/arm_calibrate.
// Raw measurements were roughly +280 (left) and -275 (right). These are set // Raw measurements were roughly +280 (left) and -275 (right). These are set
// ~10% short so backlash cannot stall the motor against the top stop. // slightly short so backlash cannot stall the motor against the top stop.
// The sign carries the direction - there is no DIR_ constant for the arms. // The sign carries the direction - there is no DIR_ constant for the arms.
static const int32_t ARM_SPAN_LEFT = 250; static const int32_t ARM_SPAN_LEFT = 250;
static const int32_t ARM_SPAN_RIGHT = -250; static const int32_t ARM_SPAN_RIGHT = -250;
static const int ARM_SPEED = 60; // how fast it travels to the target static const int ARM_SPEED = 60; // how fast it travels to the target
static const byte ARM_MAX_POWER = 40; // torque cap - keeps a jam survivable static const byte ARM_MAX_POWER = 40; // torque cap - keeps a jam survivable
static const int ARM_STEP_DEG = 8; // ignore target changes smaller than this static const int ARM_STEP_DEG = 3; // ignore target changes smaller than this
// Arms get their own command interval. They are position controlled, so the
// target moves continuously with the trigger and needs updating more often
// than a velocity axis does - at the per-port 100ms the arm sprints to each
// target then sits idle, which feels like stepping.
static const unsigned long ARM_MIN_GAP_MS = 60;
// Per-axis power caps for the velocity-controlled axes. // Per-axis power caps for the velocity-controlled axes.
static const int TRACK_MAX = 100; // LEGO speed range is -100..100 static const int TRACK_MAX = 100; // LEGO speed range is -100..100
@@ -235,7 +241,7 @@ static void driveArm(HubLink &hl, byte port, int32_t target, ArmState &st) {
unsigned long now = millis(); unsigned long now = millis();
if (labs((long)target - (long)st.lastTarget) < ARM_STEP_DEG) return; if (labs((long)target - (long)st.lastTarget) < ARM_STEP_DEG) return;
if ((now - st.lastSentAt) < MOTOR_MIN_GAP_MS) return; if ((now - st.lastSentAt) < ARM_MIN_GAP_MS) return;
if ((now - hl.lastCmdAt) < HUB_MIN_GAP_MS) return; if ((now - hl.lastCmdAt) < HUB_MIN_GAP_MS) return;
#if DEBUG_MOTORS #if DEBUG_MOTORS
@@ -297,8 +303,10 @@ static void serviceHub(HubLink &hl) {
} }
} }
// Define "arms down" as angle zero. Has to happen after hub1 connects, and // Define "arms down" as angle zero. Runs once per boot, after hub1 connects,
// the arms must physically be at the bottom of their travel. // with the arms physically at the bottom of their travel. Not repeated on a
// reconnect: the hub keeps its encoder preset, and re-zeroing mid-session
// would redefine zero wherever the arms happened to be sitting.
static void zeroArms() { static void zeroArms() {
if (gArmsZeroed || !gHubs[1].hub.isConnected()) return; if (gArmsZeroed || !gHubs[1].hub.isConnected()) return;
@@ -370,7 +378,7 @@ static void applyFrame(const Frame &f) {
int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100 int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100
* DIR_RIGHT_TRACK; * DIR_RIGHT_TRACK;
// L1 / R1 swing the head while held. // L1 / R1 turn the head while held.
int headTurn = ((f.buttons & BTN_R1) ? HEAD_MAX int headTurn = ((f.buttons & BTN_R1) ? HEAD_MAX
: (f.buttons & BTN_L1) ? -HEAD_MAX : 0) * DIR_HEAD_TURN; : (f.buttons & BTN_L1) ? -HEAD_MAX : 0) * DIR_HEAD_TURN;
@@ -388,8 +396,11 @@ static void applyFrame(const Frame &f) {
driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]); driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]);
driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]); driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]);
driveMotor(gHubs[0], PORT_D, bodyLift, gPort[2]); driveMotor(gHubs[0], PORT_D, bodyLift, gPort[2]);
driveMotor(gHubs[1], PORT_B, headTurn, gPort[3]);
driveMotor(gHubs[1], PORT_C, headTilt, gPort[4]); // hub1 B is the tilt motor and C is the turn motor - the reverse of what
// the first build assumed.
driveMotor(gHubs[1], PORT_B, headTilt, gPort[3]);
driveMotor(gHubs[1], PORT_C, headTurn, gPort[4]);
driveArm(gHubs[1], PORT_A, leftTarget, gArmLeft); driveArm(gHubs[1], PORT_A, leftTarget, gArmLeft);
driveArm(gHubs[1], PORT_D, rightTarget, gArmRight); driveArm(gHubs[1], PORT_D, rightTarget, gArmRight);
@@ -418,11 +429,8 @@ void loop() {
serviceHub(gHubs[0]); serviceHub(gHubs[0]);
if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]); if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]);
if (gHubs[1].hub.isConnected()) { // Zeroes once per boot. Deliberately not reset when the hub drops.
zeroArms(); if (gHubs[1].hub.isConnected()) zeroArms();
} else {
gArmsZeroed = false; // re-zero after a reconnect
}
bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected(); bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected();
digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW); digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW);