Gyroscope FeedForward and PID

[Gandagorn]

So I have some code here that should make our robot turn at a minimum of PI radians/second but I’ve never used a feedforward before and I was wondering if I’m missing something glaringly obvious:

        kP = SmartDashboard.getNumber("Rotational P", Constants.r_kP);
        kI = SmartDashboard.getNumber("Rotational I", Constants.r_kI);
        kD = SmartDashboard.getNumber("Rotational D", Constants.r_kD);
        double currentAngle = gyro.getAngle();
        double finalAngle = angle + desiredAngle;
        SmartDashboard.putNumber("final angle", finalAngle);
        SmartDashboard.putNumber("current angle", currentAngle);
        SmartDashboard.putNumber("Rotation Rate", gyro.getRate());

        gyroController.setPID(kP, kI, kD);

        if (finalAngle < 0) {
            if (currentAngle <= finalAngle) {
                drive.arcadeDrive(0, 0);
                return true;
            }
            drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI, 2), 0);
        }

        if (finalAngle > 0) {
            if (currentAngle >= finalAngle) {
                drive.arcadeDrive(0, 0);
                return true; 
            }
            drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI, 2), 0);
        }
        return false;
    }

also, with this setup, is the robot capped at rotating PI radians per second or will the PID controller allow for faster rotation? Thanks!

[FRC6302]

I am assuming you are trying to turn to a specific gyro angle here.

@Gandagorn

I have some code here that should make our robot turn at a minimum of PI radians/second

I am curious why you wanting to give it a minimum speed. It seems like that would probably make it way more likely to overshoot.

@Gandagorn

drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI, 2), 0);

I am not too familiar with this method (assuming you are using the wpilib DifferentialDrive class) but I feel like you want your first parameter (xSpeed) to be zero and your second parameter (zRotation) to be nonzero.

@Gandagorn

feedforward.calculate(Math.PI, 2)

I don’t think there is a clearly defined acceleration parameter you would want here so I think you should omit that argument.

This line might work better

drive.arcadeDrive(0, feedforward.calculate(gyroController.calculate(currentAngle, finalAngle) / 12)

This treats the output of the gyro PID as a velocity and then gives the velocity to the feedforward which then converts that velocity into a voltage. I divided by 12 because the drive method you are using wants the inputs between -1 and 1, and the voltage goes up to around 12. With this approach, you shouldn’t need the if statements because it should turn to the right angle regardless of where the final angle is.

You’ll want to characterize your drivetrain with SysID to get the feedforward constants to put in the constructor.

This thread talks more about combining feedforward and PID and you might find it helpful for trying to understand what is going on.

Disclaimer: lots of people know way more about this than me so they may have better advice

[Gandagorn]

@FRC6302

I am assuming you are trying to turn to a specific gyro angle here.

yes that is what I am trying to do.

@FRC6302

I am curious why you wanting to give it a minimum speed. It seems like that would probably make it way more likely to overshoot.

the reason I want to give it a minimum speed is because the robot won’t drive the distance I want it to for all angles
this has a better description:

ADXRS450 Gyroscope reading can't keep up with robot rotation

Sorry… hit the wall of my knowledge… I haven’t used sysid this year. We have manually tuned our subsystems.

@FRC6302

I am not too familiar with this method (assuming you are using the wpilib DifferentialDrive class) but I feel like you want your first parameter (xSpeed) to be zero and your second parameter (zRotation) to be nonzero.

Yeah I thought so too although for some reason the parameters are flipped and I have no idea why.

@FRC6302

I don’t think there is a clearly defined acceleration parameter you would want here so I think you should omit that argument.

That makes a lot of sense Thanks!!

[FRC6302]

I am looking at your github and seeing lots of weird things but I don’t know where exactly the error is coming from. What command are you using to turn to an angle, and where are you calling it from? And where are you setting what angle to turn to?

And to answer your question from the other thread you linked, SysID gives you the values that you pass into the SimpleMotorFeedforward constructor, namely kV, kS, and kA. The velocity argument that you pass into the feedforward.calculate() method must change depending on the error from the target, otherwise you would just move at a constant velocity the whole time. The feedforward method converts a desired velocity into a corresponding voltage to send to your motors.

Using the SimpleMotorFeedforward class is more complicated than just setting a minimum motor velocity to overcome static friction. Although I don’t think static friction is the problem, the class with fix that for you too with the kS term, so you will not have to explicitly put that in your code like you trying to

[Gandagorn]

@FRC6302

Using the SimpleMotorFeedforward class is more complicated than just setting a minimum motor velocity to overcome static friction. Although I don’t think static friction is the problem, the class with fix that for you too with the kS term, so you will not have to explicitly put that in your code like you trying to

my apologies for the confusion, the code above hasn’t been tested yet. I’m testing it tomorrow and I wondered if I had the feed forward correct.

@FRC6302

And to answer your question from the other thread you linked, SysID gives you the values that you pass into the SimpleMotorFeedforward constructor, namely kV, kS, and kA. The velocity argument that you pass into the feedforward.calculate() method must change depending on the error from the target, otherwise you would just move at a constant velocity the whole time. The feedforward method converts a desired velocity into a corresponding voltage to send to your motors.

How should my velocity setpoint change for different angle setpoints, is there some way I can a function that will output the velocity when given an angle?

@FRC6302

drive.arcadeDrive(0, feedforward.calculate(gyroController.calculate(currentAngle, finalAngle) / 12)

also, will this output a velocity? How does it gather the velocity from a position setpoint?

[FRC6302]

@Gandagorn

How should my velocity setpoint change for different angle setpoints, is there some way I can a function that will output the velocity when given an angle?

The line I showed you does this.

@Gandagorn

How does it gather the velocity from a position setpoint?

Basically it treats the output of the positional PID loop as a velocity, although I’m not sure if it has meaningful units. For example, when the position error is zero (meaning you are at the desired angle), you get a zero velocity output, which is good because if you are already at the correct angle then you don’t wanna move any more. When the position error is bigger (like 90 degrees away from the desired angle), you get a larger output of the PID which is just a larger velocity. You need a larger velocity in this case because you are farther away. As this larger velocity takes you towards the desired angle, the output of the PID should decrease (meaning velocity will decrease) and you will slowly come to a stop at the desired angle, if everything is tuned right.

The velocity output from the PID goes into the feedforward.calculate() method which converts it into a voltage to send to your motors.

Looking at the other thread though, I have a feeling it still won’t work even after you put this line in. So could you answer these questions?

@FRC6302

I am looking at your github and seeing lots of weird things but I don’t know where exactly the error is coming from. What command are you using to turn to an angle, and where are you calling it from? And where are you setting what angle to turn to?

[Gandagorn]

@FRC6302

What command are you using to turn to an angle

I am using this method in the DriveTrain class to turn to an angle which is called by the AngleCorrect command:

public boolean rotatePID() {
        kP = SmartDashboard.getNumber("Rotational P", Constants.r_kP);
        kI = SmartDashboard.getNumber("Rotational I", Constants.r_kI);
        kD = SmartDashboard.getNumber("Rotational D", Constants.r_kD);
        double currentAngle = gyro.getAngle();
        double finalAngle = angle + desiredAngle;
        SmartDashboard.putNumber("final angle", finalAngle);
        SmartDashboard.putNumber("current angle", currentAngle);
        SmartDashboard.putNumber("Rotation Rate", gyro.getRate());

        gyroController.setPID(kP, kI, kD);

        if (finalAngle < 0) {
            if (currentAngle <= finalAngle) {
                drive.arcadeDrive(0, 0);
                return true;
            }
            drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI), 0);
        }

        if (finalAngle > 0) {
            if (currentAngle >= finalAngle) {
                drive.arcadeDrive(0, 0);
                return true;
            }
            drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI), 0);
        }
        return false;
    }

@FRC6302

What command are you using to turn to an angle

The AngleCorrect Command

@FRC6302

And where are you setting what angle to turn to?

I set the angle using this method in the DriveTrain class:

public void setAngle(double a) {
        desiredAngle = a;
    }

this is called in the AngleCorrect Command.

[FRC6302]

Where are you actually calling the command though? You haven’t committed to your Github in 2 days, so maybe that’s why I am not seeing it.

[Bmongar]

The arcade drive method of differential drive has the first parameter as speed and the second as rotation rate. You have your rotation rate as 0 and your gyro pid is feeding into the speed

[Gandagorn]

each instance of the command is called in the autonomous classes. I think that code is a couple of commits behind but it still should be there.

[Gandagorn]

I thought so too although when I pass the parameters in as they are supposed to work the speed and rotation are switched so it rotates when it should drive straight and drives straight when it should rotate

[Gandagorn]

the github has been updated

[tharun2338]

You have to add an FeedForward constants as you did for Constants.r_kP, Constants.r_kI, Constants.r_kD

[Gandagorn]

those have been added in the Constants class after being tuned with SYSID

[tharun2338]

Did you call it

[Gandagorn]

yes in the constructor I instantiate the new simplemotorfeedforward with the constants