Oscillating Motor - PID Subsystem

[Asymons]

Hello CD,
Yet another PID help thread, I apologize, but my team and I were having troubles with implementation of the PID Subsystem to properly work with our elevator lift. Originally we had the lift just stop once it went over a certain setpoint(which worked to some degree) but we now wanted a more accurate and elegant solution.

Setup:
We have two standard CIM motors as the output, and one encoder as the input. There is no CAN, just PWM and regular victors being used.

Issue:
-The implementation of the PID Subsystem results in the motor reaching the desired setpoint; however, oscillates when it reaches it.
-When the Robot is disabled quickly after the fear that the oscillation will break the elevator, when it is re-enabled it continues the PID command right when enabled without any buttons pressed. When the robot code is re-deployed or reset, this does not happen.

Attempted Solutions:
-Followed the GearBots Example, seems to have had some success, except for the oscillation.
-Lower P constant (I and D have remained at 0). Seems to not work, it has went as low as 0.02.
-Change the isFinished() method within the command to instead of detect for onTarget() method to a custom method that implemented our old code of the lift stopping after a certain setpoint. Did not work as well.
-Changed Absolute Tolerance and even tried Percent Tolerance, these were set to 200 ticks for the former and 30% for the latter. Still oscillation.

The Code:

Subsystem

package org.usfirst.frc.team4716.robot.subsystems;

import org.usfirst.frc.team4716.robot.RobotMap;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;

/**
 *
 */
public class PIDElevator extends PIDSubsystem {

	private DigitalInput limitSwitch;
	private Encoder elevEncoder;
	private SpeedController m1,m2;
	
    // Initialize your subsystem here
    public PIDElevator() {
    	super("PIDElevator",0.1,0.0,0.0);
    	setAbsoluteTolerance(5);
    	getPIDController().setContinuous(false);
    	
    	limitSwitch = new DigitalInput(1);
    	m1 = new Victor(4);
    	m2 = new Victor(5);
    	elevEncoder = new Encoder(RobotMap.ELEVATOR_ENCODER_PORT_1, RobotMap.ELEVATOR_ENCDER_PORT_2,
				  false, EncodingType.k4X);
        // 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 elevEncoder.pidGet();
    }
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
    	m1.set(-output);
    	m2.set(-output);
    }
    
    public boolean getLimit(){
    	return limitSwitch.get();	
    }
    
    public boolean isUp(double setpoint){
    	return (elevEncoder.get() >= setpoint) ? true:false;
    	
    }
    
    public void setMotor(double x){
    	m1.set(x);
    	m2.set(x);
    	
    }
}

Command

package org.usfirst.frc.team4716.robot.commands;

import org.usfirst.frc.team4716.robot.Robot;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class SetElevatorSetpoint extends Command {

	private double setpoint;
	
    public SetElevatorSetpoint(double setpoint) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	this.setpoint = setpoint;
    	requires(Robot.pidelevator);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.pidelevator.enable();
    	Robot.pidelevator.setSetpoint(setpoint);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	return Robot.pidelevator.onTarget();
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.pidelevator.disable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.pidelevator.disable();
    }
}

Sorry if the post is a bit lengthy, I just wanted to give as much information as possible to reduce the amount of replies to clarify information.

[GeeTwo]

I haven't looked at your details yet, but in general a larger magnitude for D (that is, derivative of the error, or velocity for a mechanical system going for a position) will act like a dashpot/shock absorber and dampen your system.

[Asymons]

Originally Posted by GeeTwo

I haven't looked at your details yet, but in general a larger magnitude for D (that is, derivative of the error, or velocity for a mechanical system going for a position) will act like a dashpot/shock absorber and dampen your system.

Thanks for the advice. Once you read the details, I have a couple of questions: Are you suggesting I should add in the derivative of error to fix the oscillation of the motor?
How would the tuning work if the system has an issue with just a P-closed loop essentially? Would that not continue having error in the sense of a continued oscillation?

[GeeTwo]

Originally Posted by Asymons

Thanks for the advice. Once you read the details, I have a couple of questions: Are you suggesting I should add in the derivative of error to fix the oscillation of the motor?

No, just to use a non-zero, probably constant value of D. It is convenient to think of D as standing for dampening as well as derivative.

Originally Posted by Asymons

How would the tuning work if the system has an issue with just a P-closed loop essentially? Would that not continue having error in the sense of a continued oscillation?

A P-only PID feedback system is (assuming that the mechanical system is linear) is mathematically identical to an undamped spring-and-mass; you would expect it to oscillate for a long time. When the system is far from the target, it is accelerated towards the target. By the time it reaches the target, it has significant speed, but there is no frictional term to slow it down, so it overshoots. You have essentially made a system in which F=kx (Hooke's Law). In terms of a differential equation, it becomes the simple x''=(k/m)x, where k/m is proportional to your P term. If you have done calculus 1 it is easy to verify that one solution to this equation is x=sin(sqrt(k/m)t), that is displacement is described as an undamped sine wave. Some sort of friction term is required to dissipate the initial "potential energy"; neither P nor I do this. Both D and mechanical friction will dampen the oscillations.

[Asymons]

Originally Posted by GeeTwo

No, just to use a non-zero, probably constant value of D. It is convenient to think of D as standing for dampening as well as derivative.

A P-only PID feedback system is (assuming that the mechanical system is linear) is mathematically identical to an undamped spring-and-mass; you would expect it to oscillate for a long time. When the system is far from the target, it is accelerated towards the target. By the time it reaches the target, it has significant speed, but there is no frictional term to slow it down, so it overshoots. You have essentially made a system in which F=kx (Hooke's Law). In terms of a differential equation, it becomes the simple x''=(k/m)x, where k/m is proportional to your P term. If you have done calculus 1 it is easy to verify that one solution to this equation is x=sin(sqrt(k/m)t), that is displacement is described as an undamped sine wave. Some sort of friction term is required to dissipate the initial "potential energy"; neither P nor I do this. Both D and mechanical friction will dampen the oscillations.

Thank you for this excellent response! As soon as I get access to the robot again, I'll try adjusting the variables and add a D constant. Hopefully this is the only issue, otherwise I'll be back here. Would it be acceptable to private message you if another issue arises as it seems you have some experience dealing with PID loops? I'll try not to be too bothersome

[GeeTwo]

Originally Posted by Asymons

Thank you for this excellent response! As soon as I get access to the robot again, I'll try adjusting the variables and add a D constant. Hopefully this is the only issue, otherwise I'll be back here. Would it be acceptable to private message you if another issue arises as it seems you have some experience dealing with PID loops? I'll try not to be too bothersome

Certainly, though my PID knowledge is far more theoretical than practical. If you don't mind, though, it probably makes more sense to put this on the open forum where someone else may benefit. I know there are a lot of "lurkers" who never post answers and rarely ask questions themselves, but find answers on CD regularly. CD is here for them, too!

--Gus

[otherguy]

Regarding the command continuing to run between the robot being enabled/disabled:

Your command will only finish if the onTarget method returns true.

protected boolean isFinished() {
    	return Robot.pidelevator.onTarget();
    }

When the robot is disabled commands aren't automatically disabled, the command is still trying to move the motors, the outputs to the motor controllers are disabled. So when you re-enable, the command continues to try to get the elevator to the setpoint.

Add something like:

Scheduler.getInstance().removeAll();
Scheduler.getInstance().disable();

in your disabled periodic() method inside your main robot class. This will disable any command that's active when the robot is disabled.

[Asymons]

In that case, Gus' suggestion, as well as James worked!

So my solution was as followed:
-Added the D value, set it to 1.0 and eventually lowered this value to 0.1 due to the lift "skipping".
-Absolute Tolerance was adjusted from 200 to 20, which it finally remained at 50 to keep the lift moving smoothly.
-P value was adjusted to 0.05
-Added Robot.elevator.disable(); at end of command

Doing all this, I was able to get the lift to stop at a set point, or have it skip once and get to the set point.

I'm most likely going to learn common tuning practices;however, I've mostly seen people use the "guess and check" method.