Add Automatic Aiming

src/main/java/frc/robot/AutoAim.java

@@ -0,0 +1,151 @@
+package frc.robot;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class AutoAim {
+
+    private double error;
+    private double motorPower;
+    private double angleDegrees;
+    private double gyroDegrees;
+
+    private boolean sweeping;
+
+    public enum AutoAimState {
+        SET_ANGLES,
+        AIM,
+        IDLE;
+    }
+    private AutoAimState state;
+    private Drivetrain drive;
+    private Limelight limelight;
+    private Shooter shooter;
+    private AHRS gyro;
+
+    /**
+     * Constructor
+     * 
+     * @param drive     drive train object.
+     * @param limelight limelight object.
+     * @param shooter   shooter object.
+     * @param gyro      gyro object.
+     */
+    public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro) {
+        state = AutoAimState.IDLE;
+        this.drive = drive;
+        this.limelight = limelight;
+        this.shooter = shooter;
+        this.gyro = gyro;
+    }
+
+    /**
+     * Runs Automatic Aiming state machine.
+     */
+    public void run() {
+        switch (state) {
+
+            case SET_ANGLES:
+                SetAngles();
+                break;
+
+            case AIM:
+                AimX();
+                AimAngle();
+                break;
+
+            case IDLE:
+                break;
+        }
+
+        SmartDashboard.putString("AutoAimState", state.toString());
+        SmartDashboard.putNumber("MotorPower", motorPower);
+        SmartDashboard.putNumber("gyroDegrees", gyroDegrees);
+    }
+
+    /**
+     * Sets degrees to turn the robot to.
+     */
+    private void SetAngles() {
+        if (limelight.isTargetVisible()) {
+            sweeping = false;
+            drive.stopSweep();
+            gyroDegrees = gyro.getYaw() + limelight.getTargetXDegrees();
+            state = AutoAimState.AIM;
+        } else {
+            if (!sweeping) {
+                drive.resetState();
+            }
+            sweeping = true;
+            drive.sweep();
+        }
+    }
+
+    /**
+     * Aims to robot to the X-coordinate of the power port.
+     */
+    private void AimX() {
+        // error = (gyroDegrees - gyro.getYaw()) / 29.8;
+        error = (gyroDegrees - gyro.getYaw());
+        // motorPower = -.5 * error;
+        motorPower = -error * 1;
+        drive.driveLeft(motorPower);
+        drive.driveRight(-motorPower);
+    }
+
+    /**
+     * Aims the shooter angle to the appropriate angle, based on distance, to the
+     * power port.
+     */
+    private void AimAngle() {
+        angleDegrees = 1 * limelight.getTargetVertical(); // TODO: Use a table from testing to create an equation to
+                                                          // plug this into
+        shooter.setAngle(angleDegrees);
+    }
+
+    /**
+     * Gets if done aiming.
+     * 
+     * @return true if robot is done aiming, false if not done aiming.
+     */
+    private boolean getDoneAiming() {
+        if (getWithinTolerance(angleDegrees, shooter.getAngleInDegrees(), 1) && getWithinTolerance(gyroDegrees, gyro.getYaw(), 1)) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    /**
+     * Gets if input is within tolerance of its goal.
+     * 
+     * @param goal      desired value of input.
+     * @param current   current value of input.
+     * @param tolerance tolerance of current input to desired input.
+     * @return true if within tolerance, false if not within tolerance.
+     */
+    private boolean getWithinTolerance(double goal, double current, double tolerance) {
+        return Math.abs(goal - current) < tolerance;
+    }
+
+    /**
+     * Resets the state machine.
+     */
+    public void resetState() {
+        state = AutoAimState.SET_ANGLES;
+    }
+
+    /**
+     * Stops the state machine
+     */
+    public void stopState() {
+        state = AutoAimState.IDLE;
+    }
+
+    /**
+     * Gets the aiming state.
+     */
+    public AutoAimState getState() {
+        return state;
+    }
+}

src/main/java/frc/robot/Drivetrain.java

@@ -1,5 +1,7 @@
 package frc.robot;
 
+import com.kauailabs.navx.frc.AHRS;
+
 public class Drivetrain {
 
     /**
@@ -8,23 +10,37 @@ public class Drivetrain {
      */
     private static final double DEADBAND_LIMIT = 0.02;
 
+    private enum SweepState {
+        SET_ANGLE, SWEEP_LEFT, SWEEP_RIGHT, IDLE;
+    }
+
+    private SweepState sweepState;
+
     private DriveModule left;
     private DriveModule right;
     private PowercellDetection detector;
+    private Limelight limelight;
+    private AHRS gyro;
+
+    private double gyroAngle;
 
     /**
      * Constructor
      *
      * @param left     The left drive module
      * @param right    The right drive module
      * @param detector The powercell detection object.
+     * @param gyro     The gyro object.
      */
-    Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector) {
+    Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector, AHRS gyro) {
         this.left = left;
         this.right = right;
         right.setInverted(true);
 
         this.detector = detector;
+        this.gyro = gyro;
+
+        sweepState = SweepState.IDLE;
     }
 
     /**
@@ -90,6 +106,76 @@ public double deadband(double in) {
         return Math.abs(in) > DEADBAND_LIMIT ? in : 0.0;
     }
 
+    /**
+     * Runs the sweeping procedure.
+     */
+    public void sweep() {
+        switch (sweepState) {
+
+            case SET_ANGLE:
+                sweepAngle();
+                break;
+
+            case SWEEP_LEFT:
+                sweepLeft();
+                break;
+
+            case SWEEP_RIGHT:
+                sweepRight();
+                break;
+
+            case IDLE:
+                break;
+        }
+    }
+
+    /**
+     * Sets home gyro angle.
+     */
+    private void sweepAngle() {
+        gyroAngle = gyro.getAngle();
+        sweepState = SweepState.SWEEP_LEFT;
+    }
+
+    /**
+     * Sweeps to the left 80 degrees.
+     */
+    private void sweepLeft() {
+        if (gyro.getAngle() < gyroAngle + 80) {
+            driveLeft(-0.25);
+            driveRight(0.25);
+        } else {
+            sweepState = SweepState.SWEEP_RIGHT;
+        }
+    }
+
+    /**
+     * Sweeps to the right 80 degrees.
+     * 
+     */
+    private void sweepRight() {
+        if (gyro.getAngle() > gyroAngle - 80) {
+            driveLeft(0.25);
+            driveRight(-0.25);
+        } else {
+            sweepState = SweepState.SWEEP_LEFT;
+        }
+    }
+
+    /**
+     * Resets to the state to setting the angle.
+     */
+    public void resetState() {
+        sweepState = SweepState.SET_ANGLE;
+    }
+
+    /**
+     * Sets the state to idle.
+     */
+    public void stopSweep() {
+        sweepState = SweepState.IDLE;
+    }
+
     /**
      * Moves the robot to intercept powercells
      *

src/main/java/frc/robot/Limelight.java

@@ -7,6 +7,10 @@
 
 public class Limelight {
 
+    // TODO: Get proper dimension varibles.
+    private final double IMG_WIDTH = 320;
+    private final double CAM_FOV = 59.6;
+
     private double tv;
     private double tx;
     private double ty;
@@ -54,6 +58,8 @@ public double getTargetX() {
         return this.tx;
     }
 
+
+
     /**
      * @return current target y-coordinate.
      */
@@ -75,6 +81,13 @@ public double getTargetVertical() {
         return this.tvert;
     }
 
+    /**
+     * @return current target x-coordinate in degrees.
+     */
+    public double getTargetXDegrees() {
+        return this.tx*(CAM_FOV/IMG_WIDTH);
+    }
+
     public void setLightEnabled(boolean enabled) {
         limelightTable.getEntry("ledMode").forceSetNumber(enabled ? 3: 1);
     }

src/main/java/frc/robot/Manipulation.java

@@ -59,7 +59,7 @@ public void setIntakeSpin(boolean spin) {
      * @param extend true if it should extend, false if not.
      */
     public void setIntakeExtend(boolean extend) {
-        // intakePneumatics.set(extend ? Value.kForward : Value.kReverse);
+        intakePneumatics.set(extend ? Value.kForward : Value.kReverse);
     }
 
     /**