import edu.wpi.first.wpilibj.command.Command;
public class MoveIntakeArm extends Command {
+ private double targetAngle;
+ private double currentAngle;
+ private double targetSpeed;
+ private double SENSITIVITY_THRESHOLD = 0.1;
+ private double NO_MOVEMENT_SPEED = 0;
- public MoveIntakeArm() {
+ public MoveIntakeArm(double angle, double speed) {
requires(Robot.intakeArm);
+ targetAngle = angle;
+ targetSpeed = speed;
}
@Override
protected void initialize() {
- Robot.intakeArm.getArmAngle();
+ currentAngle = Robot.intakeArm.getArmAngle();
+ double difference = targetAngle - currentAngle;
+ if (difference > 0) {
+ Robot.intakeArm.setArmVoltage(targetSpeed);
+ } else {
+ Robot.intakeArm.setArmVoltage(-targetSpeed);
+ }
}
@Override
protected void execute() {
- if (Robot.intakeArm.getArmVoltage() == 1) {
- }
}
@Override
protected boolean isFinished() {
- return false;
+ currentAngle = Robot.intakeArm.getArmAngle();
+ double distance = Math.abs(currentAngle - targetAngle);
+ if (distance <= SENSITIVITY_THRESHOLD) {
+ return true;
+ } else {
+ return false;
+ }
+
}
@Override
protected void end() {
+ Robot.intakeArm.setArmVoltage(NO_MOVEMENT_SPEED);
}
@Override