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() {
}
}
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?
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.
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.
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?
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.