Using a PID subsystem to move to vision angle

we are using a PID subsystem to line up to be square with the target. We are using a PID subsystem and we use the setsetpoint(15) command but it blows pas the target and spins in circles. Does anyone know what we’re doing wrong

Forgot to upload the code here it is:
/* Copyright © 2018 FIRST. All Rights Reserved. /
Open Source Software - may be modified and shared by FRC teams. The code /
must be accompanied by the FIRST BSD license file in the root directory of /
the project. /

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import frc.robot.*;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

  • Add your docs here.
    public class PIDtest extends PIDSubsystem {
    double angle;
    String[] VisionValues;
    // network table init
    NetworkTable table;
    CANSparkMax MAXdrive = new CANSparkMax(OI.MAXtoprightdrive, MotorType.kBrushless);
    CANSparkMax MAXdrive2 = new CANSparkMax(OI.MAXtopleftdrive, MotorType.kBrushless);
    CANSparkMax MAXdrive3 = new CANSparkMax(OI.MAXbottomleftdrive, MotorType.kBrushless);
    CANSparkMax MAXdrive4 = new CANSparkMax(OI.MAXbottomrightdrive, MotorType.kBrushless);
    // network table init
    • Add your docs here.
      public PIDtest() {
      // Intert a subsystem name and PID values here
      super(“DriveSubsystem”, 1, 0, 0);
      try {
      NetworkTableInstance inst = NetworkTableInstance.getDefault();
      table = inst.getTable(“CVResultsTable”);
      VisionValues = table.getEntry(“VisionResults”).getString("").split(",");
      angle = Double.parseDouble(VisionValues[3]);
      } catch (Exception e) {
      System.out.println(“Jetson Not Running”);
      setOutputRange(-.2, .2);
      // Use these to get going:
      // setSetpoint() - Sets where the PID controller should move the system
      // to
      // enable() - Enables the PID controller.

public void initDefaultCommand() {
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());

protected double returnPIDInput() {
// Return your input value for the PID loop
// e.g. a sensor, like a potentiometer:
// yourPot.getAverageVoltage() / kYourMaxVoltage;
return angle;

protected void usePIDOutput(double output) {
// Use output to drive your system, like a motor
// e.g. yourMotor.set(output);

I am not a programmer* but here is some general troubleshooting advise: Run through your PID look by hand, doing the calculations manually. The person who write this code should not do it, someone else, while the programmer watches. When you approach the setpoint I think you will find the math gets wonky (maybe because something goes negative?).

*But I play one on TV

One thing I see that is clearly wrong is that you are always returning the same value from returnPIDInput(). The value returned from returnPIDInput() should indicate in some fashion how much further there is to go to get on target. For a turn or rotation this is typically a yaw value from a gyro. In your case, if your vision processing can keep up, it could be a value from the vision system. In either case, gyro or vision, you would need to be reading that each time returnPIDInput() is called and it would need to approach 15 as the robot turns in the test you are attempting.

I hope this helps,

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.