Snap to Heading

Technical Java
, , ,
1

Does anyone have a good example of snapping to a certain heading? I am trying to do this with inputs from POV. 1094 had this in 2019, but I cannot find it.

2

Yes, we did this last year and will do this again this year. Here is a comment I wrote last summer about our heading/drift correction logic; in addition to using it for heading correction, we also use it for snapping to an angle based on the D-pad. The TL;DR:

  • Use a PID controller to rotate the robot to a desiredHeading variable whenever the driver isn’t manually rotating the robot (you don’t want this to interfere with manual control).
  • For drift correction, set that heading when the driver is manually rotating (as that is the intended heading)
  • For snapping to an angle, just set that variable manually; it’ll use that PID controller to rotate itself to that angle.
2 Likes
3

I think your convertCardinalDirections() is what I needed. Does this look like it would work?

1 Like
4

I’ll look at it more once I’m on my computer, but it seems right. Make sure that this is only running when you’re not manually rotating it, otherwise it’ll get in the way.

5

All the drivetrain commands require the subsystem so that should not be an issue here.

1 Like
6

Looks like this should work. The main thing I noticed is that the RapidHeading command only runs when the POV isn’t in the center, so I think you will have to hold it down until you’ve snapped to the angle, but I might be wrong.

Your isFinished function also always returns false; I don’t know whether that will cause any issues. You might be able to modify that to make it so it finishes once you’ve snapped to the angle, which would probably fix the above issue.

7

What values do you use for your heading controller?

8

That behavior is intentional. The drivers want to use a flight stick, but this always leads to driving like a tank because it is difficult to rotate in a controlled way while moving. I am trying to use this as a sort of work-around so that a direction can be held while driving which is why isFinished returns false.

Is there an advantage to binding the trigger as onFalse rather than whileFalse? It seems that this should be an actively commanded thing that can be released immediately.

9

See we’re working on a Snap to heading function now. What i’m curious about is how teams in the past have handled rotation2d rollover at 180/-180 degrees.

10

Convert your joystick inputs to polar coordinates and do some simple math. Team 1806 does it like this:

public double snapRotation(){
        DoubleSupplier x = () -> -driverController.getRightX();
        DoubleSupplier y = () -> -driverController.getRightY();

        double[] rightJoyPolarCoordinate = PolarCoordinate.toPolarCoordinate(y,x);
    
        double r = rightJoyPolarCoordinate[0];
        double theta = Units.radiansToDegrees(rightJoyPolarCoordinate[1]);

        if(r <0.8){
            return RobotContainer.S_SWERVE.getSwerveDrive().getOdometryHeading().getDegrees();
        }

        theta /= 45;
        theta = Math.round(theta) * 45;
        return theta;
    }

Dividing by 45 degrees then rounding the answer then multiplying back by 45 degrees ensures that your only values will be 0,45,90,135,180,-135,-90,-45
Full repo is here if you want our PolarCoordinates class: GitHub - frc1806/Crescendo-1806: FRC Team 1806's 2024 Robot Code

NOTE: Our joysticks are negative and swapped around from x and y to be field oriented. Otherwise 0 degrees is pointing east

1 Like
11

The WPILib PIDController has an option to handle this automatically, using the method enableContinuousInput

3 Likes
12

Sorry I’m only responding to this now. We use a kP = 0.015, kI = 0.0, kD = 0.0. Only kP, so pretty simple. You might need/want to tune it better.

1 Like
13

Makes sense re: holding the button.

For your question about onFalse vs whileFalse, what trigger are you referring to?

14

I have that bound to POVCenter. This is effectively the same as using if(CommandJoystick.getHID.getPOV != 1) { foo }
to activate it.

15

Ah. I’d say since you want the driver’s to hold the button, do whileFalse. In our code all the button did was run an instantCommand to set a variable, so we did onFalse.

16

Okay, that concurs with my thought process as well.

1 Like
17

Why exactly does it add 77 to the heading?

18

We wanted that to be the offset, not 90.

1 Like
19

Oh, @four one thing to keep in mind: I believe that the degrees for the D-pad and the degrees for robot rotation in WPILib’s coordinate system are opposite, hence some of what we’re doing with adding values. I am not looking at the code right now so I can’t give specifics, but make sure you keep that in mind.

What’s in convertCardinalDirections should work but it’s good to understand how your code works in case you have any issues!

1 Like