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() 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() if tm.hasPeriodPassed(1.0): print("Gyro", self.pigeon.getYawPitchRoll()) print('Yaw',self.pigeon.getYawPitchRoll()) 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)