PID Tuning for timed robot

Hello,
Now that we have a robot that, for the most part, works as we intended, we are beginning to try some automation. The first step for us is to be able to rotate to a specific angle. We are using the pigeon IMU, which is working well.

What we did is rewrite the sample Navx code to work with the pigeon (we are using getYaw().

If we use wpilib.RobotDrive() as our drive base, we can tune (actually the default proportion works) it so it rotates to an angle perfectly.

However, when we update it to the wpilib.drive.differentialDrive(), We cannot keep it from oscillating (it either does not move or oscillates around the setpoint).

If we try to add the PID to the Tests section as in the sample, we get a depreciated api warning and there is no motor control slider on the dashboard.

Can we use the PID subsystem (which is the recommendation from FRC) with the timed robot framework?

Does anyone have any advice about where to start as far as converting the working .05 proportion from wpilib.RobotDrive() to wpilib.drive.DifferentialDrive()?

I spent some time in the simulator, and am seeing similar issues. The rotate to angle works with wpilib.RobotDrive(), but with wpilib.drive.DifferentialDrive, I can either have the proportion so low that the bot never reaches a setpoint, or it oscillates consistently by the same amount. I wonder what we are doing wrong.

Thank you all. Our first comp this year was so much smoother than privious because of the help of this forum. Our code is below.

#!/usr/bin/env python3

import wpilib
import ctre
from networktables import NetworkTables


class MyRobot(wpilib.SampleRobot):
    """This is a demo program showing the use of the Pigeon to implement
    a "rotate to angle" feature. This demo works in the pyfrc simulator.
    
    This example will automatically rotate the robot to one of four
    angles (0, 90, 180 and 270 degrees).
    
    This rotation can occur when the robot is still, but can also occur
    when the robot is driving.  When using field-oriented control, this
    will cause the robot to drive in a straight line, in whatever direction
    is selected.
    
    This example also includes a feature allowing the driver to "reset"
    the "yaw" angle.  When the reset occurs, the new gyro angle will be
    0 degrees.  This can be useful in cases when the gyro drifts, which
    doesn't typically happen during a FRC match, but can occur during
    long practice sessions.
    
    Note that the PID Controller coefficients defined below will need to
    be tuned for your drive system.
    """

    # The following PID Controller coefficients will need to be tuned */
    # to match the dynamics of your drive system.  Note that the      */
    # SmartDashboard in Test mode has support for helping you tune    */
    # controllers by displaying a form where you can enter new P, I,  */
    # and D constants and test the mechanism.                         */

    # Often, you will find it useful to have different parameters in
    # simulation than what you use on the real robot

    if wpilib.RobotBase.isSimulation():
        # These PID parameters are used in simulation
        kP = 0.02
        kI = 0.00
        kD = 0.00
        kF = 0.00
                #This is the Constructor for the Navx
        frontLeft_motor = ctre.WPI_TalonSRX(0)
    else:
        # These PID parameters are used on a real robot
        kP = 0.05
        kI = 0.00
        kD = 0.00
        kF = 0.00

    kToleranceDegrees = 2.0

    def robotInit(self):
        # Left motor 
        frontLeft_motor = ctre.WPI_TalonSRX(0)
        backLeft_motor = ctre.WPI_TalonSRX(1) 
        # Right motor
        frontRight_motor = ctre.WPI_TalonSRX(2)
        backRight_motor = ctre.WPI_TalonSRX(3)

        self.myRobot = wpilib.RobotDrive(
            frontLeft_motor, backLeft_motor, frontRight_motor, backRight_motor
        )
        self.myRobot.setExpiration(0.1)
        self.stick = wpilib.Joystick(0)

       
        self.pigeon = ctre.pigeonimu.PigeonIMU(frontLeft_motor)
        self.yaw = lambda: self.pigeon.getYawPitchRoll()[0]
        
        turnController = wpilib.PIDController(
        self.kP, self.kI, self.kD, self.kF,self.yaw , output=self
        )
        
        turnController.setInputRange(-180.0, 180.0)
        turnController.setOutputRange(-.7, .7)
        turnController.setAbsoluteTolerance(self.kToleranceDegrees)
        turnController.setContinuous(True)

        self.turnController = turnController
        self.rotateToAngleRate = 0

        
        #Put items on Shuffleboard
        self.sd = NetworkTables.getTable('SmartDashboard')

        # Add the PID Controller to the Test-mode dashboard, allowing manual  */
        # tuning of the Turn Controller's P, I and D coefficients.            */
        # Typically, only the P value needs to be modified.                   */
        wpilib.LiveWindow.addActuator("DriveSystem", "RotateController", turnController)


    def operatorControl(self):
        """Runs the motors with onnidirectional drive steering.
        
        Implements Field-centric drive control.
        
        Also implements "rotate to angle", where the angle
        being rotated to is defined by one of four buttons.
        
        Note that this "rotate to angle" approach can also
        be used while driving to implement "straight-line
        driving".
        """

        tm = wpilib.Timer()
        tm.start()

        self.myRobot.setSafetyEnabled(True)
        while self.isOperatorControl() and self.isEnabled():

            self.yaw = self.pigeon.getYawPitchRoll()[0]

            
            if tm.hasPeriodPassed(1.0):
                print("Gyro", self.pigeon.getYawPitchRoll())
                print('Yaw',self.pigeon.getYawPitchRoll()[0])
                pass

            rotateToAngle = False
            if self.stick.getRawButton(2):
                self.turnController.setSetpoint(0.0)
                rotateToAngle = True
            elif self.stick.getRawButton(3):
                self.turnController.setSetpoint(90.0)
                rotateToAngle = True
            elif self.stick.getRawButton(4):
                self.turnController.setSetpoint(179.9)
                rotateToAngle = True
            elif self.stick.getRawButton(5):
                self.turnController.setSetpoint(-90.0)
                rotateToAngle = True
            if self.stick.getRawButton(1):
                self.turnController.setSetpoint(90.0)
                rotateToAngle = True

            if rotateToAngle:
                self.turnController.enable()
                currentRotationRate = self.rotateToAngleRate
            else:
                self.turnController.disable()
                currentRotationRate = self.stick.getX()

            # Use the joystick Y axis for forward movement,
            # and either the X axis for rotation or the current
            # calculated rotation rate depending upon whether
            # "rotate to angle" is active.
            #
            # This works better for mecanum drive robots, but this
            # illustrates one way you could implement this using
            # a 4 wheel drive robot

            self.myRobot.arcadeDrive(self.stick.getY(), currentRotationRate)

            wpilib.Timer.delay(0.005)  # wait for a motor update time

    def pidWrite(self, output):
        """This function is invoked periodically by the PID Controller,
        based upon Pigeon yaw angle input and PID Coefficients.
        """
        self.rotateToAngleRate = output


if __name__ == "__main__":
    wpilib.run(MyRobot)

Might be worth reading the RobotDrive porting guide in the DifferentialDrive docs.

1 Like

Thank you. I took a look at the docs, but it did not mention a change in power save for automatically inverting the motors.

Perhaps you missed this part?

Inputs smaller than RobotDriveBase.kDefaultDeadband will be set to 0, …

arcadeDrive() is equivalent to RobotDrive.arcadeDrive() if a deadband of 0 is used

Emphasis mine.

1 Like

Thank you. I saw that, but did not realize we needed to declare it explicitly. In doing so, it worked perfectly (at least in sim, we will check later on our test drive base).