Issues with arm PID

Our programmers have been working on re-writing the code for our robot, and they’re currently working on a PID loop to keep the arm raised at a certain position. To get a reading of the arm’s position they have a pot. This code is supposed to hold the arm as it is when it reaches 3v, a value that we measured out beforehand. However, when we execute this code, all the motor does is weakly turn to push the arm down once the pot exceeds 3v, without attempting to lift it at all. This is baffling to me, as this is almost the exact same code as we use for out drivestraight function, which works perfectly.
Attached is the command for the arm subsystem

package org.usfirst.frc4.Team4Descent.commands;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.interfaces.Potentiometer;
import org.usfirst.frc4.Team4Descent.Robot;
import org.usfirst.frc4.Team4Descent.RobotMap;
import org.usfirst.frc4.Team4Descent.subsystems.Shooter;

/**
 *
 * @author Rachel
 */
public class ArmPosition extends Command {
    
   public double 
            speed,
            turn,
            angleMod,
            Kp,
            Kd,
            potRaw,
            botAngle,
            previousP;
   
    AnalogChannel shooterPOT = RobotMap.shooterPOT;
    
    public ArmPosition() {
        requires(Robot.shooter);
    
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        
       //Kp and Kd values for tuning
        Kp          = 0.07;
        Kd          = 0; //Percent
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        //PID loop
        double
            setPoint = 3.00,
            processVariable = shooterPOT.getVoltage(),
            PID_P = -(setPoint - processVariable),
            PID_D = (PID_P - previousP),
            PID = (PID_P * Kp) + (PID_D * Kd);

        //Sets the arm pivot motor equal to the output PID value
        Shooter.shooterTalon.set(PID); 

//Limits arm speed for testing
if(Shooter.shooterTalon.get() >= 0.05)
{
    Shooter.shooterTalon.set(0.05);
}else if(Shooter.shooterTalon.get() <= -0.05)
{
    Shooter.shooterTalon.set(-0.05);
}
	//Sets the previous process variable to the current one
        previousP = processVariable;
        
        System.out.println(shooterPOT.getVoltage());
       
    }

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

    // Called once after isFinished returns true
    protected void end() {
    }

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

Hi,

You have a very small kP value, likely just enough to move the arm when gravity helps (hence the observed motion on the way down). You also don’t have an integral. The integral term is used to increase the effort the longer a position error exists.

I.e., lets say you start with the poteniometer at 0v -> an error of 3v. The controller will apply 3*.07 = .21% - > 2.1v at the talon. What would move the arm? If 2.1v is not enough, it will never move. If you had an integral, it would start adding more power the longer the error existed. The kD term is only useful for sudden changes (derivate) in position, which you are not getting at the moment (plus you zeroed it).

Try increasing the kP variable, and adding an integral. Have you looked at the PID class provided?

Cheers,
Bart

I have a few questions about your design (a picture of your arm would greatly help understand your system!)

Can the motors to the arm be back driven?
Are the motors to the arm fighting gravity in one direction?
Should the motors still be “on” and stalled once the arm reaches the position at 3v?

Is sending 0.2 to the motor enough to raise it? Your Kp constant doesn’t look nearly large enough to control an arm against gravity, and I don’t see an I term to overcome the steady-state error you’ll have.

Start by increasing Kp until it oscillates, then dial it back just enough to stop the oscillations. If that doesn’t keep the arm where you want it to be, consider adding I to what’s now just a P controller.

The value is definitely not enough to move the arm, we had an issue earlier where the code was always sending a high value to the motor and it swung hard enough to almost break someone’s arm/the robot, so we cut it down for testing. However, the issue is that the arms are getting literally 0 power before they pass that 3 mark- When I print the pid value I get 0 and the motor lights don’t turn on till it passes that mark.

What kind of arm is it? For a lot of lifting / arm systems, it might be more beneficial to add a spring instead.

What voltage does your pot read at your low point (before passing 3)?
Stupid question, but just making sure the pot reads in advance.

In the low/start position, what are the other variables reading?

You are sending conflicting commands to the motor by having multiple calls to shooterTalon.set. If you want to limit the speed, you need to do that before calling set.

Besides which, you are limiting power to +/-5% of maximum. I don’t know your gear ratio, but that may simply not be enough to hold position.

we arent using PID at all on our arm.
PG188 in brake mode is all it takes to hold position.

The pot is reading ~3.5 at the lowest position of the arm and 1 at the highest

Even without the limiting code, the exact same movements are observed.

And I know the voltage is too low- the problem is that it isn’t outputting anything at all until the value is below 3 (arm up)

Given all those things, I would suggest finding out what the variables are actually reading; output each variable, and see where in your equation you are getting zero. Could you post those?

//Limits arm speed for testing
if(Shooter.shooterTalon.get() >= 0.05)
{
    Shooter.shooterTalon.set(0.05);
}else if(Shooter.shooterTalon.get() <= -0.05)
{
    Shooter.shooterTalon.set(-0.05);
}

These limits keep your values barely outside of the Talon’s deadbands, and your output could be getting deadbanded depending on calibration.

Also,

        previousP = processVariable;

should be

        previousP = PID_P;

although this won’t have any effect while Kd = 0.

EDIT: saw OP’s last post. Did you increase Kp?

Rachel, just as previous posts have stated, your kP value looks way too small. I’d recommend setting your kP to 1 and work from there. The goal is to make your P component barely oscillate. Once the P component oscillates, remove the oscillation by increasing your kD constant from 0 to what ever value achieves the goal with minimal or no oscillation.

Assuming you’ve implemented Connor’s recommendations, you should be fine. Feel free to contact us if you’d like some additional help.

The issue was with the arm kP. Thanks!