diff --git a/receiver/receiver.ino b/receiver/receiver.ino index a5de98d..4ded327 100644 --- a/receiver/receiver.ino +++ b/receiver/receiver.ino @@ -26,10 +26,10 @@ * ARMS ARE POSITION CONTROLLED: * 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 - * on connect, so BOTH ARMS MUST BE DOWN when the hub connects. - * Measured travel was about 275 motor degrees each way; the spans below are - * set short of that deliberately, so backlash cannot drive the motor into - * its end stop while holding. + * once, on the first hub1 connect after boot, so BOTH ARMS MUST BE DOWN at + * that moment. They are deliberately NOT re-zeroed on a reconnect - the hub + * keeps its encoder preset, and re-zeroing mid-session would redefine zero + * at whatever position the arms happened to be in. * * FILE ORDER MATTERS: * The Arduino IDE injects generated function prototypes immediately before @@ -48,8 +48,8 @@ // // hub 0 - lower body hub 1 - upper body // A right track A left arm -// B left track B head turn -// C (free) C head tilt +// B left track B head tilt +// C (free) C head turn // D body lift D right arm static const char *HUB0_ADDR = "90:84:2b:61:f2:d7"; 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. // 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. static const int32_t ARM_SPAN_LEFT = 250; static const int32_t ARM_SPAN_RIGHT = -250; 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 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. 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(); 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 DEBUG_MOTORS @@ -297,8 +303,10 @@ static void serviceHub(HubLink &hl) { } } -// Define "arms down" as angle zero. Has to happen after hub1 connects, and -// the arms must physically be at the bottom of their travel. +// Define "arms down" as angle zero. Runs once per boot, after hub1 connects, +// 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() { 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 * 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 : (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_B, leftTrack, gPort[1]); 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_D, rightTarget, gArmRight); @@ -418,11 +429,8 @@ void loop() { serviceHub(gHubs[0]); if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]); - if (gHubs[1].hub.isConnected()) { - zeroArms(); - } else { - gArmsZeroed = false; // re-zero after a reconnect - } + // Zeroes once per boot. Deliberately not reset when the hub drops. + if (gHubs[1].hub.isConnected()) zeroArms(); bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected(); digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW);