When creating a command for our hatch extender which is controlled by a limit switch, I’m following the WPI documentation and putting the method in the initialize() command to make the motor drive. However, the motor only briefly pulses and then stops, even though the isFinished() criteria isn’t being completed. The motor safety is off as well. I’m probably missing something obvious.
This is the command for the mechanism
public class FourBarLinkageCommand extends Command
{
boolean linkageDeployed = false;
public FourBarLinkageCommand()
{
linkageDeployed = false;
requires(Robot.fourBarLinkage);
System.out.println("Four Bar Linkage has been created");
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
}
// Called just before this Command runs the first time
@Override
protected void initialize()
{
System.out.println("linkage initialized");
Robot.fourBarLinkage.initializeCounter();
Robot.fourBarLinkage.hatchForward();
System.out.println("Initialized At " + java.lang.System.currentTimeMillis());
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute()
{
System.out.println("TIMESTAMP: " + java.lang.System.currentTimeMillis());
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished()
{
System.out.println(Robot.fourBarLinkage.isFrontSwitchSet() || Robot.fourBarLinkage.isRearSwitchSet());
return Robot.fourBarLinkage.isFrontSwitchSet() || Robot.fourBarLinkage.isRearSwitchSet(); // finishes when the limit switch is activated, shouldn't be an issue due to how slowly it goes
}
// Called once after isFinished returns true
@Override
protected void end()
{
Robot.fourBarLinkage.stopLinkage();
System.out.println("linkage finished at: " + java.lang.System.currentTimeMillis());
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted()
{
}
}
This is the subsystem for the mechanism
public class FourBarLinkageForHatch extends Subsystem
{
// Put methods for controlling this subsystem
// here. Call these from Commands.
WPI_TalonSRX fourBarLinkageTalon = RobotMap.fourBarLinkageTalon;
DigitalInput hatchArmSwitchFront = new DigitalInput(RobotMap.HATCH_ARM_SWITCH_ID_FRONT);
DigitalInput hatchArmSwitchBack = new DigitalInput(RobotMap.HATCH_ARM_SWITCH_ID_BACK);
Counter counterFront = new Counter(hatchArmSwitchFront);
Counter counterRear = new Counter(hatchArmSwitchBack);
@Override
public void initDefaultCommand()
{
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());
}
public void hatchForward()
{
fourBarLinkageTalon.set(ControlMode.PercentOutput, 0.8);
}
public void hatchBack()
{
fourBarLinkageTalon.set(ControlMode.PercentOutput, -0.8);
}
public void stopLinkage()
{
fourBarLinkageTalon.set(ControlMode.PercentOutput, 0);
}
public boolean isFrontSwitchSet(){
return counterFront.get() > 0; // increments when the limit switch is activated
}
public boolean isRearSwitchSet(){
return counterRear.get() > 0;
}
public void initializeCounter(){
counterFront.reset();
counterRear.reset();
}
}