Replace pneumatic intake arm deploy with motorized NEO control - #196
Merged
Conversation
…commands and fix units as needed
Constants.java's UPPER_SNAKE_CASE rename (PR #192, merged from main) landed after the motorized intake arm's CAN ports and current-limit/voltage constants were written against the old names, so they never got renamed. Update the two Spark IO files to match.
Intake now owns one TrapezoidProfile and advances it every periodic() cycle; both arm IOs independently track the resulting setpoint via their own closed-loop control. Commands (stop/deployArm/retractArm) only move the goal state, never call setPosition() directly.
Only the left arm's Spark has an absolute encoder wired up, so self-seeding the relative encoder inside each IntakeArmIOSpark instance was wrong for the right side. IntakeArmIO.resetEncoder() now takes a target position; Intake reads the left arm's absolute encoder once it reports connected and uses that single reading to seed both arms' relative encoders and reset the trapezoidal profile's goal/setpoint.
Intake.isDeployed() and the commented-out initializeIntakeArmCommand() (stale from before the left/right arm split) had no callers. IntakeArmIO.setOpenLoop()/configureSoftLimits() were only reachable from that dead command, so drop them from the interface and both implementations. Also removes a stray no-op semicolon in RollerIOSpark.setOpenLoop().
Commands could move armGoal before the boot-time seed completed (or if it never completes on a sensor fault), and the pre-seed hold-still behavior only worked because minPosRad happened to equal the relative encoder's power-on zero. Gate profile advancement and setPosition() on armSeeded explicitly so the arm holds under brake mode instead of closing a position loop against an unseeded, false-zero encoder. Also clamp the absolute-encoder seed into the soft limit range and raise an alert when the clamp fires, so a miscalibrated absEncoderOffset produces a visible warning instead of silently wedging the arm against a soft limit. Drops the unused kSlot1/kPVel closed-loop config left over from the removed arm velocity control.
Previously a hardcoded false stub. Now true once the arm is seeded and both sides' measured position is within STOWED_TOLERANCE_RAD of minPosRad, mirroring the atGoal()/atSetpoint() pattern ProfiledPID Controller uses. Gated on armSeeded so it can't report "stowed" off an unseeded, false-zero encoder reading before boot calibration completes.
Both arm motors' inverted flags are flipped (also flips each side's relative encoder direction, since REVLib ties relative-encoder phase to motor invert in brushless mode) and the absolute encoder is now configured inverted to match. With the raw range flipped, stowed is now the top of the [minPosRad, maxPosRad] range and deployed is the bottom, so Intake now targets the new STOWED_POS_RAD (110°, aliases maxPosRad) / DEPLOYED_POS_RAD (0°, aliases minPosRad) constants instead of using minPosRad/maxPosRad directly as go-to targets. Soft limits and clamping in the IO classes are untouched since they still bound the same absolute [0°, 110°] range.
motorSim.iterate() already enforces the configured soft limits (REVLib's SparkSim reads the same motorConfig.softLimit and zeroes output past it, mirroring firmware behavior), so the manual position/velocity snap to maxPosRad was duplicate and, unlike the soft-limit path, discontinuously zeroed velocity instead of letting the sim physics decelerate naturally.
Position 0 is horizontal in the current coordinate convention, so gravity torque (and the voltage needed to hold against it) scales with cos(position) and vanishes at vertical. Adds ArmConstants.kG and folds kG * cos(rotation) into the feedforward voltage in both the real and sim IO. kG defaults to 0.0 — not yet measured, so this has no effect until tuned on the robot.
Position readback doesn't carry the hundreds-of-ms latency REV NEO velocity measurement does (that's why velocity feedforward stays commanded, not measured), so kG*cos() now reads the actual sensor position: relEncoder.getPosition() in the real IO, motorSim.getPosition() in sim. Swaps the sim's armSim from a bare DCMotorSim (no gravity term) to SingleJointedArmSim, which models the same cos(position) gravity torque as a uniform rod pivoting at one end. Without this, a nonzero kG in sim would have fought a gravity force that didn't exist; now sim and real hardware are physically consistent for the same kG. Adds MOMENT_OF_INERTIA_KG_M2 (carries over the previously-inline 0.004 placeholder) and ARM_LENGTH_METERS (new placeholder, ~16in) — both unmeasured, flagged for tuning on the robot.
position/velocityMetersPerSec were carried over from the linear RollerIO naming, but the arm's encoder conversion factors (encoderPositionFactor = 2π/reduction, encoderVelocityFactor = 2π/(60·reduction)) produce radians and rad/s, not meters/s — confirmed by every consumer (STOWED_POS_RAD, STOWED_TOLERANCE_RAD, Radians.of(), RadiansPerSecond.of()) already treating them as such. Renamed to positionRad/velocityRadPerSec, matching the convention already used by HoodIO/TurretIO/ModuleIO for the same kind of angular measurement.
SingleJointedArmSim hardcodes the center of mass at length/2. For a mass concentrated near the roller end rather than a uniform rod, the value to plug in is 2x the measured pivot-to-mass-concentration distance, not the raw measurement.
The right arm never had an absolute encoder wired up, so it was reading noise from a floating input; stop constructing one for it. The boot-time seed could grab a stale first CAN frame instead of a settled reading, so gate it behind a discard-then-moving-average settle window. Swap the left/right motor inversion flags, which had the relative encoders running backwards, and recalibrate absEncoderOffset/minPosRad/maxPosRad from bench measurements (five-second holds at stowed, horizontal, and deployed) now that the sensors read correctly. Update the arm sim to match: start stowed like the real robot, and report absolutePosition only where the real IO does. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
DoubleSolenoid) intake arm deploy mechanism with a motorized one: two NEOs driven off a single shared trapezoidal motion profile, closed-loop on each Spark's relative encoder.kG * cos(position)), modeled in both the real IO and the arm sim.absEncoderOffset/minPosRad/maxPosRadagainst horizontal, stowed, and deployed hardstops; fixed the left/right motor inversion (was driving the relative encoders backwards) and removed a phantom absolute-encoder read on the right arm (never physically wired).IntakeArmIOSimSpark, replacing the old open-loopIntakeArmIOSim, so sim exercises the same seeding and profiling code path as real hardware.Test plan
gradlew compileJava)Intake.periodic()without exception undersimulateJavaarmSeedOutOfRangeAlertstill fires on boot (true stowed sits slightly past the calibratedmaxPosRad) — functional (seed clamps safely), but calibration could be tightened further🤖 Generated with Claude Code