YAGSL Rotation Issues

JoeFizzy · January 28, 2024, 1:29pm

We’re continuing to make progress with our Swerve on YAGSL build, but we’ve run into a problem when trying to rotate the robot.

For testing purposes, we started with the YAGSL-Example project (we updated to the latest code yesterday).

Our Swerve modules are custom-designed but use Neos for the drive and Angle motors and CTRE Mag Encoders for the Absolute encoders.

We are able to move in both the x and y coordinates successfully. When we rotate, however, the robot will begin to rotate, then eventually will start to “ping pong” back and forth. The ping pong effect may happen very quickly (after a few degrees of rotation) or it may happen after about 180 degrees of rotation. It never rotates more than about 180 degrees.

Here are the things we’ve tried:

We’ve verified that rotating the chassis CCW increases the IMU/gyro heading.
We’ve verified that rotating each individual swerve module increases the readings on both the absolute and relative encoders
We’ve verified that forward power on the controller moves the robot forward as expected
We’ve tried adjusting the P value in controllerproperties.json – increasing the value seems to increase the rotational speed, but both large and small values of P experience the rotational issue.
We haven’t adjust the drive and angle motor PID values because the individual modules appear to be working properly (the individual modules rotate to reasonable positions and move in the correct direction until rotation is applied).

I didn’t have a chance to capture of video of the FRC Web Components, but when I looked at it, the wheels appeared to be pointing in the correct directions, but the screen showed red and blue arrows pointing away from each other.

This looks a lot like a PID tuning issue, except that the ping-ponging never stops – it will keep going indefinitely until we either drive using the left stick or disable the robot.

Any suggestions on how to troubleshoot this further? Our code can be found here:

https://github.com/FRC3620/FRC3620_2024_Cheep-Cheep_YAGSL/tree/submoduled

Fletch1373 · January 28, 2024, 2:59pm

I’m not so sure I would trust this comment right now. The comment above lines 60-63 in RobotContainer.java contradicts what the code is actually doing. You invert the right side (rotation) values but not the left side (translation) values. If both inputs AND motor inversion are wrong, then it’ll still drive straight, but it’ll be wrong when rotating as you describe.

ETA:

I would do the following:

Set your robot so it matches this diagram: When to invert? - YAGSL
Pull up the REV hardware client and double-check the motor configuration to see whether it is internally inverted (reset back to false if so)
Manually manipulate each drive motor and watch what each is reporting is happening from its internal encoder. Turning the wheel CCW (from the left side/bevel gear perspective as shown in the diagram) should result in a positive change in internal encoder ticks. If not, change “invert->drive” for that module’s json.
Repeat step 3, but with angle motors. From a top-down perspective, a CCW rotation of the wheel should result in a positive change in the internal encoder count. If not, change “invert->angle” in the module json.
Open up Phoenix Tuner(or whatever the latest tool they have is), and repeat step 4 watching what the absolute encoder is seeing. From a top-down perspective, a CCW rotation of the wheel should result in a positive change in the absolute encoder count. If not, set “absoluteEncoderInverted” in the module json to true
Verify the same as step 5, but with the robot as a whole, and invert your gyro if necessary. You indicated in your first bulletpoint that this is already correct, so I’m not putting much emphasis on it.
Deploy code and try to drive. If translation (forward/back or left/right) or rotation is backwards at this point, then invert the appropriate input in RobotContainer.java at the lines I mentioned above.
If changes made in step 7, deploy again, and you should have a nicely controlled swerve bot.

fovea1959 · January 28, 2024, 8:36pm

I worked with @JoeFizzy on this. We put some additional telemetry into the SwerveController.getRawTargetSpeeds() and SwerveController.headingCalculate() methods¹ to put the targetHeadingAngle up on the dashboard.

I don’t remember the exact weirdness, but the actual heading was bouncing around the targetHeadingAngle +/- 180 degrees when the ‘turn’ joystick was released. As I remember, manually forcing a turn worked correctly, as well as manual x/y translation of the chassis.

We are pretty sure that the individual modules are working correctly (would it turn and drive correctly under manual control), but we suspect something wonky around the thetaController in SwerveController.

¹ why doesn’t getRawTargetSpeeds() use headingCalculate()?

nstrike · January 28, 2024, 9:06pm

Honestly, it should.

github.com

BroncBotz3481/YAGSL-Example/blob/dev/src/main/java/swervelib/SwerveController.java#L207


      
           * @param targetHeadingAngleRadians  Target heading in radians.
           * @param currentHeadingAngleRadians Current heading in radians.
           * @return {@link ChassisSpeeds} the robot should move to.
           */
          public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double targetHeadingAngleRadians,
                                                  double currentHeadingAngleRadians)
          {
            // Calculates an angular rate using a PIDController and the commanded angle. Returns a value between -1 and 1
            // which is then scaled to be between -maxAngularVelocity and +maxAngularVelocity.
            return getRawTargetSpeeds(xSpeed, ySpeed,
                                      thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) *
                                      config.maxAngularVelocity);
          }
          
          /**
           * Calculate the angular velocity given the current and target heading angle in radians.
           *
           * @param currentHeadingAngleRadians The current heading of the robot in radians.
           * @param targetHeadingAngleRadians  The target heading of the robot in radians.
           * @return Angular velocity in radians per second.
           */

I have been contemplating on removing SwerveController entirely but decided against it bc i didn’t have an example replacement up before the v1 release.

However you are correct. Maybe i need the wrapping to be changed from -Pi to Pi to 0 to 2Pi

github.com

BroncBotz3481/YAGSL-Example/blob/dev/src/main/java/swervelib/SwerveController.java#L51


      
          /**
           * Construct the SwerveController object which is used for determining the speeds of the robot based on controller
           * input.
           *
           * @param cfg {@link SwerveControllerConfiguration} containing the PIDF variables for the heading PIDF.
           */
          public SwerveController(SwerveControllerConfiguration cfg)
          {
            config = cfg;
            thetaController = config.headingPIDF.createPIDController();
            thetaController.enableContinuousInput(-Math.PI, Math.PI);
            lastAngleScalar = 0;
          }
          
          /**
           * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link ChassisSpeeds}.
           *
           * @param speeds Chassis speeds.
           * @return {@link Translation2d} of the speed the robot is going in.
           */
          public static Translation2d getTranslation2d(ChassisSpeeds speeds)

fovea1959 · January 29, 2024, 2:54am

we double checked all the encoder and directions, got those correct.

part of the problem was that we don’t have a strong understanding of what the varitous *Drive commands in the YAGSL-Example do.

We do have the base working (form SwerveDrive on down), so right now, I think we’ll just layer our own heading controller on top and come back later.