Position-controlled arms
Trigger position now sets arm ANGLE rather than power, via setAbsoluteMotorPosition with BrakingStyle::HOLD. Encoders are zeroed on hub1 connect, so both arms must be down at that moment. Spans measured with tools/arm_calibrate: roughly +280 left, -275 right, both comfortably under one motor revolution so the absolute encoder is unambiguous. Set to +/-250 to keep backlash clear of the end stop. L3/R3 are no longer bound - releasing the trigger is lowering. Also documents the async-init trap in serviceHub.
This commit is contained in:
+120
-50
@@ -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);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user