Hello, I’m having an issue with PID loops. I have a simple turning PID loop that uses gyro measurements to reach a set point. The set point is 90 degrees clockwise. However, for some reason, the output of the PID loop is fluctuating from positive to negative and so forth even though the robot isn’t remotely close to reaching the set point. There are also big spikes where the output goes from around 0 to ~50 or -50 (which is obviously abnormal since my P value is 0.0028). Here’s a graph of the gyro measurement over time. The red line indicates the set point.
Here are some logs as well:
---
PID output: 0.23409657029593428
Gyro 6.394082037166332
----
PID output: 0.23409657029593428
Gyro 6.394082037166332
----
PID output: 0.23409657029593428
Gyro 6.394082037166332
----
PID output: 0.16576128203155518
Gyro 6.40091365314029
----
PID output: 0.23407744177120718
Gyro 6.40091365314029
----
PID output: -10.131908201013147
Gyro 7.437222051067308
----
PID output: 0.23117577825701152
Gyro 7.437222051067308
----
PID output: 0.23117577825701152
Gyro 7.437222051067308
----
PID output: -34.93454030683504
Gyro 10.952809295148171
----
PID output: 0.22133213397358512
Gyro 10.952809295148171
----
PID output: 13.162503872632346
Gyro 9.659054372660592
----
PID output: 0.22495464775655033
Gyro 9.659054372660592
----
PID output: 0.22495464775655033
Gyro 9.659054372660592
----
PID output: 37.499900885741376
Gyro 5.932603155202993
----
PID output: 0.2353887111654316
Gyro 5.932603155202993
----
PID output: 34.1763810613627
Gyro 2.539454001946181
----
PID output: 0.2448895287945507
Gyro 2.539454001946181
----
PID output: 0.2448895287945507
Gyro 2.539454001946181
----
PID output: -54.58354211398988
Gyro 8.020762399873199
----
PID output: 0.22954186528035503
Gyro 8.020762399873199
----
PID output: -82.22431503904241
Gyro 16.263840028569447
----
PID output: 0.20646124792000556
Gyro 16.263840028569447
----
PID output: 0.20646124792000556
Gyro 16.263840028569447
----
PID output: -30.044361232652005
Gyro 19.288075490697253
----
PID output: 0.19799338862604768
Gyro 19.288075490697253
----
PID output: 54.69035530738005
Gyro 13.84036465785504
----
PID output: 0.21324697895800587
Gyro 13.84036465785504
----
PID output: 0.21324697895800587
Gyro 13.84036465785504
----
PID output: 31.37292372848143
Gyro 10.725269209628214
----
PID output: 0.221969246213041
Gyro 10.725269209628214
----
PID output: 45.14593981564094
Gyro 6.234129671756021
----
PID output: 0.23454443691908314
Gyro 6.234129671756021
----
PID output: 0.23454443691908314
Gyro 6.234129671756021
----
PID output: -45.9451200750961
Gyro 10.850803454298424
----
PID output: 0.2216177503279644
Gyro 10.850803454298424
----
PID output: -88.55347155332092
Gyro 19.725827377964695
----
PID output: 0.19676768334169886
Gyro 19.725827377964695
----
PID output: 0.19676768334169886
Gyro 19.725827377964695
----
PID output: -23.205655601750472
Gyro 22.065414622045562
----
PID output: 0.1902168390582724
Gyro 22.065414622045562
----
PID output: 61.05234256165852
Gyro 15.980905712280277
----
PID output: 0.20725346400561523
Gyro 15.980905712280277
----
PID output: 0.20725346400561523
Gyro 15.980905712280277
----
PID output: 38.376352429587435
Gyro 12.165064251331165
----
PID output: 0.21793782009627274
Gyro 12.165064251331165
----
PID output: 0.21793782009627274
Gyro 12.165064251331165
----
PID output: 46.884262908081055
Gyro 7.49973803387357
----
PID output: 0.23100073350515402
Gyro 7.49973803387357
----
PID output: -40.51608539004855
Gyro 11.573306047185207
----
PID output: 0.2195947430678814
Gyro 11.573306047185207
----
PID output: -97.01170779135026
Gyro 21.293714586236092
----
PID output: 0.19237759915853894
Gyro 21.293714586236092
----
PID output: 0.19237759915853894
Gyro 21.293714586236092
----
PID output: -121.05031790901046
Gyro 33.41459029185542
----
PID output: 0.15843914718280483
Gyro 33.41459029185542
----
and so on.
How can I fix this?
Here’s my code:
PID command:
package frc.robot.commands;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import frc.robot.Constants.DriveConstants;
import frc.robot.subsystems.Drivetrain;
import java.util.ArrayList;
import java.io.FileWriter;
import java.io.IOException;
/** A command that will turn the robot to the specified angle. */
public class TurnToAngle extends PIDCommand {
/**
* Turns to robot to the specified angle.
*
* @param targetAngleDegrees The angle to turn to
* @param drive The drive subsystem to use
*/
public static final double kTurnP = 0.0028; // anything below ~0.0025 results in less than 0.18 power which is not enough to get robot to turn against static friction
public static final double kTurnI = 0;
public static final double kTurnD = 0.2;
private static long m_startTime;
public static ArrayList<String> data = new ArrayList<String>();
public TurnToAngle(double targetAngleDegrees, Drivetrain drive) {
super(
new PIDController(kTurnP, kTurnI, kTurnD),
// Close loop on heading
drive::getHeading,
// Set reference to target
targetAngleDegrees,
// Pipe output to turn robot
// output -> drive.arcadeDrive(0, output),
output -> {
drive.arcadeDrive(0, clamp(-output, -1, 1));
// drive.arcadeDrive(0, 0.4);
long time = System.currentTimeMillis() - m_startTime;
data.add(String.format("%d;%f", time, drive.getHeading()));
System.out.println("----");
System.out.println("PID output: " + Double.toString(output));
System.out.println("Gyro " + Double.toString(drive.getGyroAngleZ()));
},
// Require the drive
drive
);
// Set the controller to be continuous (because it is an angle controller)
getController().enableContinuousInput(-180, 180);
// Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
// setpoint before it is considered as having reached the reference
getController()
.setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
m_startTime = System.currentTimeMillis();
}
public static double clamp(double num, double min, double max)
{
if (num < min) { return min; }
if (num > max) { return max; }
return num;
}
@Override
public boolean isFinished() {
// End when the controller is at the reference.
if (getController().atSetpoint()) {
try {
FileWriter writer = new FileWriter("tuning_data.txt");
for(String str: data) {
writer.write(str + System.lineSeparator());
}
writer.close();
System.out.println("WRITTEN");
} catch(IOException e) {
System.out.println(e);
}
}
return getController().atSetpoint();
}
}
Drivetrain code (it is the default pololu romi code plus one method I defined myself, specifically the getHeading method)
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.romi.RomiGyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Drivetrain extends SubsystemBase {
private static final double kCountsPerRevolution = 1440.0;
private static final double kWheelDiameterInch = 2.75591; // 70 mm
// The Romi has the left and right motors set to
// PWM channels 0 and 1 respectively
private final Spark m_leftMotor = new Spark(0);
private final Spark m_rightMotor = new Spark(1);
// The Romi has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
private final Encoder m_leftEncoder = new Encoder(4, 5);
private final Encoder m_rightEncoder = new Encoder(6, 7);
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
// Set up the RomiGyro
private final RomiGyro m_gyro = new RomiGyro();
// Set up the BuiltInAccelerometer
private final BuiltInAccelerometer m_accelerometer = new BuiltInAccelerometer();
/** Creates a new Drivetrain. */
public Drivetrain() {
SendableRegistry.addChild(m_diffDrive, m_leftMotor);
SendableRegistry.addChild(m_diffDrive, m_rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
// Use inches as unit for encoder distances
m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
resetEncoders();
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
}
public void resetEncoders() {
m_leftEncoder.reset();
m_rightEncoder.reset();
}
public int getLeftEncoderCount() {
return m_leftEncoder.get();
}
public int getRightEncoderCount() {
return m_rightEncoder.get();
}
public double getLeftDistanceInch() {
return m_leftEncoder.getDistance();
}
public double getRightDistanceInch() {
return m_rightEncoder.getDistance();
}
public double getAverageDistanceInch() {
return (getLeftDistanceInch() + getRightDistanceInch()) / 2.0;
}
/**
* The acceleration in the X-axis.
*
* @return The acceleration of the Romi along the X-axis in Gs
*/
public double getAccelX() {
return m_accelerometer.getX();
}
/**
* The acceleration in the Y-axis.
*
* @return The acceleration of the Romi along the Y-axis in Gs
*/
public double getAccelY() {
return m_accelerometer.getY();
}
/**
* The acceleration in the Z-axis.
*
* @return The acceleration of the Romi along the Z-axis in Gs
*/
public double getAccelZ() {
return m_accelerometer.getZ();
}
/**
* Current angle of the Romi around the X-axis.
*
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleX() {
return m_gyro.getAngleX();
}
/**
* Current angle of the Romi around the Y-axis.
*
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleY() {
return m_gyro.getAngleY();
}
/**
* Current angle of the Romi around the Z-axis.
*
* @return The current angle of the Romi in degrees
*/
public double getGyroAngleZ() {
return m_gyro.getAngleZ();
}
/** Reset the gyro. */
public void resetGyro() {
m_gyro.reset();
}
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngleZ(), 360);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
Robot container (there’s nothing much here, it just runs turnToAngle aiming at 90 degrees)
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import frc.robot.commands.ArcadeDrive;
import frc.robot.commands.AutonomousDistance;
import frc.robot.commands.AutonomousTime;
import frc.robot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.romi.OnBoardIO;
import edu.wpi.first.wpilibj.romi.OnBoardIO.ChannelMode;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.TurnToAngle;
/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...
private final Drivetrain m_drivetrain = new Drivetrain();
private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
// NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
// that is specified when launching the wpilib-ws server on the Romi raspberry pi.
// By default, the following are available (listed in order from inside of the board to outside):
// - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
// - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
// - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
// - PWM 2 (mapped to Arduino Pin 21)
// - PWM 3 (mapped to Arduino Pin 22)
//
// Your subsystem configuration should take the overlays into account
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
configureButtonBindings();
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Example of how to use the onboard IO
// Trigger onboardButtonA = new Trigger(m_onboardIO::getButtonAPressed);
// onboardButtonA
// .onTrue(new PrintCommand("Button A Pressed"))
// .onFalse(new PrintCommand("Button A Released"));
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// return new TurnToAngle(90, m_drivetrain);
// return new DriveDistance(-0.5, 10, m_drivetrain);
// return new AutonomousTime(m_drivetrain);
// return new AutonomousDistance(m_drivetrain);
return new TurnToAngle(90, m_drivetrain);
}
}
Robot.java (again nothing much to see here)
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
public Robot() {
// super(Constants.timeBetweenLoops);
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
@Override
public void disabledPeriodic() {}
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
// Get selected routine from the SmartDashboard
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
System.out.println("Autnoumous command issued");
}
}
/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
}