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
Code:
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() {
}
}