Code:
package Subsystems;
import Hardware.Arm;
import Hardware.Sensors.Sensors;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
/**
* PIDarm -
* @author Joel Gallant
*/
public class PIDarm extends PIDSubsystem {
private static final double Kp = 6.0;
private static final double Ki = 0.0;
private static final double Kd = 0.0;
private Arm arm = new Arm();
public PIDarm() {
super("PIDarm", Kp, Ki, Kd);
// setSetpoint() - Sets where the PID controller should move the system to.
// setStepointRange() - Sets the range that the setpoint may be in.
// enable() - Enables the PID controller.
setSetpointRange(arm.HIGH, arm.LOW);
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
}
protected double returnPIDInput() {
// Return your input value for the PID loop.
return Sensors.getArmPosition();
}
protected void usePIDOutput(double output) {
// Use output to drive your system, like a motor.
arm.move(output);
}
}
That's as simple as it gets. Yet it does nothing.
For further detail into the PIDOutput...
Code:
public void move(double speed){
if(speed > -0.05 && Sensors.getBack() && Sensors.getArmPosition() < HIGH) {
leftArm().set(speed*maxArmSpeed);
rightArm().set(speed*maxArmSpeed);
}else if(speed > 0.05 && Sensors.getFront() && Sensors.getArmPosition() > LOW) {
leftArm().set(speed*maxArmSpeed);
rightArm().set(speed*maxArmSpeed);
}else {
leftArm().set(0);
rightArm().set(0);
}
}
That's the method used for move arm.
leftArm() and rightArm() are as followed
Code:
private SpeedController leftArm;
private SpeedController rightArm;
private SpeedController leftArm() {
if(leftArm == null) {
leftArm = new Victor(PortMapping.armLeftChannel);
}
return leftArm;
}
private SpeedController rightArm() {
if(rightArm == null) {
rightArm = new Victor(PortMapping.armRightChannel);
}
return rightArm;
}
Maybe I am not seeing something, but this should not create a new allocation of the PWM.
BTW PWM port 5 is leftArm, which is the one that gets allocated wrongly.