Working configuration - verified on hardware
- Swap HUB0/HUB1 addresses; they were the wrong way round. - Add DIR_* constants for all seven motors, with DIR_RIGHT_TRACK at -1. - Swap the right arm buttons: Cross raises, Triangle lowers. - Move struct Frame into the types block - the Arduino prototype generator needs it declared above the first function definition. - Pin core 2.0.17 in the header; 3.x will not build Legoino.
This commit is contained in:
+44
-16
@@ -3,9 +3,12 @@
|
||||
*
|
||||
* Model: Johnny 5 (Short Circuit) MOC, 7 motors across 2 Technic hubs.
|
||||
*
|
||||
* Board package: esp32 (the normal Espressif one)
|
||||
* 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.
|
||||
*
|
||||
* Listens for gamepad frames from the Bluepad32 board on Serial2 and drives
|
||||
* both hubs over BLE using Legoino. This board must NOT have the Bluepad32
|
||||
* board package selected - keeping BTstack and NimBLE on separate chips is
|
||||
@@ -18,6 +21,13 @@
|
||||
*
|
||||
* Control scheme is tank drive: every input drives exactly one motor.
|
||||
*
|
||||
* 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".
|
||||
*
|
||||
* Created by: Jess Rogerson (yelling commands at Claude.AI)
|
||||
*/
|
||||
|
||||
@@ -32,8 +42,8 @@
|
||||
// B left track B head turn
|
||||
// D body lift C left arm
|
||||
// D right arm
|
||||
static const char *HUB0_ADDR = "90:84:2b:61:e6:8c";
|
||||
static const char *HUB1_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";
|
||||
|
||||
// Port numbers are just bytes in the LEGO protocol, same on every hub type.
|
||||
static const byte PORT_A = 0x00;
|
||||
@@ -51,6 +61,17 @@ static const byte PORT_D = 0x03;
|
||||
|
||||
static const int DEADZONE = 40; // raw stick counts ignored around centre
|
||||
|
||||
// Motor directions. Flip to -1 if an axis runs backwards - mirrored mountings
|
||||
// are normal on a symmetric model, and this is cheaper than editing signs
|
||||
// scattered through applyFrame().
|
||||
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;
|
||||
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.
|
||||
@@ -105,6 +126,12 @@ struct PortState {
|
||||
unsigned long lastSentAt;
|
||||
};
|
||||
|
||||
struct Frame {
|
||||
int lx, ly, rx, ry;
|
||||
unsigned buttons, dpad;
|
||||
int l2, r2;
|
||||
};
|
||||
|
||||
// ================================================================= globals
|
||||
|
||||
static HubLink gHubs[2] = {
|
||||
@@ -203,12 +230,6 @@ static uint8_t xorChecksum(const char *s, size_t len) {
|
||||
return c;
|
||||
}
|
||||
|
||||
struct Frame {
|
||||
int lx, ly, rx, ry;
|
||||
unsigned buttons, dpad;
|
||||
int l2, r2;
|
||||
};
|
||||
|
||||
static bool parseFrame(char *line, Frame &f) {
|
||||
char *star = strrchr(line, '*');
|
||||
if (!star) return false;
|
||||
@@ -235,17 +256,24 @@ static void applyFrame(const Frame &f) {
|
||||
if (f.buttons & BTN_L1) scale = SCALE_PRECISION;
|
||||
if (f.buttons & BTN_R1) scale = SCALE_FULL;
|
||||
|
||||
int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * scale / 100;
|
||||
int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * scale / 100;
|
||||
int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * scale / 100 * DIR_LEFT_TRACK;
|
||||
int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * scale / 100 * DIR_RIGHT_TRACK;
|
||||
|
||||
int headTilt = (f.dpad & DPAD_U) ? HEAD_MAX : (f.dpad & DPAD_D) ? -HEAD_MAX : 0;
|
||||
int headTurn = (f.dpad & DPAD_R) ? HEAD_MAX : (f.dpad & DPAD_L) ? -HEAD_MAX : 0;
|
||||
int headTilt = ((f.dpad & DPAD_U) ? HEAD_MAX
|
||||
: (f.dpad & DPAD_D) ? -HEAD_MAX : 0) * DIR_HEAD_TILT;
|
||||
int headTurn = ((f.dpad & DPAD_R) ? HEAD_MAX
|
||||
: (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TURN;
|
||||
|
||||
// R2 raises, L2 lowers. Both analog 0..1023, so the lift stays proportional.
|
||||
int bodyLift = constrain((f.r2 - f.l2) * LIFT_MAX / 1023, -LIFT_MAX, LIFT_MAX);
|
||||
int bodyLift = constrain((f.r2 - f.l2) * LIFT_MAX / 1023,
|
||||
-LIFT_MAX, LIFT_MAX) * DIR_BODY_LIFT;
|
||||
|
||||
int leftArm = (f.buttons & BTN_X) ? ARM_MAX : (f.buttons & BTN_B) ? -ARM_MAX : 0;
|
||||
int rightArm = (f.buttons & BTN_Y) ? ARM_MAX : (f.buttons & BTN_A) ? -ARM_MAX : 0;
|
||||
// Square raises the left arm, Circle lowers it.
|
||||
int leftArm = ((f.buttons & BTN_X) ? ARM_MAX
|
||||
: (f.buttons & BTN_B) ? -ARM_MAX : 0) * DIR_LEFT_ARM;
|
||||
// Cross raises the right arm, Triangle lowers it.
|
||||
int rightArm = ((f.buttons & BTN_A) ? ARM_MAX
|
||||
: (f.buttons & BTN_Y) ? -ARM_MAX : 0) * DIR_RIGHT_ARM;
|
||||
|
||||
driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]);
|
||||
driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]);
|
||||
|
||||
Reference in New Issue
Block a user