diff --git a/receiver/receiver.ino b/receiver/receiver.ino index 4f9e2d4..a5de98d 100644 --- a/receiver/receiver.ino +++ b/receiver/receiver.ino @@ -9,6 +9,8 @@ * * Core 3.x will not build Legoino - you get 'std::string does not name a type' * and a ReadUInt32LE declaration mismatch. Stay on 2.0.17 for this board. + * Do NOT select the esp32_bluepad32 package here either: it starts BTstack + * before setup() runs and NimBLE aborts with ESP_ERR_INVALID_STATE. * * Wiring to the transmitter board: * RX GPIO16 <- TX GPIO17 on transmitter @@ -21,12 +23,19 @@ * Pins output 3.3V, not 3V. ~12mA per pin is comfortable, 40mA is the hard * limit. Anything drawing more than ~20mA per circuit needs a transistor. * + * 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. + * * FILE ORDER MATTERS: * The Arduino IDE injects generated function prototypes immediately before * the FIRST function definition in the file. Any type used in a function * signature must be declared above that point - which is why HubLink, - * PortState and Frame all live in the types block. Move a function above - * them and you get "'Frame' has not been declared". + * PortState, ArmState and Frame all live in the types block. * * Created by: Jess Rogerson (yelling commands at Claude.AI) */ @@ -45,7 +54,6 @@ static const char *HUB0_ADDR = "90:84:2b:61:f2:d7"; static const char *HUB1_ADDR = "90:84:2b:61:e6:8c"; -// Port numbers are just bytes in the LEGO protocol, same on every hub type. static const byte PORT_A = 0x00; static const byte PORT_B = 0x01; static const byte PORT_C = 0x02; @@ -56,11 +64,9 @@ static const byte PORT_D = 0x03; static const int LED_1_PIN = 25; // Circle toggles this pair static const int LED_2_PIN = 26; // Triangle toggles this pair -// Technic / Control+ motors are tacho motors -> leave this at 1. #define USE_TACHO_MOTORS 1 -// Set to 1 to log every motor command that goes out. Useful for proving which -// port a command actually lands on. Noisy - turn it back off afterwards. +// Set to 1 to log every command that goes out. Noisy - turn it back off. #define DEBUG_MOTORS 0 static const int DEADZONE = 40; // raw stick counts ignored around centre @@ -71,32 +77,34 @@ static const int DIR_RIGHT_TRACK = -1; static const int DIR_BODY_LIFT = 1; static const int DIR_HEAD_TILT = 1; static const int DIR_HEAD_TURN = 1; -static const int DIR_LEFT_ARM = 1; -static const int DIR_RIGHT_ARM = 1; -// Per-axis power caps. Everything except the tracks runs into a mechanical end -// stop and there is no position feedback, so holding a direction at a stop -// stalls the motor. Lower these if an axis feels forceful. -static const int TRACK_MAX = 100; // LEGO speed range is -100..100 -static const int HEAD_MAX = 45; -static const int LIFT_MAX = 60; -static const int ARM_UP_MAX = 45; -static const int ARM_DOWN_MAX = 30; // gravity helps on the way down +// 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. +// 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 + +// Per-axis power caps for the velocity-controlled axes. +static const int TRACK_MAX = 100; // LEGO speed range is -100..100 +static const int HEAD_MAX = 45; +static const int LIFT_MAX = 60; // Track speed, toggled by Cross. static const int SPEED_SLOW = 50; static const int SPEED_FAST = 100; -// Bluepad32 button masks. Verify against your own pad with DEBUG_BUTTONS in -// the transmitter sketch if any of these seem wrong. +// Bluepad32 button masks. Verify with DEBUG_BUTTONS in the transmitter. static const unsigned BTN_A = 0x0001; // Cross static const unsigned BTN_B = 0x0002; // Circle static const unsigned BTN_X = 0x0004; // Square static const unsigned BTN_Y = 0x0008; // Triangle static const unsigned BTN_L1 = 0x0010; static const unsigned BTN_R1 = 0x0020; -static const unsigned BTN_L3 = 0x0100; // left stick click -static const unsigned BTN_R3 = 0x0200; // right stick click static const unsigned DPAD_U = 0x01; static const unsigned DPAD_D = 0x02; @@ -133,6 +141,11 @@ struct PortState { unsigned long lastSentAt; }; +struct ArmState { + int32_t lastTarget; + unsigned long lastSentAt; +}; + struct Frame { int lx, ly, rx, ry; unsigned buttons, dpad; @@ -147,10 +160,13 @@ static HubLink gHubs[2] = { }; // Indexes follow physical ports, not functions: -// 0 hub0/A 1 hub0/B 2 hub0/D -// 3 hub1/A 4 hub1/B 5 hub1/C 6 hub1/D -static PortState gPort[7] = {{999, 0}, {999, 0}, {999, 0}, {999, 0}, - {999, 0}, {999, 0}, {999, 0}}; +// 0 hub0/A 1 hub0/B 2 hub0/D 3 hub1/B 4 hub1/C +static PortState gPort[5] = {{999, 0}, {999, 0}, {999, 0}, {999, 0}, {999, 0}}; + +// Arms are position controlled, so they get their own state. +static ArmState gArmLeft = {INT32_MIN, 0}; +static ArmState gArmRight = {INT32_MIN, 0}; +static bool gArmsZeroed = false; static unsigned long lastFrameAt = 0; static bool failsafeEngaged = true; @@ -173,13 +189,13 @@ static int stickToSpeed(int raw, int maxSpeed) { return sign * (int)scaled; } -// Analog trigger, 0..1023, to a 0..maxSpeed power with a deadzone at the bottom. -static int triggerToSpeed(int raw, int maxSpeed) { +// Analog trigger, 0..1023, to a target angle between 0 and span. +static int32_t triggerToAngle(int raw, int32_t span) { if (raw <= TRIGGER_DEADZONE) return 0; - long scaled = ((long)(raw - TRIGGER_DEADZONE) * maxSpeed) / - (1023L - TRIGGER_DEADZONE); - if (scaled > maxSpeed) scaled = maxSpeed; - return (int)scaled; + long travel = (long)raw - TRIGGER_DEADZONE; + long full = 1023L - TRIGGER_DEADZONE; + if (travel > full) travel = full; + return (int32_t)((travel * span) / full); } static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) { @@ -189,8 +205,8 @@ static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) { bool stopping = (speed == 0 && st.lastSpeed != 0); // Stops always go out immediately. Everything else is rate limited twice: - // per port, and per hub - hub 1 has four motors on it, and the per-port - // limit alone lets through more than the hub will swallow. + // per port, and per hub - the per-port limit alone lets through more than + // a hub with several motors on it will swallow. if (!stopping) { if (speed == st.lastSpeed) return; if ((now - st.lastSentAt) < MOTOR_MIN_GAP_MS) return; @@ -212,18 +228,52 @@ static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) { hl.lastCmdAt = now; } +// Position control. HOLD keeps the motor actively at the target rather than +// letting gravity drag the arm back down. +static void driveArm(HubLink &hl, byte port, int32_t target, ArmState &st) { + if (!hl.hub.isConnected() || !gArmsZeroed) return; + + 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 - hl.lastCmdAt) < HUB_MIN_GAP_MS) return; + +#if DEBUG_MOTORS + Serial.printf("TX %s port %u angle %ld\n", hl.label, port, (long)target); +#endif + + hl.hub.setAbsoluteMotorPosition(port, ARM_SPEED, target, ARM_MAX_POWER, + BrakingStyle::HOLD); + + st.lastTarget = target; + st.lastSentAt = now; + hl.lastCmdAt = now; +} + static void stopEverything() { driveMotor(gHubs[0], PORT_A, 0, gPort[0]); driveMotor(gHubs[0], PORT_B, 0, gPort[1]); driveMotor(gHubs[0], PORT_D, 0, gPort[2]); - driveMotor(gHubs[1], PORT_A, 0, gPort[3]); - driveMotor(gHubs[1], PORT_B, 0, gPort[4]); - driveMotor(gHubs[1], PORT_C, 0, gPort[5]); - driveMotor(gHubs[1], PORT_D, 0, gPort[6]); + driveMotor(gHubs[1], PORT_B, 0, gPort[3]); + driveMotor(gHubs[1], PORT_C, 0, gPort[4]); + + // Arms: a plain speed command overrides the position hold and goes limp. + // Reset the cached targets so the next trigger movement re-commands. + if (gHubs[1].hub.isConnected()) { + gHubs[1].hub.setTachoMotorSpeed(PORT_A, 0); + gHubs[1].hub.setTachoMotorSpeed(PORT_D, 0); + } + gArmLeft.lastTarget = INT32_MIN; + gArmRight.lastTarget = INT32_MIN; } // Connect the hubs one at a time. Kicking off two scans at once upsets the // shared NimBLE scanner and you end up with one hub connected and one sulking. +// +// Note the 'initialised' one-shot. init() starts an ASYNCHRONOUS scan, so +// immediately afterwards isConnected() and isConnecting() are both still +// false. Guarding on those alone re-enters NimBLEDevice::init() thousands of +// times a second and the Bluetooth controller aborts. static void serviceHub(HubLink &hl) { if (hl.hub.isConnected()) return; @@ -247,6 +297,23 @@ 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. +static void zeroArms() { + if (gArmsZeroed || !gHubs[1].hub.isConnected()) return; + + delay(500); // let the hub finish reporting its ports + gHubs[1].hub.setAbsoluteMotorEncoderPosition(PORT_A, 0); + delay(200); + gHubs[1].hub.setAbsoluteMotorEncoderPosition(PORT_D, 0); + delay(200); + + gArmsZeroed = true; + gArmLeft.lastTarget = INT32_MIN; + gArmRight.lastTarget = INT32_MIN; + Serial.println("Arms zeroed at current position"); +} + static uint8_t xorChecksum(const char *s, size_t len) { uint8_t c = 0; for (size_t i = 0; i < len; i++) c ^= (uint8_t)s[i]; @@ -307,29 +374,25 @@ static void applyFrame(const Frame &f) { int headTurn = ((f.buttons & BTN_R1) ? HEAD_MAX : (f.buttons & BTN_L1) ? -HEAD_MAX : 0) * DIR_HEAD_TURN; - // D-pad: up/down lifts the body, left/right tilts the head. Both move - // while held. + // D-pad: up/down lifts the body, left/right tilts the head. int bodyLift = ((f.dpad & DPAD_U) ? LIFT_MAX : (f.dpad & DPAD_D) ? -LIFT_MAX : 0) * DIR_BODY_LIFT; int headTilt = ((f.dpad & DPAD_R) ? HEAD_MAX : (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TILT; - // Arms: analog trigger raises proportionally, stick click lowers. - int leftRaise = triggerToSpeed(f.l2, ARM_UP_MAX); - int rightRaise = triggerToSpeed(f.r2, ARM_UP_MAX); - - int leftArm = ((f.buttons & BTN_L3) ? -ARM_DOWN_MAX : leftRaise) - * DIR_LEFT_ARM; - int rightArm = ((f.buttons & BTN_R3) ? -ARM_DOWN_MAX : rightRaise) - * DIR_RIGHT_ARM; + // Arms: trigger position IS arm angle. Released means "go to zero", which + // gravity is already doing, so the motor mostly just catches it. + int32_t leftTarget = triggerToAngle(f.l2, ARM_SPAN_LEFT); + int32_t rightTarget = triggerToAngle(f.r2, ARM_SPAN_RIGHT); 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_A, leftArm, gPort[3]); - driveMotor(gHubs[1], PORT_B, headTurn, gPort[4]); - driveMotor(gHubs[1], PORT_C, headTilt, gPort[5]); - driveMotor(gHubs[1], PORT_D, rightArm, gPort[6]); + driveMotor(gHubs[1], PORT_B, headTurn, gPort[3]); + driveMotor(gHubs[1], PORT_C, headTilt, gPort[4]); + + driveArm(gHubs[1], PORT_A, leftTarget, gArmLeft); + driveArm(gHubs[1], PORT_D, rightTarget, gArmRight); } // ==================================================================== main @@ -347,6 +410,7 @@ void setup() { digitalWrite(LED_2_PIN, LOW); Serial.println("LEGO hub receiver starting (Johnny 5 Evolved)"); + Serial.println("Both arms must be DOWN before hub1 connects"); } void loop() { @@ -354,6 +418,12 @@ 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 + } + bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected(); digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW);