Smoother arms, and swap head tilt/turn ports
- 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.
This commit is contained in:
+27
-19
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user