SetAngle for turret not working properly

[xyyuii]

Hello, so I’m testing this system with a turret from another robot, to where when it reaches the max angle of the turret, it goes all the way around to the min angle, and when it’s done with that it goes back to tracking the target. However, for some reason, when it goes to turn to the max angle, it doesn’t go all the way and just stops midway for some reason. Could someone take a look at it and see if I’m just missing something? I’ve checked the PIDs, and they’re all working fine, and the setAngle and set methods are working fine too.

package frc.robot.subsystems;

import java.util.Map;

import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;

import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

/**
 * Class encapsulating turret function.
 */
public class Turret extends SubsystemBase {

    // device initialization
    private final CANSparkMax turretMotor = new CANSparkMax(23, MotorType.kBrushless);
    private final CANEncoder turretEncoder = turretMotor.getEncoder();
    private final ProfiledPIDController aimingPID = new ProfiledPIDController(/*0.021*/ 0.035, 0.0, 0.0, new Constraints(MAX_VELOCITY, MAX_ACCELERATION));  
    private final ProfiledPIDController anglePID = new ProfiledPIDController(0.03, 0.0, 0.0046, new Constraints());
    //0.1, 0.15
    // constants
    private static final double MAX_VELOCITY = 1.5; // rpm
    private static final double MAX_ACCELERATION = 0.75; // rpm / sec
    private static final double GEAR_RATIO = 40.0 * (120.0 / 16.0); // angular velocity will be divided by this amount
    private static final double ENCODER_TO_DEGREES = 360.0 / GEAR_RATIO; // degrees
    private static final double RAMP_RATE = 0.5; // seconds
    private static final double MAX_ANG = 255;
    private static final double MIN_ANG = -32;

    private double reduction = 1.0;

    private boolean MANUAL = false;
    private static boolean maxchanging = false;
    private static boolean minchanging = false;

    /**
     * Constructs new Turret object and configures devices.
     */
    public Turret() {
        resetEncoders();
        setRampRate();
        turretMotor.setIdleMode(IdleMode.kBrake);
        Notifier shuffle = new Notifier(() -> updateShuffleboard());        
        shuffle.startPeriodic(0.1);

        SmartDashboard.putNumber("Set T Angle P", anglePID.getP());
        SmartDashboard.putNumber("Set T Angle I", anglePID.getI());
        SmartDashboard.putNumber("Set T Angle D", anglePID.getD());

        SmartDashboard.putNumber("Set T Angle", 0.0);
    }

    /**
     * Runs every loop.
     */
    @Override
    public void periodic() {
    }

    /**
     * Writes values to Shuffleboard.
     */
    private void updateShuffleboard() {
        SmartDashboard.putNumber("Turret pos (deg)", getPosition());
        //SmartDashboard.putNumber("Turret temp", turretMotor.getMotorTemperature());
        //SmartDashboard.putNumber("Turret raw vel", turretEncoder.getVelocity());
    }

    public void setPIDs(double kP, double kI, double kD) {
        anglePID.setPID(kP, kI, kD);
    }

    /**
     * Sets turret motor to given percentage [-1.0, 1.0].
     */
    public void set(double percent) {   
            turretMotor.set(percent * reduction);
        }
        
    }

    /**
     * Turns turret to angle in degrees.
     */
    public void setAngle(double angle) {
        set(anglePID.calculate(getPosition(), angle));
    }

    public void trackTarget(double measure) {
        // set(aimingPID.calculate(measure, 0));

        
        if (getPosition() >= MAX_ANG || !maxchanging)
        {
            maxchanging = true;
            setAngle(-30);
            if (getPosition() <= -30) {
                maxchanging = false;

            }
        }
        else if (getPosition() <= MIN_ANG || !minchanging)
        {
            minchanging = true;
            setAngle(253);
            if (getPosition() >= 253) {
                minchanging = false;
            }
        }
        if (!maxchanging && !minchanging)
        {
            setAngle(getPosition() - measure);
        }
    }

    /**
     * Configures motor ramp rates.
     */
    public void setRampRate() {
        turretMotor.setClosedLoopRampRate(0);
        turretMotor.setOpenLoopRampRate(RAMP_RATE);
    }

    public void setReduction(double reduction) {
        this.reduction = reduction;
    }

    /**
     * Resets turret encoder value to 0.
     */
    public void resetEncoders() {
        turretEncoder.setPosition(0);
    }

    /**
     * Returns turret encoder position in degrees.
     */
    public double getPosition() {
        return turretEncoder.getPosition() * ENCODER_TO_DEGREES;
    }

    /**
     * Returns turret encoder velocity in degrees per second.
     */
    public double getVelocity() {
        return turretEncoder.getVelocity() * ENCODER_TO_DEGREES / 60.0;
    }

    /**
     * Returns max position of turret in degrees.
     */
    public double getMaxPosition() {
        return MAX_ANG;
    }

    /**
     * Returns min position of turret in degrees.
     */
    public double getMinPosition() {
        return MIN_ANG;
    }

    public boolean getManual() {
        return MANUAL;
    }

    public void setManual(boolean manual) {
        MANUAL = manual;
    }

    public void toggleManual() {
        MANUAL = !MANUAL;
    }

    /**
     * Returns turret motor temperature in Celsius.
     */
    public double getTemp() {
        return turretMotor.getMotorTemperature();
    }

}

[FRC6302]

If I am correctly understanding what you are trying to do, I think you need to rethink the logic in your trackTarget method. If maxchanging is false (which is what it is initially set to) then the first if statement in trackTarget runs, which I don’t think is what you are wanting to happen.

Maybe try changing

if (getPosition() >= MAX_ANG || !maxchanging)

to

if (getPosition() >= MAX_ANG || maxchanging)

and doing the same for the if statement right below.

[xyyuii]

Oh, so about that, we noticed that whenever it ran our track target code, it ran it over and over, and after it sees that the position of the turret is equal to or beyond the max_ang, it sets the angle to -30, however when the turret moves towards -30, the if statement (getPosition() >= MAX_ANG) isn’t met anymore, and so it would most likely cancel the loop, so we added a maxchanging variable to be false, and when the if statement is met, it changes it to true, and so the loop will continue to run until both the (getPosition() >= MAX_ANG) and maxchanging ) conditions aren’t met anymore. honestly, looking at the code, it should theoretically work, but for some reason it just won’t work, it goes through the loop, but everytime it goes to set the angle to -30, it starts turning but then suddenly stops midway,we don’t know what’s interrupting it at this point

[FRC6302]

Hmm. Did you try the code change I suggested? How did it effect what happened?

Also, what is the measure argument that you are passing into the trackTarget method? Can you post the code of where you are calling the method?

[xyyuii]

yes, we’ve tried it before and it didn’t work and for measure, we’re using limelight for the vision on our turret, so you’re basically putting in the angle the limelight detects in the measure parameter

this is the command that actually uses the trackTarget() method

package frc.robot.commands.turret;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Limelight;
import frc.robot.subsystems.Turret;

/**
 * Turns turret to keep vision target centered.
 */
public class AutoAim extends CommandBase {

    private final Turret turret;
    private final Limelight limelight;

    /**
     * Constructs new AutoAim command to turn turret to keep vision target centered.
     *
     * @param turret - Turret subsystem to use.
     * @param limelight - Limelight subsystem to use.
     */
    public AutoAim(Turret turret, Limelight limelight) {
        addRequirements(turret);
        this.turret = turret;
        this.limelight = limelight;
    }

    @Override
    public void initialize() {
        limelight.trackTarget();
    }

    @Override
    public void execute() {
        turret.trackTarget(limelight.getHorizontalAngle());
    }

    @Override
    public boolean isFinished() {
        return Math.abs(limelight.getHorizontalAngle()) < 0.5;
    }

    @Override
    public void end(boolean interrupted) {
        turret.set(0);
    }

}

this is where the command is used/binded in the robot controller, (its under the new bindings method)

package frc.robot;

import java.io.IOException;

import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc.robot.commands.climb.FireBrake;
import frc.robot.commands.climb.ManualClimb;
import frc.robot.commands.drive.CenterBall;
import frc.robot.commands.drive.DriveCenterPort;
import frc.robot.commands.drive.FollowTrajectory;
import frc.robot.commands.auto.LTrenchSixBall;
import frc.robot.commands.auto.MTrenchSixBall;
import frc.robot.commands.auto.TrenchEightBall;
import frc.robot.commands.auto.WheelEightBall;
import frc.robot.commands.auto.WheelFiveBall;
import frc.robot.commands.hood.LowerHood;
import frc.robot.commands.hood.RaiseHood;
import frc.robot.commands.hood.ToggleHood;
import frc.robot.commands.intake.LowerIntake;
import frc.robot.commands.intake.RaiseIntake;
import frc.robot.commands.intake.SpinIndexer;
import frc.robot.commands.intake.SpinJustIntake;
import frc.robot.commands.intake.ToggleIntake;
import frc.robot.commands.magazine.AutoMagazine;
import frc.robot.commands.magazine.RunMagazine;
import frc.robot.commands.shooter.AutoFire;
import frc.robot.commands.shooter.AutoFireQuantity;
import frc.robot.commands.shooter.InMotionFire;
import frc.robot.commands.shooter.SpinShooter;
import frc.robot.commands.test.MotorTest;
import frc.robot.commands.turret.AutoAim;
import frc.robot.commands.turret.ManualTurret;
import frc.robot.commands.turret.TurretToAngle;
import frc.robot.commands.turret.ZeroTurret;
import frc.robot.subsystems.Climber;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hood;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.IntakeCamera;
import frc.robot.subsystems.Limelight;
import frc.robot.subsystems.Magazine;
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.Turret;
import frc.robot.utils.ButtonBox;
import frc.robot.utils.CspController;
import frc.robot.utils.CspSequentialCommandGroup;
import frc.robot.utils.KillAll;
import frc.robot.utils.TempManager;
import frc.robot.utils.Waypoints;
import frc.robot.utils.BrownoutProtection;
import frc.robot.utils.WaypointsList;
import frc.robot.utils.CspController.Scaling;

/**
 * Class containing setup for robot.
 */
public class RobotContainer {

    // subsystem initialization
    private final Drivetrain drivetrain = new Drivetrain();
    private final Magazine magazine = new Magazine();
    private final Shooter shooter = new Shooter();
    private final Turret turret = new Turret();
    private final Climber climber = new Climber();
    private final IntakeCamera camera = new IntakeCamera();
    private final Intake intake = new Intake();
    private final Hood hood = new Hood();
    private final Limelight limelight = new Limelight();
    private final TempManager tempManager = new TempManager(climber, drivetrain, intake, magazine, shooter, turret);
    private final BrownoutProtection bop = new BrownoutProtection(drivetrain, intake, magazine, shooter, turret);

    // controller initialization
    private final CspController pilot = new CspController(0);
    private final CspController copilot = new CspController(1);
    //private final ButtonBox buttonBox = new ButtonBox(2);

    // EMERGENCY POWER!!!!!!

    // auto chooser initialization
    private final SendableChooser<CspSequentialCommandGroup> autoChooser = new SendableChooser<CspSequentialCommandGroup>();

    // state variables
    private Pose2d initialPose = new Pose2d();

    public TempManager getTempManager() {
        return tempManager;
    }

    public BrownoutProtection getBrownoutProtection() {
        return bop;
    }

    /**
     * Initializes robot subsystems, controllers, commands, and chooser.
     * 
     * @throws IOException
     */
    public RobotContainer() throws IOException {
        setDefaultCommands();
        configureButtonBindings();
        putChooser();
    }

    /**
     * Resets variables and sensors for each subsystem.
     */
    public void resetSubsystems() {
        drivetrain.reset(initialPose);
        turret.resetEncoders();
    }

    /**
     * Sets the default command for each subsystem, if applicable.
     */
    private void setDefaultCommands() {
        drivetrain.setDefaultCommand(new RunCommand(
                () -> drivetrain.arcade(pilot.getY(Hand.kLeft, Scaling.SQUARED), pilot.getX(Hand.kRight, Scaling.LINEAR), pilot.getBumper(Hand.kLeft)),
                drivetrain));
        turret.setDefaultCommand(new RunCommand(() -> turret.set(0.0), turret));;
    
        //shooter.setDefaultCommand(new SpinShooter(shooter, 2500.0));
    }

    /**
     * Binds commands to buttons on controllers.
     */
    private void configureButtonBindings() {
        
        newBindings();
        //oldBindings();
        
        pilot.getStartButtonObj().whenPressed(new KillAll());


        copilotBindings();

        copilot.getStartButtonObj().whenPressed(new KillAll());

        buttonboxBindings();

        putDashboardButtons();
    }

    private void copilotBindings() {
        copilot.getAButtonObj().whenPressed(new InstantCommand(() -> climber.engagePneuBrake(true), climber));
        copilot.getAButtonObj().whenReleased(new InstantCommand(() -> climber.engagePneuBrake(false), climber));

        // \

        copilot.getXButtonObj().whileHeld(new RunMagazine(magazine, 1.0));
        copilot.getXButtonObj().whenReleased(new RunMagazine(magazine, 0.0));

        copilot.getYButtonObj().whenPressed(new FireBrake(climber));

        copilot.getRtButtonObj().whileActiveContinuous(new AutoMagazine(magazine, intake, true, true));
        copilot.getLtButtonObj().whileActiveContinuous(new AutoMagazine(magazine, intake, false, true));

        copilot.getDpadLeftButtonObj().whileHeld(new ManualTurret(turret, 0.2));
        copilot.getDpadLeftButtonObj().whenReleased(new ManualTurret(turret, 0));
        copilot.getDpadRightButtonObj().whileHeld(new ManualTurret(turret, -0.2));
        copilot.getDpadRightButtonObj().whenReleased(new ManualTurret(turret, 0));

        copilot.getRbButtonObj().whileHeld(new ManualClimb(climber, -0.9));
        copilot.getRbButtonObj().whenReleased(new ManualClimb(climber, 0));
        copilot.getLbButtonObj().whileHeld(new ManualClimb(climber, 0.6));
        copilot.getLbButtonObj().whenReleased(new ManualClimb(climber, 0));

        copilot.getBackButtonObj().whenPressed(new ZeroTurret(turret));
    }

    private void newBindings() {

        pilot.getRtButtonObj().whileActiveContinuous(new AutoMagazine(magazine, intake, true, true)).whenInactive(new AutoMagazine(magazine, intake, true, false));
        pilot.getLtButtonObj().whileActiveContinuous(new AutoMagazine(magazine, intake, false, true)).whenInactive(new AutoMagazine(magazine, intake, false, false));

        pilot.getRbButtonObj().whileHeld(new CenterBall(drivetrain, camera, () -> pilot.getY(Hand.kLeft)));

        pilot.getDpadUpButtonObj().whenPressed(new ManualClimb(climber, -0.9)).whenReleased(new ManualClimb(climber, 0));
        pilot.getDpadDownButtonObj().whileHeld(new ManualClimb(climber, 0.6)).whenReleased(new ManualClimb(climber, 0));

        pilot.getDpadLeftButtonObj().whileHeld(new ManualTurret(turret, 0.2)).whenReleased(new ManualTurret(turret, 0));
        pilot.getDpadRightButtonObj().whileHeld(new ManualTurret(turret, -0.2)).whenReleased(new ManualTurret(turret, 0));

        pilot.getLsButtonObj().whenPressed(new LowerIntake(intake));
        pilot.getRsButtonObj().whenPressed(new RaiseIntake(intake));

        pilot.getYButtonObj().whileHeld(new AutoFire(drivetrain, shooter, magazine, intake, limelight, turret, hood, true));
        pilot.getYButtonObj().whenReleased(new AutoFire(drivetrain, shooter, magazine, intake, limelight, turret, hood, false));

        // pilot.getXButtonObj().whileHeld(new RunMagazine(magazine, -0.9));
        // pilot.getXButtonObj().whenReleased(new RunMagazine(magazine, 0));

        pilot.getXButtonObj().whenPressed(new TurretToAngle(turret, 30));

        pilot.getBButtonObj().whenPressed(new AutoAim(turret, limelight));
   
    }

    private void oldBindings() {
        // pilot.getRbButtonObj().whileHeld(new ManualClimb(climber, -0.9));
        // pilot.getRbButtonObj().whenReleased(new ManualClimb(climber, 0));

        pilot.getLbButtonObj().whileHeld(new RunCommand(() -> intake.spinIntake(1.0), intake));
        pilot.getLbButtonObj().whenReleased(new RunCommand(() -> intake.spinIntake(0.0), intake));

        pilot.getLtButtonObj().whileActiveContinuous(new ToggleHood(hood));

        pilot.getRbButtonObj().whileHeld(new CenterBall(drivetrain, camera, () -> pilot.getY(Hand.kLeft)));
        pilot.getLbButtonObj().whenPressed(new ToggleIntake(intake));

        pilot.getDpadDownButtonObj().whenPressed(new LowerIntake(intake));
        pilot.getDpadUpButtonObj().whenPressed(new RaiseIntake(intake));

        pilot.getYButtonObj().whileHeld(new AutoFire(drivetrain, shooter, magazine, intake, limelight, turret, hood, true));
        pilot.getYButtonObj().whenReleased(new AutoFire(drivetrain, shooter, magazine, intake, limelight, turret, hood, false));

        pilot.getXButtonObj().whileHeld(new RunMagazine(magazine, -0.9));
        pilot.getXButtonObj().whenReleased(new RunMagazine(magazine, 0));

        pilot.getBButtonObj().whileHeld(new AutoMagazine(magazine, intake, true, true));
        pilot.getBButtonObj().whenReleased(new AutoMagazine(magazine, intake, true, false));

        pilot.getAButtonObj().whileHeld(new AutoMagazine(magazine, intake, false, true));
        pilot.getAButtonObj().whenReleased(new AutoMagazine(magazine, intake, false, false));
    }

    private void putDashboardButtons() {
        SmartDashboard.putData("Set Shooter PIDs", new RunCommand(() -> shooter.setPIDs(
            SmartDashboard.getNumber("Set S P", 0.0),
            SmartDashboard.getNumber("Set S I", 0.0),
            SmartDashboard.getNumber("Set S D", 0.0)), shooter));
        SmartDashboard.putData("Set Shooter Velocity", new RunCommand(() -> shooter.setVelocity(SmartDashboard.getNumber("Set S Velocity", 0.0)), shooter));

        SmartDashboard.putData("Set Turret Angle PIDs", new InstantCommand(() -> turret.setPIDs(
            SmartDashboard.getNumber("Set T Angle P", 0.0),
            SmartDashboard.getNumber("Set T Angle I", 0.0),
            SmartDashboard.getNumber("Set T Angle D", 0.0)), turret));
        SmartDashboard.putData("Set Turret Angle", new TurretToAngle(turret, SmartDashboard.getNumber("Set T Angle", 0.0)));

        SmartDashboard.putData("Rezero Turret", new ZeroTurret(turret));

        SmartDashboard.putData("Rezero Odometry", new InstantCommand(() -> drivetrain.reset(initialPose), drivetrain));
    }

    private void buttonboxBindings() {
        // buttonBox.getButton2Obj().whenPressed(new InstantCommand(() -> climber.engagePneuBrake(false), climber));
        // buttonBox.getButton1Obj().whileHeld(new ManualClimb(climber, -0.9))
        //     .whenReleased(new ManualClimb(climber, 0.0));
        // buttonBox.getButton3Obj().whileHeld(new ManualClimb(climber, 0.6))
        //     .whenReleased(new ManualClimb(climber, 0.0));
        // buttonBox.getButton4Obj().whenPressed(new RunMagazine(magazine, 0.625))
        //     .whenReleased(new RunMagazine(magazine, 0.0));
        // buttonBox.getButton5Obj().whenPressed(new SpinShooter(shooter, 3500.0))
        //     .whenReleased(new SpinShooter(shooter, 2000.0));
        // buttonBox.getButton6Obj().whenPressed(new TurretToAngle(turret, 0.0));
        // buttonBox.getButton7Obj().whenPressed(new TurretToAngle(turret, 180.0));
        // buttonBox.getButton8Obj().whileHeld(new RunCommand(() -> {
        //     intake.spin(0.0, 0.0);
        //     magazine.set(0.0);
        //     climber.set(0.0);
        //     turret.set(0.0);
        //     shooter.set(0.0);
        // }, intake, magazine, climber, turret, shooter));
    }

    /**
     * Configures and places auto chooser on dashboard.
     * 
     * @throws IOException
     */
    private void putChooser() throws IOException {
        autoChooser.addOption("Do Nothing", null);
        
        autoChooser.addOption("Left Trench 6-Ball (M)", new MTrenchSixBall(drivetrain, turret, shooter, magazine, intake, limelight));
        autoChooser.addOption("Left Trench 6-Ball (L)", new LTrenchSixBall(drivetrain, shooter, intake, magazine, limelight, turret));
        //autoChooser.addOption("Left Trench 8-Ball", new TrenchEightBall(drivetrain, turret, shooter, magazine, intake, limelight));
        autoChooser.addOption("Wheel 5-Ball", new WheelFiveBall(drivetrain, magazine, intake, shooter, limelight, turret));
        
        SmartDashboard.putData("Auto Chooser", autoChooser);
    }

    /**
     * Returns the currently selected command from the auto chooser and gets its initial pose.
     */
    public Command getAutoCommand() {
        Command autoCommand = autoChooser.getSelected();
        if (autoCommand != null) {
            initialPose = autoChooser.getSelected().getInitialPose();
        } else {
            initialPose = new Pose2d();
        }
        return autoCommand;
    }

    public Command getTestCommand() {
        return new MotorTest(shooter, intake, magazine, turret);
    }
}

[JohnGilb]

I would suggest being more explicit in how you track your state. Below I use an enum, but it could be done in a number of different ways. The basic idea is to just make it a little easier to see what state you are in at any given moment. You can also send that information to the smartdashboard/shuffleboard which will make it easier to see what state the robot is in while watching its live behavior.

I also agree with FRC6302 that your initial logic seems suspect, and the fact it didn’t behave as expected with that fix is also interesting.

public enum TurretTrackingMode {
    Normal,
    ForceToMax,
    ForceToMin
}

   private TurretTrackingMode currentTurretMode = TurretTrackingMode.Normal;

    public void trackTarget(double measure) {           
        switch (currentTurretMode) {
            case Normal:
                // Check to see if we should be doing something else
                if (measure > MAX_ANG) {
                    currentTurretMode = TurretTrackingMode.ForceToMin;
                    break;
                }
                if (measure < MIN_ANG) {
                    currentTurretMode = TurretTrackingMode.ForceToMax;
                    break;
                }
                // If we're not doing anything special, just track normally
                setAngle(getposition() - measure);
                break;
            case ForceToMax:
                // Check to see if we're close enough to max
                if (Math.abs(getposition() - 253) < 3 ) {
                    currentTurretMode = TurretTrackingMode.Normal;
                    break;
                }
                // If we haven't made it, keep aiming towards the max
                setAngle(253);
                break;
            case ForceToMin:
                // Check to see if we're close enough to min
                if (Math.abs(getposition() - -30) < 3 ) {
                    currentTurretMode = TurretTrackingMode.Normal;
                    break;
                }
                // If we haven't made it, keep aiming towards the min
                setAngle(-30);
                break;
            }
        }
    }