# Turn to Angle Example

We are trying to get the TurnToAngle commands from this example working. Our code. It is currently either oscillating (until it times out at 5 sec) or not turning depending on the value of P that is used.

I think one of the things we need here is a static offset to help with friction. This obviously has to help according to the direction so can’t just be a constant. I think adding an F term might do this but am not sure how I would add an F term here. I started a bit of code in TurnToAngle that would add this offset but couldn’t actually get it working.

I noticed that our gyro is going from 180 to -180. I thought the pigeon wrapped? It looks like this is using the 180/-180 what is the advantage of that? Seems to just add complication around the point it crosses over?

What is the advantage/disadvantage of TurnToAngleProfiled vs regular? I couldn’t find much documentation on how these worked / how to use them.

I could have tried adding a D term too but from what I have heard this should work with just P so I think something else is wrong.

Unless someone knows a better way, the FeedForward (F) term would just be added or subtracted to the output. Something like this. I moved `useOuput` into a function because this would be a long lamdba:

``````public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
super(
new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
this::useOutput,
// Require the drive
drive);
...
}

public void useOutput(double output) {
if (output > 0) {
drive.arcadeDrive(0, output + F);
} else if (output < 0) {
drive.arcadeDrive(0, output - F);
} else {
drive.arcadeDrive(0, output)
}
}

``````

In the example, this line handles the fact that the gyro is 180 to -180 and not continuous. See the Javadoc:
`getController().enableContinuousInput(-180, 180);`

The TurnToAngle example will try to get to the target as fast as possible. The Profiled example will use a trapezoidal profile to accelerate and decelerate smoothly. See Trapezoidal Motion Profiles in WPILib for details.

In your opinion is the profiled motion worth it? I don’t think we are that concerned with jerk just want a quick and accurate turn

I hate to say it, but “quick” and “accurate” are mutually exclusive.

The Roborio is only capable of reacting to sensor input every 20ms. That means if your robot is charging across the floor at 10 meters per second, then it’s reading the distance sensors only once every 20 centimeters. If you tried to read your position at full speed, you could be off by as much as 20cm for that reason. Likewise, if you spin around at 180 degrees per second (which these robots are definitely capable of) then you’re ticking off 3.6 degrees between each sensor reading, so that’s how much error you could have in your turn.

That’s why the trapezoidal motion is so helpful. It’s fast for most of the travel, but slows down just as you get close to the goal angle/distance so that the roborio has time to verify that the sensor readings are correct. This basic principle is also the fundamental theory behind PID position control, albeit with a lot more parameters you can fiddle with.

1 Like

10m/s is 32.8 ft/s.

Correspondingly, a robot going at (a more reasonable) 15ft/s reads its distance sensors every 3.6inches, or 9.15cm at a rio loop of 20ms

Past experience tells me that this isn’t the case at least to the level of speed and accuracy I am hoping for. I have much more experience in LV and have done numerous turns there and I have seen countless other teams do this well. I know it won’t be super easy

My understanding of Trapezoidal profile is that it isn’t ramping down to ensure it hits the position (it may incidentally have that effect) but to ensure a smooth deceleration to reduce jerk, but maybe I am wrong?

We are using Talons. If the update rate is really the issue have teams had more success doing this onboard the Talons?

Talons are awesome. They, by contrast, have a ONE millisecond sensor/response cycle. I’m a fan. I try to do PID on the Talons when I can for precisely this reason. This year we’re using Talons with position PID on the drive motors for both driving and turning, which is scary fast if you don’t limit the output. Yes, PID tuning can be a bit tricky, and your wpilib commands will still need to detect when they’ve reached the goal distance/angle, but the Talon will handle the process of getting them there quickly and with correctly tapered output.

It would take a little more work on the Talons, but it should be possible. I think you’d want to use a Pigeon IMU connected to one of the Talons, differential auxiliary PID, and Motion Magic for the trapezoidal profile.

2 Likes

This makes literally no difference on most FRC mechanisms, especially because the default velocity filtering on the talon has a characteristic timescale of ~150ms.

2 Likes

I would be interested if anyone has a better way to apply feedforward. We can characterize a drivetrain and create a `SimpleFeedforward` instance, but is there a good way to apply it here?

The input for `SimpleFeedforward` is velocity (meters/sec usually) and the output is voltage so I’m not sure how to apply it when the input is degrees and the output is angular velocity as a percentage.

1 Like

Cascade from your turning loop to your drive controller. The drive controller can be feedforward-only (open loop); as long as it includes the `kS` term, it will correctly account for stiction.

The linked example uses the `DifferentialDrive` class, so I imagine that would be out. What would a feedforward-only drive controller look like? You could use `DifferentialDriveKinematics` to compute wheel velocities, but would you then just transform that to a voltage based on your max velocity?

We are using velocity closed-loop for our drivetrain, so this should work out of the box, but it would be great to have a feedforward-only fallback in case a drivetrain encoder fails (we’ve had tons of trouble with drivetrain encoders in the past).

Take the wheel velocities computed by `DifferentialDriveKinematics` and plug them into a `SimpleMotorFeedforward`. Send the resulting voltages to the motors.

1 Like

Genius! That is so simple it didn’t even occur to me.

This code didn’t seem to work. I think “drive” was out of context in useOutput

Can you ELI5 the SimpleMotorFeedForward stuff or point me to a similar example?

Yeah, `drive` would be out of scope if you don’t make another change. I would just create a field in the class to hang onto it.

``````private final DriveSubsystem drive;

public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
super(
new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
this::useOutput,
// Require the drive
drive);
this.drive = drive;
...
}

public void useOutput(double output) {
if (output > 0) {
drive.arcadeDrive(0, output + F);
} else if (output < 0) {
drive.arcadeDrive(0, output - F);
} else {
drive.arcadeDrive(0, output)
}
}
``````

An alternative would be to just make it a lambda, but it’s kind of a best practice not to have big lambdas.

``````public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
super(
new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
output () -> {
if (output > 0) {
drive.arcadeDrive(0, output + F);
} else if (output < 0) {
drive.arcadeDrive(0, output - F);
} else {
drive.arcadeDrive(0, output)
}
},
// Require the drive
drive);
...
}

public void useOutput(double output)
``````

Disclaimer: this is code I typed in here so it might have errors

https://docs.wpilib.org/en/latest/docs/software/advanced-control/controllers/feedforward.html

`SimpleMotorFeedForward` (SFF from here on) would be a better approach than using a `kF` constant, but you’ll probably have success with your approach. The idea is that you would use the Robot Characterization tool to find the characteristics of your robot. It finds the voltage required to overcome static friction, which is what we’re most concerned about here.

You then take the robot characteristics and construct an instance of SFF. SFF has a `calculate` method you call with a velocity and it will give you the voltage to apply. To figure out the wheel velocities to plug in, you can use `DifferentialDriveKinematics` which takes your wheelbase, and a forward and angular velocity (much like `arcadeDrive`).

If all you want to do is spin to an angle, this is probably too much effort for the payout. If you want to run trajectories or have other sophisticated drivetrain control you will have to go through this anyway, so you could also apply it when spinning to an angle.

The first example didn’t work because: “Cannot refer to ‘this’ nor ‘super’ while explicitly invoking a constructor”

The second had a couple of issues (semicolon in else case and no need for () before ->) but those I can solve figuring out how to construct the lamdas and such is still difficult for me. Makes sense when I see it.

``````    super(
new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
output -> {
if (output > 0) {
drive.arcadeDrive(0, output + F);
} else if (output < 0) {
drive.arcadeDrive(0, output - F);
} else {
drive.arcadeDrive(0, output);
}
},
// Require the drive
drive);
...
}
``````

I will probably try this Wednesday and then report back to see if I need to change to onboard or fancy FeedForward. I may change to the FeedForward anyway when/if I get the Ramsete stuff working.