Made all of the ControllerHelper class

[AndrewWerner29]

and replaced code that it was needed in and removed unneeded code

[claude]

The ControllerHelper utility is a nice consolidation, but there are several behavioral breaks: velocity scaling (maxSpeed/maxAngularVelocity) is dropped, the Y-axis inversion is dropped, and modifyAxisCubed squares rather than cubes. These will cause incorrect robot motion at runtime.

[claude]

omegaRps is now in [-1, 1], not rad/s. The original multiplied by (maxAngularVelocity.in(RadiansPerSecond) / 2.0). This removes the half-speed rotation cap and will result in the robot turning at full angular velocity at full stick deflection instead of half.

[AndrewWerner29]

how do I fix this @claude

[claude]

Claude finished @AndrewWerner29's task in 2m 38s —— View job

---

Fixes applied

• Read the changed files
• Understand the scaling issues
• Fix velXMps and velYMps scaling + Y-axis inversion in fieldOrientedDriveCommand
• Fix omegaRps scaling in fieldOrientedDriveCommand
• Fix velXMps and velYMps scaling in faceAngleWhileDrivingCommand
• Fix modifyAxisCubed comment in ControllerHelper (it squares, not cubes)
• Push changes

---

Here's what was wrong and what was changed:

velXMps / velYMps (lines 132–133) — scale was 1 (unitless [-1,1]), but withVelocityX/Y expects m/s. Also the Xbox left Y-axis returns negative when pushed forward, so you need to negate it to get positive = forward:

// Before
double velXMps = ControllerHelper.modifyAxisCubic(driveCont.getLeftY(), 1);
double velYMps = ControllerHelper.modifyAxisCubic(driveCont.getLeftX(), 1);

// After
double velXMps = ControllerHelper.modifyAxisCubic(driveCont.getLeftY(), -maxSpeed.in(MetersPerSecond));
double velYMps = ControllerHelper.modifyAxisCubic(driveCont.getLeftX(), -maxSpeed.in(MetersPerSecond));

omegaRps (line 134) — scale was 1 (unitless [-1,1]), but withRotationalRate expects rad/s. The original code used half of max angular velocity to cap rotation at 50%:

// Before
double omegaRps = ControllerHelper.modifyAxisCubed(driveCont.getRightX(), 1);

// After
double omegaRps = ControllerHelper.modifyAxisCubed(driveCont.getRightX(), maxAngularVelocity.in(RadiansPerSecond) / 2.0);

faceAngleWhileDrivingCommand — same maxSpeed scaling fix applied to both velocity suppliers.

ControllerHelper.modifyAxisCubed comment — updated "Cube" → "Square" since the code does clippedValue * clippedValue.

[claude]

Same missing maxSpeed scaling and inversion as fieldOrientedDriveCommand. The DoubleSuppliers passed to the base faceAngleWhileDrivingCommand overload should provide velocities in m/s, but these will return normalized [-1, 1] values.

[AndrewWerner29]

@claude how could i test this? Do not change anything, just tell me how to test it

[claude]

Claude finished @AndrewWerner29's task in 45s —— View job

---

How to Test the ControllerHelper Changes

There are two main approaches: simulation testing and on-robot testing.

---

Option 1: Robot Simulation (./gradlew simulateJava)

This is the safest and fastest way to verify the changes without needing the real robot.

1. Run the simulation: ./gradlew simulateJava

2. Open AdvantageScope and connect to the simulation

3. Open Shuffleboard or the Driver Station sim GUI and connect a joystick (or use a virtual joystick)

4. What to watch in AdvantageScope:

   • Swerve/CommandedSpeeds — push the left stick fully forward; vxMetersPerSecond should be approximately +4.85 (maxSpeed). If it's −4.85, the Y-axis inversion is wrong. If it's ~1.0, the scale is wrong.
   • Swerve/CommandedSpeeds.omegaRadiansPerSecond — rotate the right stick fully; value should be roughly ±12.57 rad/s (half of 4 rev/s × 2π), not ±1.0

5. Specific checks:

   Action	Expected vxMetersPerSecond	Wrong if...
   Push left stick fully forward	≈ +4.85 m/s	it's −4.85 (inversion wrong) or ±1.0 (scale wrong)
   Push left stick fully left	vyMetersPerSecond ≈ +4.85 m/s	same as above
   Rotate right stick fully right	omegaRps ≈ −12.57 rad/s	it's ±1.0 or ±25.13 (scale wrong)

---

Option 2: On the Robot (WoodBot/PracticeBot)

1. Deploy: ./gradlew deploy (while connected to the robot network)

2. Open AdvantageScope and connect to the robot (10.3.60.2)

3. Enable in Teleop mode

4. Sanity checks with the driver controller:

   • Push left stick forward → robot should drive forward (away from you). If it goes backward, Y-axis inversion is still wrong.
   • Push left stick left → robot should strafe left.
   • Rotate right stick right → robot should turn clockwise (right).
   • Do these checks from both alliances (the alliance flip code in fieldOrientedDriveCommand swaps the X/Y signs for red alliance).

5. Log values to verify in AdvantageScope:

   • Swerve/CommandedSpeeds — at full stick deflection, vx/vy should reach close to ±4.85 m/s; omega should reach ~±12.57 rad/s.

---

Option 3: Unit Test for ControllerHelper (no hardware needed)

You can write a quick JUnit test to verify modifyAxisCubic and modifyAxisCubed return correct values:

// Example assertions to verify (values below assume DEADBAND = 0.05)
// modifyAxisCubic(1.0, 4.85) should return 4.85  (full forward = full speed)
// modifyAxisCubic(-1.0, 4.85) should return -4.85
// modifyAxisCubic(0.0, 4.85) should return 0.0 (inside deadband)
// modifyAxisCubed(1.0, 12.57) should return 12.57

Run with: ./gradlew test

---

Key thing to verify (from the review comments)

The open question from @irosenbery is whether the negative sign on the scale (-maxSpeed.in(MetersPerSecond)) in faceAngleWhileDrivingCommand (line 234–235) is correct. You can test this specifically by:

1. Enabling faceAngleWhileDrivingCommand (the face-angle drive mode)
2. Pushing the left stick forward — the robot should drive forward, not backward
3. If it goes backward, the inversion needs to move inside ControllerHelper instead of being on the scale parameter

[AndrewWerner29]

@claude i can't seem to find the "swerve/commandSpeed"

[claude]

Claude finished @AndrewWerner29's task in 50s —— View job

---

Finding drive telemetry in AdvantageScope

My apologies — Swerve/CommandedSpeeds was incorrect. That field exists in the code but is not logged with Logger.recordOutput(), so it won't appear in AdvantageScope.

Here are the keys that actually exist under Swerve/ in the logs:

AdvantageScope key	What it shows
Swerve/TargetState	Target swerve module states (speed + angle per module) — best proxy for commanded motion
Swerve/CurrentState	Actual measured module states
Swerve/CurrentPose	Robot position on the field
Swerve/Rotation2d	Raw gyro heading

To verify the drive command is working correctly, use Swerve/TargetState:

• Push left stick fully forward → all 4 modules should show ~4.85 m/s speed and 0° angle (forward)
• Push left stick fully left → all 4 modules should show ~4.85 m/s speed and 90° angle (strafe left)
• Rotate right stick right → modules should show a rotation pattern (~12.57 rad/s equivalent)

If the module speeds are near 1.0 m/s instead of 4.85 m/s, the scaling fix didn't take effect. If forward stick gives backward motion, the inversion is still wrong.

---

[AndrewWerner29]

@claude pushing the right stick fully left is inverted, how do i fix this

[claude]

Claude finished @AndrewWerner29's task in 1m 13s —— View job

---

Fixed Inverted Right Stick

• Read relevant files
• Identify the issue
• Implement the fix
• Commit and push

Root cause: In fieldOrientedDriveCommand, velXMps and velYMps were already negated with - in front of the helper call, but omegaRps was not. On an Xbox controller, pushing the right stick left gives a negative raw value — but WPILib convention is that positive omegaRadiansPerSecond = counter-clockwise (turning left). Without the negation, a left stick push produced negative omega → robot turned right (wrong direction).

Fix applied (CommandSwerveDrivetrain.java line 136):

// Before
double omegaRps =
    ControllerHelper.modifyAxisCubed(
        driveCont.getRightX(), maxAngularVelocity.in(RadiansPerSecond) / 2.0);

// After
double omegaRps =
    -ControllerHelper.modifyAxisCubed(
        driveCont.getRightX(), maxAngularVelocity.in(RadiansPerSecond) / 2.0);

This is now consistent with how velXMps and velYMps are handled.

[JRTaylord]

You're almost at the finish line, just a few more changes and then final testing and you're good to merge this in

[JRTaylord]

This looks great! Just update it from main and then have Kaleb test how it drives, then I'll merge it in

[JRTaylord]

Add in credits to 3663 for the helper class

[JRTaylord]

Needs to pass formatting build processes

[Hunter-A-T]

@claude yo clanka make a list of all the things i need to do to fix this

[claude]

Claude finished @Hunter-A-T's task in 55s —— View job

---

Here's everything left to fix before this PR can merge:

Outstanding items

Code changes (in ControllerHelper.java):

• Add a credit comment to Team 3663 at the top of the class — JRTaylord requested this on line 11
• Fix the @return Javadoc on modifyAxisCubic — it currently says "inverted axis value" but the method does NOT invert (the caller does the negation). JRTaylord confirmed the behavior is correct, just the comment is wrong.

Build / formatting:

• Run ./gradlew spotlessApply to fix formatting so the format check build passes — JRTaylord's last CHANGES_REQUESTED on 2026-04-05

Branch hygiene:

• Merge main into this branch (there are commits on main that post-date this branch) — JRTaylord said "just update it from main" on 2026-04-04

Testing (before merge):

• Have Kaleb drive-test the robot — JRTaylord said this is required before he'll merge it in

---

That's it — 4 code/infra tasks and 1 drive test. Once those are done and the formatting build passes, JRTaylord said he'll merge it.