Hi,
2523 is having a little trouble with our pid control system for our drive train. We’re using it to drive specified distances during autonomous, which it accomplishes without a problem. The issue we’ve run into is that when the robot reaches it’s destination, it doesn’t register as complete and allow the robot to move to the next command in the command group, but rather it just holds the robot on the assigned point. Can anyone determine what might be causing this?
For context, we use a Java, Command Based setup.
Code for our DriveForDistance Command (Obviously, ignore anything commented out):
public class DriveForDistanceUpdated extends Command {
CANTalon left;
CANTalon right;
double feet;
double speed;
double rots;
double checks;
public DriveForDistanceUpdated(double feet) {
// Use requires() here to declare subsystem dependencies
requires(Robot.driveTrain);
this.feet = feet;
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.driveTrain.reset();
Robot.driveTrain.motorL1.ClearIaccum();
Robot.driveTrain.motorR1.ClearIaccum();
Robot.driveTrain.motorL2.ClearIaccum();
Robot.driveTrain.motorR2.ClearIaccum();
Robot.driveTrain.motorL1.changeControlMode(TalonControlMode.Position);
Robot.driveTrain.motorR1.changeControlMode(TalonControlMode.Position);
Robot.driveTrain.motorL2.changeControlMode(TalonControlMode.Position);
Robot.driveTrain.motorR2.changeControlMode(TalonControlMode.Position);
//
// Robot.driveTrain.motorL1.configNominalOutputVoltage(+0, -0);
// Robot.driveTrain.motorL2.configNominalOutputVoltage(+0, -0);
// Robot.driveTrain.motorR1.configNominalOutputVoltage(+0, -0);
// Robot.driveTrain.motorL2.configNominalOutputVoltage(+0, -0);
//
// Robot.driveTrain.motorL1.configPeakOutputVoltage(+12, -12);
// Robot.driveTrain.motorL2.configPeakOutputVoltage(+12, -12);
// Robot.driveTrain.motorR1.configPeakOutputVoltage(+12, -12);
// Robot.driveTrain.motorR2.configPeakOutputVoltage(+12, -12);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
Robot.driveTrain.driveSet(feet*.64);
SmartDashboard.putNumber("Progress", Robot.driveTrain.motorR1.getPosition());
}
// Make this return true when this Command no longer needs to run execute() ||Robot.driveTrain.timedOut(.1,500)
protected boolean isFinished() {
if(checks>10){
return true;
}
if(Robot.driveTrain.motorL1.getPosition() < ((feet*.64)+.1) && Robot.driveTrain.motorL1.getPosition() > ((feet*.64)-.1) ){
checks++;
return false;
} else {
return false;
}
}
// Called once after isFinished returns true
protected void end() {
Robot.driveTrain.motorL1.changeControlMode(TalonControlMode.PercentVbus);
Robot.driveTrain.motorR1.changeControlMode(TalonControlMode.PercentVbus);
Robot.driveTrain.motorL2.changeControlMode(TalonControlMode.PercentVbus);
Robot.driveTrain.motorR2.changeControlMode(TalonControlMode.PercentVbus);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
Robot.driveTrain.motorL1.changeControlMode(TalonControlMode.PercentVbus);
Robot.driveTrain.motorR1.changeControlMode(TalonControlMode.PercentVbus);
Robot.driveTrain.motorL2.changeControlMode(TalonControlMode.PercentVbus);
Robot.driveTrain.motorR2.changeControlMode(TalonControlMode.PercentVbus);
}
}