// Called repeatedly when this Command is scheduled to run
protected void execute() {
// System.out.println(this.getName() + " executed");
int countsDiffLeft = goalCounts - leftArm.getEncoderCounts();
int countsDiffRight = goalCounts - rightArm.getEncoderCounts();
double leftCoefficient = calcSpeedCoefficient(countsDiffLeft);
double rightCoefficient = calcSpeedCoefficient(countsDiffRight);
double speedAdjustment = Climber.calcSpeedAdjustment();
// System.out.println(this.getName() + "goalCounts=" + goalCounts + " countsdiffleft=" +
// countsDiffLeft);
if (!leftArmDone) {
if (countsDiffLeft > countsTolerance) {
if (leftArm.isExtended()) {
// System.out.println("Left Arm Extended");
leftArm.stop();
leftArmDone = true;
} else {
// System.out.println("Set Left Arm to " + (defaultMovementSpeedNoLoad
// - speedAdjustment));
leftArm.setMotorSpeed((defaultMovementSpeedNoLoad - speedAdjustment) * leftCoefficient);
// leftArm.setMotorSpeed(defaultMovementSpeedNoLoad);
}
} else if (countsDiffLeft < -countsTolerance) {
if (leftArm.isRetracted()) {
// System.out.println("Left Arm Retracted");
leftArm.stop();
leftArmDone = true;
} else {
// System.out.println("Set Left Arm to " +
// (-defaultMovementSpeedUnderLoad - speedAdjustment));
leftArm.setMotorSpeed(
(-defaultMovementSpeedUnderLoad - speedAdjustment) * leftCoefficient);
// leftArm.setMotorSpeed(-defaultMovementSpeedUnderLoad);
}
} else {
// System.out.println("Left Arm In Range");
leftArm.stop();
leftArmDone = true;
}
}
// System.out.println(this.getName() + "goalCounts=" + goalCounts + " countsdiffright=" +
// countsDiffRight);
if (!rightArmDone) {
if (countsDiffRight > countsTolerance) {
if (rightArm.isExtended()) {
// System.out.println("Right Arm Extended");
rightArm.stop();
rightArmDone = true;
} else {
// System.out.println("Set Right Arm to " + (defaultMovementSpeedNoLoad
// + speedAdjustment));
rightArm.setMotorSpeed((defaultMovementSpeedNoLoad + speedAdjustment) * rightCoefficient);
// rightArm.setMotorSpeed(defaultMovementSpeedNoLoad);
}
} else if (countsDiffRight < -countsTolerance) {
if (rightArm.isRetracted()) {
// System.out.println("Right Arm Retracted");
rightArm.stop();
rightArmDone = true;
} else {
// System.out.println("Set Right Arm to " +
// (-defaultMovementSpeedNoLoad + speedAdjustment));
rightArm.setMotorSpeed(
(-defaultMovementSpeedUnderLoad + speedAdjustment) * rightCoefficient);
// rightArm.setMotorSpeed(-defaultMovementSpeedUnderLoad);
}
} else {
// System.out.println("Right Arm In Range");
rightArm.stop();
rightArmDone = true;
}
}
}
