Positional pid control problems

I’m trying to perform positional pid control but the mechanism just keeps going on until I disable it. does anyone know what I could do differently to make this work.

subsystem;

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class DriveTrain extends SubsystemBase {

    CANSparkMax leftLeader;
    CANSparkMax rightLeader;
    
    CANSparkMax leftFollower;
    CANSparkMax rightFollower;

    Encoder leftEncoder;
    Encoder rightEncoder;
    ADXRS450_Gyro gyroscope;

    PIDController driveController;
    
    DifferentialDrive drive;

    public DriveTrain() {
        // instantiating drive train Motors
        leftLeader = new CANSparkMax(1, MotorType.kBrushless);
        rightLeader = new CANSparkMax(3, MotorType.kBrushless);

        leftFollower = new CANSparkMax(2, MotorType.kBrushless);
        rightFollower = new CANSparkMax(4, MotorType.kBrushless);

        // followers follow leaders
        leftFollower.follow(leftLeader, false);
        rightFollower.follow(rightLeader, false);

        // instantiating encoders (RoboRio DIO) and gyroscope (SPI)
        leftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
        rightEncoder = new Encoder(2, 3, true, EncodingType.k4X);
        gyroscope = new ADXRS450_Gyro();

        // reset encoders and gyroscope
        leftEncoder.reset();
        rightEncoder.reset();
        gyroscope.reset();

        // set pulse per revolution of encoders
        leftEncoder.setDistancePerPulse(Constants.DISTANCE_PER_PULSE);
        rightEncoder.setDistancePerPulse(Constants.DISTANCE_PER_PULSE);


        // instantiating PID controllers
        driveController = new PIDController(Constants.kP, Constants.kI, Constants.kD);

        // instantiating differential drive instance
        drive = new DifferentialDrive(leftLeader, rightLeader);
    }

    public void reset() {
        leftEncoder.reset();
        rightEncoder.reset();
    }

    public double getLeftDistance() {
        return leftEncoder.getDistance();
    }

    public double getRightDistance() {
        return rightEncoder.getDistance();
    }

    public void off() {
        leftLeader.set(0);
        rightLeader.set(0);
    }

    public void drive(double f, double t) {
        drive.arcadeDrive(f, t);
    }

    public void drivePID(double setpoint) {
        leftLeader.set(driveController.calculate(leftEncoder.getDistance(), setpoint));
        rightLeader.set(-(driveController.calculate(rightEncoder.getDistance(), setpoint)));
    }

    public boolean atSetpoint() {
        return driveController.atSetpoint();
    }

    @Override
    public void periodic() {
        
    }
}

command:

package frc.robot.commands.AutonomousCommands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.DriveTrain;

public class DriveDistance extends CommandBase {
    DriveTrain driveTrain;
    double setpoint;

    public DriveDistance(DriveTrain DT, double s) {
        driveTrain = DT;
        setpoint = s;

        addRequirements(driveTrain);
    }

    // Called when the command is initially scheduled.
    @Override
    public void initialize() {
        // driveTrain.reset();
    }

    // Called every time the scheduler runs while the command is scheduled.
    @Override
    public void execute() {
        driveTrain.drivePID(setpoint);
        if (driveTrain.atSetpoint()) {
            driveTrain.off();
        }
    }

    // Called once the command ends or is interrupted.
    @Override
    public void end(boolean interrupted) {}

    // Returns true when the command should end.
    @Override
    public boolean isFinished() {
        return false;
    }
}

I would start by checking if the encoders are actually reporting correct values, e.g. when you move the chassis forward both encoders start approaching the setpoint, otherwise driveTrain.atSetpoint() will never return true. Also it seems like you are using one PID to control two different motors, it may work but I think It may be better if you get the average distance traveled by each encoder, feed that to the PID and store its output in a variable, which you can then give to the motors.

I would also recommend that you implement the check of wheter or not you reached the setpoint in the “isFinished()” method of the command, it helps the scheduler know when the command has been completed, otherwise it will never finish executing. The driveTrain.off() command would then be in the “end()” method, it gets called by the Command Scheduler once “isFinished()” returns true. For more information look into the wiki Commands — FIRST Robotics Competition documentation

so my encoders are giving me really high values. this is my distance per pulse calculation;

public static final double WHEEL_CIRCUMFERENCE = 0.4788;
    public static final double GEAR_BOX_RATIO = 10.71;
    public static final double PULSE_PER_REVOLUTION = 360;
    public static final double DISTANCE_PER_PULSE = PULSE_PER_REVOLUTION / GEAR_BOX_RATIO * WHEEL_CIRCUMFERENCE;

I am using E4T encoders from us digital. is my pulse per revolution wrong?

There could be two problems:
You use the same drivecontroller PID for both the left and right side. A PID controller will internally keep track of the last and the integral error, so you need one PID controller per side. They can and should probably be configured the same way, but you need one copy per side, can’t re-use one controller for multiple motors.
Secondly, the DifferentialDrive contains a safety timeout. You either need to call arcadeDrive(), or you need to call feed() to satisfy the timer. If you directly set the motors based on PID info, circumventing the differentialdrive, it will print timeout warnings and stop the motors.

How would i use the arcade drive. Would i just set the motorspeed to the arcade drive forward parameter?

For joystick-based teleop, you use the arcadeDrive which then talks to the left and right motors. For PID-based position control, you can directly talk to the left and right motors, but then need to call drive.feed() which tells the Differential drive to stay calm and NOT stop the motors because of a motor safety timeout.

So in your drivePID simply add drive.feed().

So i did this and the robot would go super fast and not stop just like before. I got my pid values from the characterization tool and im not sure whether my distance per pulse calculation is correct.

edit: Here is my new code with added changes:
subsystem:

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class DriveTrain extends SubsystemBase {

    CANSparkMax leftLeader;
    CANSparkMax rightLeader;
    
    CANSparkMax leftFollower;
    CANSparkMax rightFollower;

    Encoder leftEncoder;
    Encoder rightEncoder;
    ADXRS450_Gyro gyroscope;

    PIDController driveController;
    
    DifferentialDrive drive;

    public DriveTrain() {
        // instantiating drive train Motors
        leftLeader = new CANSparkMax(1, MotorType.kBrushless);
        rightLeader = new CANSparkMax(3, MotorType.kBrushless);

        leftFollower = new CANSparkMax(2, MotorType.kBrushless);
        rightFollower = new CANSparkMax(4, MotorType.kBrushless);

        // followers follow leaders
        leftFollower.follow(leftLeader, false);
        rightFollower.follow(rightLeader, false);

        // instantiating encoders (RoboRio DIO) and gyroscope (SPI)
        leftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
        rightEncoder = new Encoder(2, 3, true, EncodingType.k4X);
        gyroscope = new ADXRS450_Gyro();

        // reset encoders and gyroscope
        leftEncoder.reset();
        rightEncoder.reset();
        gyroscope.reset();

        // set pulse per revolution of encoders
        leftEncoder.setDistancePerPulse(Constants.DISTANCE_PER_PULSE);
        rightEncoder.setDistancePerPulse(Constants.DISTANCE_PER_PULSE);


        // instantiating PID controllers
        driveController = new PIDController(Constants.kP, Constants.kI, Constants.kD);

        // instantiating differential drive instance
        drive = new DifferentialDrive(leftLeader, rightLeader);
    }

    public void reset() {
        leftEncoder.reset();
        rightEncoder.reset();
    }

    public void drive(double f, double t) {
        drive.arcadeDrive(f, t);
    }

    public void drivePID() {
        leftLeader.set(driveController.calculate(leftEncoder.getDistance()));
        rightLeader.set(driveController.calculate(rightEncoder.getDistance()));
        drive.feed();
    }

    public boolean atSetpoint() {
        return driveController.atSetpoint();
    }

    public void setSetpoint(double setpoint) {
        driveController.setSetpoint(setpoint);
    }

    @Override
    public void periodic() {
        
    }
}

command:

package frc.robot.commands.AutonomousCommands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.DriveTrain;

public class DriveDistance extends CommandBase {
    DriveTrain driveTrain;
    double setpoint;
    boolean finished;

    public DriveDistance(DriveTrain DT, double s) {
        driveTrain = DT;
        setpoint = s;


        addRequirements(driveTrain);
    }

    // Called when the command is initially scheduled.
    @Override
    public void initialize() {
        driveTrain.reset();
        driveTrain.setSetpoint(setpoint);
    }

    // Called every time the scheduler runs while the command is scheduled.
    @Override
    public void execute() {
        driveTrain.drivePID();
        if (driveTrain.atSetpoint()) {
            finished = true;
        }
    }

    // Called once the command ends or is interrupted.
    @Override
    public void end(boolean interrupted) {}

    // Returns true when the command should end.
    @Override
    public boolean isFinished() {
        return finished;
    }
}

You may be able to get these working for testing, but I strongly advise upgrading to a more robust encoder model before going to competition. The AMT103, USD S4T, and Grayhill 63R are popular and robust choices.

that is my current plan. I just want to get it working for testing before I switch to a different encoder. however, the ppr on the e4t product page says 400-4000 so i’m not sure what to use for the ppr in the distance per pulse calculation.

This is not a unique position to be in - the terminology here is confusing and varies from vendor to vendor. WPILib considers a “pulse” to be a full quadrature period; not everyone agrees :wink:

For the e4t, it depends on the resolution of the particular encoder wheel you’ve pressed onto the shaft. Ideally those should have been labeled; if they were not, my best guess is that it is a 256 period-per-revolution encoder.

and that 256 should be used like this?

public static final double WHEEL_CIRCUMFERENCE = 0.4788;
    public static final double GEAR_BOX_RATIO = 10.71;
    public static final double PULSE_PER_REVOLUTION = 256;
    public static final double DISTANCE_PER_PULSE = PULSE_PER_REVOLUTION / GEAR_BOX_RATIO * WHEEL_CIRCUMFERENCE;

No. “Distance per pulse” is the distance we go in a single pulse, which gets smaller as the PPR gets larger. Why is your PPR is the numerator?

oh do I have it flipped?

should it be (gearbox ratio*wheel circumference)/ppr?

Possibly! Check the formula you’ve just posted with some example values and see if it does what you expect.

thanks!

1 Like

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