jessikitty
0c149272bd
- 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.
468 lines
17 KiB
Arduino
468 lines
17 KiB
Arduino
/*
|
|
* receiver.ino -- UART -> two LEGO Powered Up hubs (ESP32 "B")
|
|
*
|
|
* Model: Johnny 5 (Short Circuit) MOC - "Evolved" control scheme.
|
|
* 7 motors across 2 Technic hubs, plus 2 GPIO-driven LED circuits.
|
|
*
|
|
* Board package: esp32 (the normal Espressif one), core 2.0.17
|
|
* Libraries: Legoino + NimBLE-Arduino 1.4.x (both via Library Manager)
|
|
*
|
|
* 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
|
|
* TX GPIO17 -> RX GPIO16 on transmitter
|
|
* GND -> GND (mandatory - common ground)
|
|
*
|
|
* LED circuits (see LED_1_PIN / LED_2_PIN below):
|
|
* GPIO25 -> resistor -> LED pair -> GND
|
|
* GPIO26 -> resistor -> LED pair -> GND
|
|
* 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
|
|
* 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
|
|
* 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, ArmState and Frame all live in the types block.
|
|
*
|
|
* Created by: Jess Rogerson (yelling commands at Claude.AI)
|
|
*/
|
|
|
|
#include "Lpf2Hub.h"
|
|
|
|
// ---------------------------------------------------------------- settings
|
|
|
|
// Hub BLE addresses. Run tools/hub_scanner to find them - do not guess.
|
|
//
|
|
// hub 0 - lower body hub 1 - upper body
|
|
// A right track A left arm
|
|
// 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";
|
|
|
|
static const byte PORT_A = 0x00;
|
|
static const byte PORT_B = 0x01;
|
|
static const byte PORT_C = 0x02;
|
|
static const byte PORT_D = 0x03;
|
|
|
|
// LED circuits. Safe GPIOs - no boot strapping or flash duties, unlike 0, 2,
|
|
// 12 and 15.
|
|
static const int LED_1_PIN = 25; // Circle toggles this pair
|
|
static const int LED_2_PIN = 26; // Triangle toggles this pair
|
|
|
|
#define USE_TACHO_MOTORS 1
|
|
|
|
// 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
|
|
|
|
// Motor directions. Flip to -1 if an axis runs backwards.
|
|
static const int DIR_LEFT_TRACK = 1;
|
|
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;
|
|
|
|
// Arm travel in MOTOR degrees, measured with tools/arm_calibrate.
|
|
// Raw measurements were roughly +280 (left) and -275 (right). These are set
|
|
// 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 = 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
|
|
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 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 DPAD_U = 0x01;
|
|
static const unsigned DPAD_D = 0x02;
|
|
static const unsigned DPAD_R = 0x04;
|
|
static const unsigned DPAD_L = 0x08;
|
|
|
|
// Triggers rest at 0 and are noisy near the bottom of their travel.
|
|
static const int TRIGGER_DEADZONE = 60;
|
|
|
|
static const unsigned long MOTOR_MIN_GAP_MS = 100; // per port
|
|
static const unsigned long HUB_MIN_GAP_MS = 25; // per hub, ~40 cmd/sec
|
|
static const unsigned long LINK_TIMEOUT_MS = 400; // failsafe
|
|
static const unsigned long RECONNECT_GAP_MS = 2000;
|
|
|
|
static const int LINK_RX_PIN = 16;
|
|
static const int LINK_TX_PIN = 17;
|
|
static const long LINK_BAUD = 115200;
|
|
|
|
static const int STATUS_LED_PIN = 2;
|
|
|
|
// =================================================================== types
|
|
|
|
struct HubLink {
|
|
Lpf2Hub hub;
|
|
const char *addr;
|
|
const char *label;
|
|
bool initialised;
|
|
unsigned long retryAt;
|
|
unsigned long lastCmdAt; // per-hub rate limit, shared across its ports
|
|
};
|
|
|
|
struct PortState {
|
|
int lastSpeed;
|
|
unsigned long lastSentAt;
|
|
};
|
|
|
|
struct ArmState {
|
|
int32_t lastTarget;
|
|
unsigned long lastSentAt;
|
|
};
|
|
|
|
struct Frame {
|
|
int lx, ly, rx, ry;
|
|
unsigned buttons, dpad;
|
|
int l2, r2;
|
|
};
|
|
|
|
// ================================================================= globals
|
|
|
|
static HubLink gHubs[2] = {
|
|
{Lpf2Hub(), HUB0_ADDR, "hub0", false, 0, 0},
|
|
{Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0},
|
|
};
|
|
|
|
// Indexes follow physical ports, not functions:
|
|
// 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;
|
|
|
|
// Latched state, changed on button press rather than while held.
|
|
static int gTrackSpeed = SPEED_SLOW;
|
|
static bool gLed1On = false;
|
|
static bool gLed2On = false;
|
|
static unsigned gPrevButtons = 0;
|
|
|
|
// ================================================================= helpers
|
|
|
|
// Deadzone, then rescale so the remaining travel still reaches full speed.
|
|
static int stickToSpeed(int raw, int maxSpeed) {
|
|
if (raw > -DEADZONE && raw < DEADZONE) return 0;
|
|
int sign = (raw < 0) ? -1 : 1;
|
|
long magnitude = labs((long)raw) - DEADZONE;
|
|
long scaled = (magnitude * maxSpeed) / (512L - DEADZONE);
|
|
if (scaled > maxSpeed) scaled = maxSpeed;
|
|
return sign * (int)scaled;
|
|
}
|
|
|
|
// 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 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) {
|
|
if (!hl.hub.isConnected()) return;
|
|
|
|
unsigned long now = millis();
|
|
bool stopping = (speed == 0 && st.lastSpeed != 0);
|
|
|
|
// Stops always go out immediately. Everything else is rate limited twice:
|
|
// 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;
|
|
if ((now - hl.lastCmdAt) < HUB_MIN_GAP_MS) return;
|
|
}
|
|
|
|
#if DEBUG_MOTORS
|
|
Serial.printf("TX %s port %u speed %d\n", hl.label, port, speed);
|
|
#endif
|
|
|
|
#if USE_TACHO_MOTORS
|
|
hl.hub.setTachoMotorSpeed(port, speed);
|
|
#else
|
|
hl.hub.setBasicMotorSpeed(port, speed);
|
|
#endif
|
|
|
|
st.lastSpeed = speed;
|
|
st.lastSentAt = now;
|
|
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) < ARM_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_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;
|
|
|
|
if (hl.hub.isConnecting()) {
|
|
hl.hub.connectHub();
|
|
if (hl.hub.isConnected()) {
|
|
Serial.printf("[%s] connected (%s)\n", hl.label, hl.addr);
|
|
hl.hub.setLedColor(GREEN);
|
|
} else {
|
|
Serial.printf("[%s] connect failed, retrying\n", hl.label);
|
|
hl.initialised = false;
|
|
hl.retryAt = millis() + RECONNECT_GAP_MS;
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!hl.initialised && millis() >= hl.retryAt) {
|
|
Serial.printf("[%s] scanning for %s\n", hl.label, hl.addr);
|
|
hl.hub.init(std::string(hl.addr));
|
|
hl.initialised = true;
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
|
|
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];
|
|
return c;
|
|
}
|
|
|
|
static bool parseFrame(char *line, Frame &f) {
|
|
char *star = strrchr(line, '*');
|
|
if (!star) return false;
|
|
*star = '\0';
|
|
|
|
unsigned expected = 0;
|
|
if (sscanf(star + 1, "%2x", &expected) != 1) return false;
|
|
if (xorChecksum(line, strlen(line)) != (uint8_t)expected) return false;
|
|
|
|
return sscanf(line, "G,%d,%d,%d,%d,%u,%u,%d,%d",
|
|
&f.lx, &f.ly, &f.rx, &f.ry,
|
|
&f.buttons, &f.dpad, &f.l2, &f.r2) == 8;
|
|
}
|
|
|
|
// Latching controls fire once per press, not continuously while held. Frames
|
|
// arrive at ~50 Hz, so without edge detection a single press would toggle
|
|
// twenty times.
|
|
static void handleLatchingButtons(unsigned buttons) {
|
|
unsigned pressed = buttons & ~gPrevButtons;
|
|
gPrevButtons = buttons;
|
|
|
|
if (pressed & BTN_A) { // Cross - alternate track speed
|
|
gTrackSpeed = (gTrackSpeed == SPEED_FAST) ? SPEED_SLOW : SPEED_FAST;
|
|
Serial.printf("track speed %d%%\n", gTrackSpeed);
|
|
}
|
|
if (pressed & BTN_B) { // Circle - LED pair 1
|
|
gLed1On = !gLed1On;
|
|
digitalWrite(LED_1_PIN, gLed1On ? HIGH : LOW);
|
|
}
|
|
if (pressed & BTN_Y) { // Triangle - LED pair 2
|
|
gLed2On = !gLed2On;
|
|
digitalWrite(LED_2_PIN, gLed2On ? HIGH : LOW);
|
|
}
|
|
}
|
|
|
|
// Tank drive. One input per motor - nothing is mixed.
|
|
static void applyFrame(const Frame &f) {
|
|
handleLatchingButtons(f.buttons);
|
|
|
|
// Square is the panic stop. LEDs are left alone - they are not motion.
|
|
if (f.buttons & BTN_X) {
|
|
stopEverything();
|
|
return;
|
|
}
|
|
|
|
int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * gTrackSpeed / 100
|
|
* DIR_LEFT_TRACK;
|
|
int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100
|
|
* DIR_RIGHT_TRACK;
|
|
|
|
// 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;
|
|
|
|
// 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: 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]);
|
|
|
|
// 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);
|
|
}
|
|
|
|
// ==================================================================== main
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Serial2.begin(LINK_BAUD, SERIAL_8N1, LINK_RX_PIN, LINK_TX_PIN);
|
|
|
|
pinMode(STATUS_LED_PIN, OUTPUT);
|
|
digitalWrite(STATUS_LED_PIN, LOW);
|
|
|
|
pinMode(LED_1_PIN, OUTPUT);
|
|
pinMode(LED_2_PIN, OUTPUT);
|
|
digitalWrite(LED_1_PIN, LOW);
|
|
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() {
|
|
// 1. Keep the hubs connected, hub0 first.
|
|
serviceHub(gHubs[0]);
|
|
if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]);
|
|
|
|
// 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);
|
|
|
|
// 2. Pull whole lines off the link.
|
|
static char buf[128];
|
|
static size_t idx = 0;
|
|
|
|
while (Serial2.available()) {
|
|
char c = (char)Serial2.read();
|
|
if (c == '\r') continue;
|
|
if (c == '\n') {
|
|
buf[idx] = '\0';
|
|
Frame f;
|
|
if (idx > 0 && parseFrame(buf, f)) {
|
|
lastFrameAt = millis();
|
|
failsafeEngaged = false;
|
|
applyFrame(f);
|
|
}
|
|
idx = 0;
|
|
} else if (idx < sizeof(buf) - 1) {
|
|
buf[idx++] = c;
|
|
} else {
|
|
idx = 0; // overrun, throw the line away
|
|
}
|
|
}
|
|
|
|
// 3. Failsafe - link went quiet, stop before something drives off a table.
|
|
if (!failsafeEngaged && (millis() - lastFrameAt) > LINK_TIMEOUT_MS) {
|
|
Serial.println("Link timeout - stopping motors");
|
|
stopEverything();
|
|
failsafeEngaged = true;
|
|
}
|
|
}
|