From 7d7b2cfcbea336d93c72b785a73cf2be82e97084 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 25 Jan 2020 14:00:42 -0500
Subject: [PATCH 01/28] Add aiming to x coordinate

---
 src/main/java/frc/robot/AutoAim.java | 49 ++++++++++++++++++++++++++++
 1 file changed, 49 insertions(+)
 create mode 100644 src/main/java/frc/robot/AutoAim.java

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
new file mode 100644
index 0000000..440cf76
--- /dev/null
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -0,0 +1,49 @@
+package frc.robot;
+
+
+public class AutoAim {
+
+    private final double P;
+    
+    private double error;
+    private double motorPower;
+
+    public enum AutoAimState {
+        AIM_X,
+        AIM_ANGLE,
+        IDLE;
+    }
+
+    private AutoAimState state;
+    private Drivetrain drive;
+    private Limelight limelight;
+
+    public AutoAim(Drivetrain drive, Limelight limelight) {
+        state = AutoAimState.AIM_X;
+        this.drive = drive;
+        this.limelight = limelight;
+    }
+
+    public void run() {
+        switch(state) {
+
+            case AIM_X:
+                aimX(limelight.getTargetX());
+                break;
+
+            case AIM_ANGLE:
+                //aimAngle(limelight.getTargetVertical());
+                break;
+
+            case IDLE:
+                break;
+        }
+    }
+
+    private void aimX(double x) {
+        error = x * P;
+        motorPower = .5 * error;
+        drive.driveLeft(-motorPower);
+        drive.driveRight(motorPower);
+    }
+}
\ No newline at end of file

From 0df298ff19c41e3811ce5598131719bdd8ff9121 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 25 Jan 2020 14:05:57 -0500
Subject: [PATCH 02/28] Remove P variable

---
 src/main/java/frc/robot/AutoAim.java | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 440cf76..d546390 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -3,8 +3,6 @@
 
 public class AutoAim {
 
-    private final double P;
-    
     private double error;
     private double motorPower;
 
@@ -41,7 +39,7 @@ public void run() {
     }
 
     private void aimX(double x) {
-        error = x * P;
+        error = x * 1;
         motorPower = .5 * error;
         drive.driveLeft(-motorPower);
         drive.driveRight(motorPower);

From 2a9dc1fd8045e93a7c3ec7f05a0d032b40c5e7be Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 25 Jan 2020 17:43:17 -0500
Subject: [PATCH 03/28] Add angle aiming

---
 src/main/java/frc/robot/AutoAim.java | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index d546390..1d9e1fd 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -5,6 +5,7 @@ public class AutoAim {
 
     private double error;
     private double motorPower;
+    private double degrees;
 
     public enum AutoAimState {
         AIM_X,
@@ -15,6 +16,7 @@ public enum AutoAimState {
     private AutoAimState state;
     private Drivetrain drive;
     private Limelight limelight;
+    private Shooter shooter;
 
     public AutoAim(Drivetrain drive, Limelight limelight) {
         state = AutoAimState.AIM_X;
@@ -27,10 +29,14 @@ public void run() {
 
             case AIM_X:
                 aimX(limelight.getTargetX());
+                if(Math.abs(limelight.getTargetX()) < 1)
+                    state = AutoAimState.AIM_ANGLE;
                 break;
 
             case AIM_ANGLE:
-                //aimAngle(limelight.getTargetVertical());
+                aimAngle(limelight.getTargetVertical());
+                if (Math.abs(degrees * DEGREES_PER_REVOLUTIONS * REVOLUTIONS_PER_ENCODER - shooter.getAngleDegrees) < 1)
+                    state = AutoAimState.IDLE;
                 break;
 
             case IDLE:
@@ -44,4 +50,9 @@ private void aimX(double x) {
         drive.driveLeft(-motorPower);
         drive.driveRight(motorPower);
     }
+
+    private void aimAngle(double h) {
+        degrees = 0; //TODO: Use a table from testing to create an equation to plug this into
+        shooter.setAngle(degrees);
+    }
 }
\ No newline at end of file

From 24b30ea162cc21022067d7a69cfb37fda1740f19 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Mon, 27 Jan 2020 18:56:48 -0500
Subject: [PATCH 04/28] Add documentation

---
 src/main/java/frc/robot/AutoAim.java | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 1d9e1fd..5cdacb2 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -18,12 +18,21 @@ public enum AutoAimState {
     private Limelight limelight;
     private Shooter shooter;
 
+    /**
+     * Constructor
+     * 
+     * @param drive drive train object.
+     * @param limelight limelight object.
+     */
     public AutoAim(Drivetrain drive, Limelight limelight) {
         state = AutoAimState.AIM_X;
         this.drive = drive;
         this.limelight = limelight;
     }
 
+    /**
+     * Runs Automatic Aiming state machine.
+     */
     public void run() {
         switch(state) {
 
@@ -44,6 +53,11 @@ public void run() {
         }
     }
 
+    /**
+     * Aims to robot to the X-coordinate of the power port.
+     * 
+     * @param x current X-coordinate of the power port.
+     */
     private void aimX(double x) {
         error = x * 1;
         motorPower = .5 * error;
@@ -51,6 +65,11 @@ private void aimX(double x) {
         drive.driveRight(motorPower);
     }
 
+    /**
+     * Aims the shooter angle to the appropriate angle, based on distance, to the power port.
+     * 
+     * @param h current height of the power port.
+     */
     private void aimAngle(double h) {
         degrees = 0; //TODO: Use a table from testing to create an equation to plug this into
         shooter.setAngle(degrees);

From f38c0c08783a41d255923028657d3b71c603187f Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Mon, 27 Jan 2020 19:03:56 -0500
Subject: [PATCH 05/28] Fix dimensional analysis

---
 src/main/java/frc/robot/AutoAim.java | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 5cdacb2..4456cc5 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -3,6 +3,9 @@
 
 public class AutoAim {
 
+    private double REVOLUTIONS_PER_DEGREE = 0; //TODO: Determine revolutions of motor required to turn angle motor a degree
+    private double ENCODERS_PER_REVOLUTION = 42;
+
     private double error;
     private double motorPower;
     private double degrees;
@@ -44,7 +47,7 @@ public void run() {
 
             case AIM_ANGLE:
                 aimAngle(limelight.getTargetVertical());
-                if (Math.abs(degrees * DEGREES_PER_REVOLUTIONS * REVOLUTIONS_PER_ENCODER - shooter.getAngleDegrees) < 1)
+                if (Math.abs(degrees * REVOLUTIONS_PER_DEGREE * ENCODERS_PER_REVOLUTION - shooter.getAngleDegree) < 1)
                     state = AutoAimState.IDLE;
                 break;
 

From c2112ed652c7a6b7cbc57d03d1658d6caaa1c777 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 1 Feb 2020 10:13:57 -0500
Subject: [PATCH 06/28] Fix aimX negative error

---
 src/main/java/frc/robot/AutoAim.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 4456cc5..94c455d 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -64,8 +64,8 @@ public void run() {
     private void aimX(double x) {
         error = x * 1;
         motorPower = .5 * error;
-        drive.driveLeft(-motorPower);
-        drive.driveRight(motorPower);
+        drive.driveLeft(motorPower);
+        drive.driveRight(-motorPower);
     }
 
     /**

From d2f018b5ac4608d1bbf17cb2d13c9e9ae5e6628a Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Tue, 18 Feb 2020 19:49:41 -0500
Subject: [PATCH 07/28] Organize states into methods

---
 src/main/java/frc/robot/AutoAim.java | 35 +++++++++++++++++-----------
 1 file changed, 21 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 43aed8d..d2713ef 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -3,9 +3,6 @@
 
 public class AutoAim {
 
-    private double REVOLUTIONS_PER_DEGREE = 0; //TODO: Determine revolutions of motor required to turn angle motor a degree
-    private double ENCODERS_PER_REVOLUTION = 42;
-
     private double error;
     private double motorPower;
     private double degrees;
@@ -40,15 +37,17 @@ public void run() {
         switch(state) {
 
             case AIM_X:
-                aimX(limelight.getTargetX());
-                if(Math.abs(limelight.getTargetX()) < 1)
+                AimX();
+                if (getWithinTolerance(0, limelight.getTargetX(), 1)) {
                     state = AutoAimState.AIM_ANGLE;
+                }
                 break;
 
             case AIM_ANGLE:
-                aimAngle(limelight.getTargetVertical());
-                if (Math.abs(degrees - shooter.getAngleInDegrees()) < 1)
+                AimAngle();
+                if (getWithinTolerance(degrees, shooter.getAngleInDegrees(), 1)) {
                     state = AutoAimState.IDLE;
+                }
                 break;
 
             case IDLE:
@@ -58,11 +57,9 @@ public void run() {
 
     /**
      * Aims to robot to the X-coordinate of the power port.
-     * 
-     * @param x current X-coordinate of the power port.
      */
-    private void aimX(double x) {
-        error = x * 1;
+    private void AimX() {
+        error = limelight.getTargetX() * 1;
         motorPower = .5 * error;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);
@@ -70,11 +67,21 @@ private void aimX(double x) {
 
     /**
      * Aims the shooter angle to the appropriate angle, based on distance, to the power port.
-     * 
-     * @param h current height of the power port.
      */
-    private void aimAngle(double h) {
+    private void AimAngle() {
         degrees = 0; //TODO: Use a table from testing to create an equation to plug this into
         shooter.setAngle(degrees);
     }
+
+    /**
+     * 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;
+    }
 }
\ No newline at end of file

From 2bdd3522f6a1f7a3cb3c533826bd1511375fad48 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Tue, 18 Feb 2020 19:51:28 -0500
Subject: [PATCH 08/28] Move state transitions into methods

---
 src/main/java/frc/robot/AutoAim.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index d2713ef..dabcaf2 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -38,16 +38,10 @@ public void run() {
 
             case AIM_X:
                 AimX();
-                if (getWithinTolerance(0, limelight.getTargetX(), 1)) {
-                    state = AutoAimState.AIM_ANGLE;
-                }
                 break;
 
             case AIM_ANGLE:
                 AimAngle();
-                if (getWithinTolerance(degrees, shooter.getAngleInDegrees(), 1)) {
-                    state = AutoAimState.IDLE;
-                }
                 break;
 
             case IDLE:
@@ -63,6 +57,9 @@ private void AimX() {
         motorPower = .5 * error;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);
+        if (getWithinTolerance(0, limelight.getTargetX(), 1)) {
+            state = AutoAimState.AIM_ANGLE;
+        }
     }
 
     /**
@@ -71,6 +68,9 @@ private void AimX() {
     private void AimAngle() {
         degrees = 0; //TODO: Use a table from testing to create an equation to plug this into
         shooter.setAngle(degrees);
+        if (getWithinTolerance(degrees, shooter.getAngleInDegrees(), 1)) {
+            state = AutoAimState.IDLE;
+        }
     }
 
     /**

From c575583e57144010cc28aafe2e91f491430cde95 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Tue, 18 Feb 2020 19:54:58 -0500
Subject: [PATCH 09/28] Add vertical distance of limelight target to degree
 calculations

---
 src/main/java/frc/robot/AutoAim.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index dabcaf2..56909cb 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -66,7 +66,7 @@ private void AimX() {
      * Aims the shooter angle to the appropriate angle, based on distance, to the power port.
      */
     private void AimAngle() {
-        degrees = 0; //TODO: Use a table from testing to create an equation to plug this into
+        degrees = 1 *limelight.getTargetVertical(); //TODO: Use a table from testing to create an equation to plug this into
         shooter.setAngle(degrees);
         if (getWithinTolerance(degrees, shooter.getAngleInDegrees(), 1)) {
             state = AutoAimState.IDLE;

From 8eba47f1c14a608e4cc6dffea87191f07fca4ff7 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Tue, 18 Feb 2020 20:52:11 -0500
Subject: [PATCH 10/28] Use gyro angle to aim robot x-coordinate

---
 src/main/java/frc/robot/AutoAim.java   | 36 +++++++++++++++++++++-----
 src/main/java/frc/robot/Limelight.java |  7 +++++
 2 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 56909cb..7a924c4 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -1,13 +1,16 @@
 package frc.robot;
 
+import com.kauailabs.navx.frc.AHRS;
 
 public class AutoAim {
 
     private double error;
     private double motorPower;
-    private double degrees;
+    private double angleDegrees;
+    private double gyroDegrees;
 
     public enum AutoAimState {
+        SET_ANGLES,
         AIM_X,
         AIM_ANGLE,
         IDLE;
@@ -17,17 +20,23 @@ public enum AutoAimState {
     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) {
-        state = AutoAimState.AIM_X;
+    public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro) {
+        state = AutoAimState.SET_ANGLES;
         this.drive = drive;
         this.limelight = limelight;
+        this.shooter = shooter;
+        this.gyro = gyro;
     }
 
     /**
@@ -36,6 +45,10 @@ public AutoAim(Drivetrain drive, Limelight limelight) {
     public void run() {
         switch(state) {
 
+            case SET_ANGLES:
+                SetAngles();
+                break;
+
             case AIM_X:
                 AimX();
                 break;
@@ -49,6 +62,14 @@ public void run() {
         }
     }
 
+    /**
+     * Sets degrees to turn the robot to.
+     */
+    private void SetAngles() {
+        gyroDegrees = gyro.getYaw() - limelight.getTargetXDegrees();
+        state = AutoAimState.AIM_X;
+    }
+
     /**
      * Aims to robot to the X-coordinate of the power port.
      */
@@ -57,7 +78,7 @@ private void AimX() {
         motorPower = .5 * error;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);
-        if (getWithinTolerance(0, limelight.getTargetX(), 1)) {
+        if (getWithinTolerance(gyroDegrees, gyro.getYaw(), 1)) {
             state = AutoAimState.AIM_ANGLE;
         }
     }
@@ -66,9 +87,9 @@ private void AimX() {
      * Aims the shooter angle to the appropriate angle, based on distance, to the power port.
      */
     private void AimAngle() {
-        degrees = 1 *limelight.getTargetVertical(); //TODO: Use a table from testing to create an equation to plug this into
-        shooter.setAngle(degrees);
-        if (getWithinTolerance(degrees, shooter.getAngleInDegrees(), 1)) {
+        angleDegrees = 1 *limelight.getTargetVertical(); //TODO: Use a table from testing to create an equation to plug this into
+        shooter.setAngle(angleDegrees);
+        if (getWithinTolerance(angleDegrees, shooter.getAngleInDegrees(), 1)) {
             state = AutoAimState.IDLE;
         }
     }
@@ -84,4 +105,5 @@ private void AimAngle() {
     private boolean getWithinTolerance(double goal, double current, double tolerance) {
         return Math.abs(goal - current) < tolerance;
     }
+    
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Limelight.java b/src/main/java/frc/robot/Limelight.java
index 0b9bc5b..f5ed6c1 100644
--- a/src/main/java/frc/robot/Limelight.java
+++ b/src/main/java/frc/robot/Limelight.java
@@ -75,6 +75,13 @@ public double getTargetVertical() {
         return this.tvert;
     }
 
+    /**
+     * @return current target x-coordinate in degrees.
+     */
+    public double getTargetXDegrees() {
+        return this.tx * 1; // TODO: Determine modifier.
+    }
+
     public void setLightEnabled(boolean enabled) {
         limelightTable.getEntry("ledMode").forceSetNumber(enabled ? 3: 1);
     }

From ba08811a9c919bf021d17521744db194bb72e2ff Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Thu, 20 Feb 2020 20:26:08 -0500
Subject: [PATCH 11/28] Add auto aiming to robot

---
 src/main/java/frc/robot/Robot.java | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index dd82e45..f75a28b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -43,6 +43,7 @@ public class Robot extends TimedRobot {
   DigitalInput lightInput;
   Shooter shooter = null;
   Drivetrain drive = null;
+  AutoAim aim;
   XboxController driver = null;
   XboxController operator = null;
   DriveModule module = null;
@@ -54,6 +55,7 @@ public class Robot extends TimedRobot {
   boolean shooterToggle = true;
   boolean drivetrainToggle = false;
   boolean navXToggle = false;
+  boolean autoAimToggle = false;
 
   double targetAngle = 0;
 
@@ -113,6 +115,14 @@ public void robotInit() {
       System.out.println("Gyro system (NavX) disabled. Skipping initialization...");
     }
 
+    if (this.autoAimToggle) {
+      System.out.print("Initializing automatic aiming system...");
+      aim = new AutoAim(drive, limelight, shooter, gyro);
+      System.out.println("done");
+    } else {
+      System.out.println("Automatic aiming disabled. Skipping initialization...");
+    }
+
     System.out.print("Initializing driver interface...");
     driver = new XboxController(0);
     operator = new XboxController(1);

From 0235f8ff0b91518746fea81ea5a4158d7212c5ab Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Thu, 20 Feb 2020 20:42:03 -0500
Subject: [PATCH 12/28] Add driver controls

---
 src/main/java/frc/robot/AutoAim.java |  7 ++++++-
 src/main/java/frc/robot/Robot.java   | 14 ++++++++++++++
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 7a924c4..011d995 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -32,7 +32,7 @@ public enum AutoAimState {
      * @param gyro gyro object.
      */
     public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro) {
-        state = AutoAimState.SET_ANGLES;
+        state = AutoAimState.IDLE;
         this.drive = drive;
         this.limelight = limelight;
         this.shooter = shooter;
@@ -43,6 +43,7 @@ public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro
      * Runs Automatic Aiming state machine.
      */
     public void run() {
+        state = AutoAimState.SET_ANGLES;
         switch(state) {
 
             case SET_ANGLES:
@@ -105,5 +106,9 @@ private void AimAngle() {
     private boolean getWithinTolerance(double goal, double current, double tolerance) {
         return Math.abs(goal - current) < tolerance;
     }
+
+    public void stopAiming() {
+        state = AutoAimState.IDLE;
+    }
     
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f75a28b..f83c757 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -57,6 +57,8 @@ public class Robot extends TimedRobot {
   boolean navXToggle = false;
   boolean autoAimToggle = false;
 
+  boolean aiming = false;
+
   double targetAngle = 0;
 
   double speed = 0;
@@ -197,6 +199,18 @@ public void teleopPeriodic() {
 
     if (this.photoswitchSensorToggle)
       SmartDashboard.putBoolean("LightClear", light.getClear());
+
+    if (this.autoAimToggle) {
+      if (operator.getYButtonPressed() && !aiming && limelight.isTargetVisible()) {
+        aim.run();
+        aiming = true;
+      } else if (operator.getYButtonPressed() && aiming) {
+        aim.stopAiming();
+        aiming = false;
+      }
+
+
+    }
   }
 
   @Override

From ddc1dfbc363021324baeb48bb87ff822161100a6 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Thu, 20 Feb 2020 20:54:54 -0500
Subject: [PATCH 13/28] Add additional controls

---
 src/main/java/frc/robot/AutoAim.java | 10 ++++++++++
 src/main/java/frc/robot/Robot.java   | 24 ++++++++++++++----------
 2 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 011d995..c39aaa4 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -107,8 +107,18 @@ private boolean getWithinTolerance(double goal, double current, double tolerance
         return Math.abs(goal - current) < tolerance;
     }
 
+    /**
+     * Stops the aiming process.
+     */
     public void stopAiming() {
         state = AutoAimState.IDLE;
     }
+
+    /**
+     * Gets the aiming state.
+     */
+    public AutoAimState getState() {
+        return state;
+    }
     
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f83c757..80b803a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -150,8 +150,14 @@ public void teleopInit() {
 
   @Override
   public void teleopPeriodic() {
-    if (this.limelightToggle)
+    if (this.limelightToggle) {
       limelight.update();
+      if (driver.getXButtonPressed()) {
+        limelight.setLightEnabled(true);
+      } else if (driver.getYButtonPressed()) {
+        limelight.setLightEnabled(false);
+      }
+    }
 
     if (this.shooterToggle) {
       double speed = 0;
@@ -170,7 +176,7 @@ public void teleopPeriodic() {
         shooter.reHome();
       }
 
-      if (shooter.getState() == Shooter.State.Idle || shooter.getState() == Shooter.State.ManualControl) {
+      if ((shooter.getState() == Shooter.State.Idle || shooter.getState() == Shooter.State.ManualControl) && !aiming) {
         shooter.manualControl(speed, shooterAngleSpeed);
       }
       shooter.update();
@@ -188,19 +194,19 @@ public void teleopPeriodic() {
       double turnInput = driver.getX(Hand.kRight);
       double speedInput = driver.getY(Hand.kLeft);
 
-      if (driver.getXButtonPressed()) {
-        limelight.setLightEnabled(true);
-      } else if (driver.getYButtonPressed()) {
-        limelight.setLightEnabled(false);
+      if (!aiming) {
+        drive.arcadeDrive(turnInput, speedInput);
       }
-
-      drive.arcadeDrive(turnInput, speedInput);
     }
 
     if (this.photoswitchSensorToggle)
       SmartDashboard.putBoolean("LightClear", light.getClear());
 
     if (this.autoAimToggle) {
+      if(aim.getState() == AutoAim.AutoAimState.IDLE){
+        aiming = false;
+      }
+
       if (operator.getYButtonPressed() && !aiming && limelight.isTargetVisible()) {
         aim.run();
         aiming = true;
@@ -208,8 +214,6 @@ public void teleopPeriodic() {
         aim.stopAiming();
         aiming = false;
       }
-
-
     }
   }
 

From 91c6b799ac47db4160377aeb9d7eaf5b5fdc8238 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Fri, 21 Feb 2020 19:07:20 -0500
Subject: [PATCH 14/28] Add angle calculations

---
 src/main/java/frc/robot/AutoAim.java   | 3 +--
 src/main/java/frc/robot/Limelight.java | 8 +++++++-
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index c39aaa4..2e43a9c 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -67,7 +67,7 @@ public void run() {
      * Sets degrees to turn the robot to.
      */
     private void SetAngles() {
-        gyroDegrees = gyro.getYaw() - limelight.getTargetXDegrees();
+        gyroDegrees = gyro.getYaw() + limelight.getTargetXDegrees();
         state = AutoAimState.AIM_X;
     }
 
@@ -120,5 +120,4 @@ public void stopAiming() {
     public AutoAimState getState() {
         return state;
     }
-    
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Limelight.java b/src/main/java/frc/robot/Limelight.java
index f5ed6c1..b63ed0f 100644
--- a/src/main/java/frc/robot/Limelight.java
+++ b/src/main/java/frc/robot/Limelight.java
@@ -7,6 +7,9 @@
 
 public class Limelight {
 
+    private final int IMG_WIDTH = 640;
+    private final int CAM_FOV = 60;
+
     private double tv;
     private double tx;
     private double ty;
@@ -54,6 +57,8 @@ public double getTargetX() {
         return this.tx;
     }
 
+
+
     /**
      * @return current target y-coordinate.
      */
@@ -79,7 +84,8 @@ public double getTargetVertical() {
      * @return current target x-coordinate in degrees.
      */
     public double getTargetXDegrees() {
-        return this.tx * 1; // TODO: Determine modifier.
+        double turn = this.tx - (IMG_WIDTH/2);
+        return turn/(IMG_WIDTH/CAM_FOV);
     }
 
     public void setLightEnabled(boolean enabled) {

From fbd04f8ed00d433b711dc5b404b83468ae1dea59 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Fri, 21 Feb 2020 20:11:00 -0500
Subject: [PATCH 15/28] Fix camera variables

---
 src/main/java/frc/robot/Limelight.java | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Limelight.java b/src/main/java/frc/robot/Limelight.java
index b63ed0f..952139c 100644
--- a/src/main/java/frc/robot/Limelight.java
+++ b/src/main/java/frc/robot/Limelight.java
@@ -7,8 +7,9 @@
 
 public class Limelight {
 
-    private final int IMG_WIDTH = 640;
-    private final int CAM_FOV = 60;
+    // TODO: Get proper dimension varibles.
+    private final int IMG_WIDTH = 1;
+    private final int CAM_FOV = 1;
 
     private double tv;
     private double tx;

From a4b7a9db1e9309ae38f158700ba907792f564970 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 22 Feb 2020 13:22:19 -0500
Subject: [PATCH 16/28] Add correct camera variables

---
 src/main/java/frc/robot/Limelight.java | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Limelight.java b/src/main/java/frc/robot/Limelight.java
index 952139c..5ae5336 100644
--- a/src/main/java/frc/robot/Limelight.java
+++ b/src/main/java/frc/robot/Limelight.java
@@ -8,8 +8,8 @@
 public class Limelight {
 
     // TODO: Get proper dimension varibles.
-    private final int IMG_WIDTH = 1;
-    private final int CAM_FOV = 1;
+    private final int IMG_WIDTH = 320;
+    private final int CAM_FOV = 59.6;
 
     private double tv;
     private double tx;
@@ -85,8 +85,7 @@ public double getTargetVertical() {
      * @return current target x-coordinate in degrees.
      */
     public double getTargetXDegrees() {
-        double turn = this.tx - (IMG_WIDTH/2);
-        return turn/(IMG_WIDTH/CAM_FOV);
+        return this.tx*(CAM_FOV/IMG_WIDTH);
     }
 
     public void setLightEnabled(boolean enabled) {

From 36e9d5945065aefcde1aa5dd3195ef67793d1edc Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 22 Feb 2020 13:36:37 -0500
Subject: [PATCH 17/28] Convert camera variables to doubles

---
 src/main/java/frc/robot/Limelight.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Limelight.java b/src/main/java/frc/robot/Limelight.java
index 5ae5336..d3dee06 100644
--- a/src/main/java/frc/robot/Limelight.java
+++ b/src/main/java/frc/robot/Limelight.java
@@ -8,8 +8,8 @@
 public class Limelight {
 
     // TODO: Get proper dimension varibles.
-    private final int IMG_WIDTH = 320;
-    private final int CAM_FOV = 59.6;
+    private final double IMG_WIDTH = 320;
+    private final double CAM_FOV = 59.6;
 
     private double tv;
     private double tx;

From 64f2b8e83b63244b93de7b8e9c7c0908b65b3e87 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 22 Feb 2020 16:21:08 -0500
Subject: [PATCH 18/28] Fix AimX error variable

---
 src/main/java/frc/robot/AutoAim.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 2e43a9c..0c789d5 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -75,7 +75,7 @@ private void SetAngles() {
      * Aims to robot to the X-coordinate of the power port.
      */
     private void AimX() {
-        error = limelight.getTargetX() * 1;
+        error = (gyroDegrees - gyro.getYaw())/29.8;
         motorPower = .5 * error;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);

From b8a169db869b04078fc83a58883ed84f6b3ed652 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sat, 22 Feb 2020 16:26:59 -0500
Subject: [PATCH 19/28] Fix toggles

---
 src/main/java/frc/robot/Robot.java | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 840ddee..ef7c3a1 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -51,12 +51,13 @@ public class Robot extends TimedRobot {
   Compressor compressor = null;
 
   // Booleans for toggling different things...
-  boolean limelightToggle = false;
+  boolean limelightToggle = true;
   boolean photoswitchSensorToggle = false;
+  boolean powercellDetectorToggle = false;
   boolean shooterToggle = true;
-  boolean drivetrainToggle = false;
-  boolean navXToggle = false;
-  boolean autoAimToggle = false;
+  boolean drivetrainToggle = true;
+  boolean navXToggle = true;
+  boolean autoAimToggle = true;
 
   boolean aiming = false;
 

From 5f1a646e44e6a3fc1e15db07791c37fa67e1f0d7 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Mon, 24 Feb 2020 20:42:49 -0500
Subject: [PATCH 20/28] Add state machine resets

---
 src/main/java/frc/robot/AutoAim.java | 12 +++++++++---
 src/main/java/frc/robot/Robot.java   |  3 ++-
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 0c789d5..01b877b 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -43,7 +43,6 @@ public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro
      * Runs Automatic Aiming state machine.
      */
     public void run() {
-        state = AutoAimState.SET_ANGLES;
         switch(state) {
 
             case SET_ANGLES:
@@ -108,9 +107,16 @@ private boolean getWithinTolerance(double goal, double current, double tolerance
     }
 
     /**
-     * Stops the aiming process.
+     * Resets the state machine.
+     */
+    public void resetState() {
+        state = AutoAimState.SET_ANGLES;
+    }
+
+    /**
+     * Stops the state machine
      */
-    public void stopAiming() {
+    public void stopState() {
         state = AutoAimState.IDLE;
     }
 
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index ef7c3a1..18b97da 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -221,10 +221,11 @@ public void teleopPeriodic() {
       }
 
       if (operator.getYButtonPressed() && !aiming && limelight.isTargetVisible()) {
+        aim.resetState();
         aim.run();
         aiming = true;
       } else if (operator.getYButtonPressed() && aiming) {
-        aim.stopAiming();
+        aim.stopState();
         aiming = false;
       }
     }

From 939ea484e7304f6479bf2ff8ec0aeb4f69525827 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Fri, 28 Feb 2020 18:52:43 -0500
Subject: [PATCH 21/28] Add drivetrain sweeping

---
 src/main/java/frc/robot/Drivetrain.java | 78 ++++++++++++++++++++++++-
 src/main/java/frc/robot/Robot.java      |  2 +-
 2 files changed, 78 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java
index d1e5983..6f91a27 100644
--- a/src/main/java/frc/robot/Drivetrain.java
+++ b/src/main/java/frc/robot/Drivetrain.java
@@ -1,5 +1,7 @@
 package frc.robot;
 
+import com.kauailabs.navx.frc.AHRS;
+
 public class Drivetrain {
     
     /**
@@ -8,9 +10,22 @@ 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
@@ -19,12 +34,16 @@ public class Drivetrain {
      * @param right The right drive module
      * @param detector The powercell detection object.
      */
-    Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector){
+    Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector, Limelight limelight, AHRS gyro){
         this.left = left;
         this.right = right;
         left.setInverted(true);
 
         this.detector = detector;
+        this.limelight = limelight;
+        this.gyro = gyro;
+
+        sweepState = SweepState.IDLE;
     }
 
     /**
@@ -85,6 +104,63 @@ 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.50);
+            driveRight(0.50);
+        } else {
+            sweepState = SweepState.SWEEP_RIGHT;
+        }
+    }
+
+    /**
+     * Sweeps to the right 80 degrees.
+
+     */
+    private void sweepRight() {
+        if (gyro.getAngle() > gyroAngle - 80) {
+            driveLeft(0.50);
+            driveRight(-0.50);
+        } else {
+            sweepState = SweepState.SWEEP_LEFT;
+        }
+
+    }
+
     /**
      * Moves the robot to intercept powercells
      * 
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 01f9d83..5bb840c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -104,7 +104,7 @@ public void robotInit() {
       System.out.print("Initializing drivetrain...");
       DriveModule leftModule = new DriveModule(new TalonFX(5), new TalonFX(6), new TalonFX(7), new Solenoid(2, 0));
       DriveModule rightModule = new DriveModule(new TalonFX(8), new TalonFX(9), new TalonFX(10), new Solenoid(2, 1));
-      drive = new Drivetrain(leftModule, rightModule, detector);
+      drive = new Drivetrain(leftModule, rightModule, detector, limelight, gyro);
       System.out.println("done");
     } else {
       System.out.println("Drivetrain disabled. Skipping initialization...");

From 9ed3479dbb7d4d75435edad9b8457ecfe883e3bd Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Fri, 28 Feb 2020 18:57:16 -0500
Subject: [PATCH 22/28] Add remote state control

---
 src/main/java/frc/robot/Drivetrain.java | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java
index 6f91a27..657a196 100644
--- a/src/main/java/frc/robot/Drivetrain.java
+++ b/src/main/java/frc/robot/Drivetrain.java
@@ -158,7 +158,20 @@ private void sweepRight() {
         } 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;
     }
 
     /**

From ba8570b7972f668cf38b0888217aee07566717b1 Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Fri, 28 Feb 2020 19:03:36 -0500
Subject: [PATCH 23/28] Add sweeping if target is not visible

---
 src/main/java/frc/robot/AutoAim.java | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 01b877b..909a7a6 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -9,6 +9,8 @@ public class AutoAim {
     private double angleDegrees;
     private double gyroDegrees;
 
+    private boolean sweeping;
+
     public enum AutoAimState {
         SET_ANGLES,
         AIM_X,
@@ -66,8 +68,18 @@ public void run() {
      * Sets degrees to turn the robot to.
      */
     private void SetAngles() {
-        gyroDegrees = gyro.getYaw() + limelight.getTargetXDegrees();
-        state = AutoAimState.AIM_X;
+        if (limelight.isTargetVisible()) {
+            sweeping = false;
+            drive.stopSweep();
+            gyroDegrees = gyro.getYaw() + limelight.getTargetXDegrees();
+            state = AutoAimState.AIM_X;
+        } else {
+            if (!sweeping) {
+                drive.resetState();
+            }
+            sweeping = true;
+            drive.sweep();
+        }
     }
 
     /**

From 87327f7ee91dc730fa3a4c8b6f2cf5200234c2fe Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Wed, 4 Mar 2020 18:45:35 -0500
Subject: [PATCH 24/28] Fix driver controlled autoaiming

---
 src/main/java/frc/robot/Robot.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 339b0c6..40d605d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -220,14 +220,14 @@ public void teleopPeriodic() {
         aiming = false;
       }
 
-      if (operator.getYButtonPressed() && !aiming && limelight.isTargetVisible()) {
+      if (operator.getYButtonPressed() && !aiming) {
         aim.resetState();
-        aim.run();
         aiming = true;
       } else if (operator.getYButtonPressed() && aiming) {
         aim.stopState();
         aiming = false;
       }
+      aim.run();
     }
   }
 

From 891bfc200eca898508092c8ab23d3c0002728e2b Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Wed, 4 Mar 2020 19:41:38 -0500
Subject: [PATCH 25/28] Turn off limelight light when not aiming

---
 src/main/java/frc/robot/Robot.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 40d605d..680db35 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -156,6 +156,7 @@ public void autonomousPeriodic() {
   @Override
   public void teleopInit() {
     // shooter.reHome();
+    limelight.setLightEnabled(false);
   }
 
   @Override
@@ -227,6 +228,7 @@ public void teleopPeriodic() {
         aim.stopState();
         aiming = false;
       }
+      limelight.setLightEnabled(aiming);
       aim.run();
     }
   }

From 97c2b62aa490a235ab858935556664b53a6a841d Mon Sep 17 00:00:00 2001
From: Programmer <programmer@redalert1741.org>
Date: Thu, 5 Mar 2020 22:09:53 -0500
Subject: [PATCH 26/28] Start testing

---
 src/main/java/frc/robot/AutoAim.java      | 28 +++++++++-----------
 src/main/java/frc/robot/Drivetrain.java   | 12 ++++-----
 src/main/java/frc/robot/Manipulation.java |  2 +-
 src/main/java/frc/robot/Robot.java        | 32 +++++++++++++----------
 4 files changed, 38 insertions(+), 36 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 909a7a6..6e3eed5 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -12,10 +12,7 @@ public class AutoAim {
     private boolean sweeping;
 
     public enum AutoAimState {
-        SET_ANGLES,
-        AIM_X,
-        AIM_ANGLE,
-        IDLE;
+        SET_ANGLES, AIM_X, AIM_ANGLE, IDLE;
     }
 
     private AutoAimState state;
@@ -24,14 +21,13 @@ public enum AutoAimState {
     private Shooter shooter;
     private AHRS gyro;
 
-
     /**
      * Constructor
      * 
-     * @param drive drive train object.
+     * @param drive     drive train object.
      * @param limelight limelight object.
-     * @param shooter shooter object.
-     * @param gyro gyro object.
+     * @param shooter   shooter object.
+     * @param gyro      gyro object.
      */
     public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro) {
         state = AutoAimState.IDLE;
@@ -45,7 +41,7 @@ public AutoAim(Drivetrain drive, Limelight limelight, Shooter shooter, AHRS gyro
      * Runs Automatic Aiming state machine.
      */
     public void run() {
-        switch(state) {
+        switch (state) {
 
             case SET_ANGLES:
                 SetAngles();
@@ -86,8 +82,8 @@ private void SetAngles() {
      * Aims to robot to the X-coordinate of the power port.
      */
     private void AimX() {
-        error = (gyroDegrees - gyro.getYaw())/29.8;
-        motorPower = .5 * error;
+        error = (gyroDegrees - gyro.getYaw()) / 29.8;
+        motorPower = -.2 * error;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);
         if (getWithinTolerance(gyroDegrees, gyro.getYaw(), 1)) {
@@ -96,10 +92,12 @@ private void AimX() {
     }
 
     /**
-     * Aims the shooter angle to the appropriate angle, based on distance, to the power port.
+     * 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
+        angleDegrees = 1 * limelight.getTargetVertical(); // TODO: Use a table from testing to create an equation to
+                                                          // plug this into
         shooter.setAngle(angleDegrees);
         if (getWithinTolerance(angleDegrees, shooter.getAngleInDegrees(), 1)) {
             state = AutoAimState.IDLE;
@@ -109,8 +107,8 @@ private void AimAngle() {
     /**
      * Gets if input is within tolerance of its goal.
      * 
-     * @param goal desired value of input.
-     * @param current current value of input.
+     * @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.
      */
diff --git a/src/main/java/frc/robot/Drivetrain.java b/src/main/java/frc/robot/Drivetrain.java
index c806f88..06b3c6a 100644
--- a/src/main/java/frc/robot/Drivetrain.java
+++ b/src/main/java/frc/robot/Drivetrain.java
@@ -30,14 +30,14 @@ private enum SweepState {
      * @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, Limelight limelight, AHRS gyro) {
+    Drivetrain(DriveModule left, DriveModule right, PowercellDetection detector, AHRS gyro) {
         this.left = left;
         this.right = right;
         right.setInverted(true);
 
         this.detector = detector;
-        this.limelight = limelight;
         this.gyro = gyro;
 
         sweepState = SweepState.IDLE;
@@ -142,8 +142,8 @@ private void sweepAngle() {
      */
     private void sweepLeft() {
         if (gyro.getAngle() < gyroAngle + 80) {
-            driveLeft(-0.50);
-            driveRight(0.50);
+            driveLeft(-0.25);
+            driveRight(0.25);
         } else {
             sweepState = SweepState.SWEEP_RIGHT;
         }
@@ -155,8 +155,8 @@ private void sweepLeft() {
      */
     private void sweepRight() {
         if (gyro.getAngle() > gyroAngle - 80) {
-            driveLeft(0.50);
-            driveRight(-0.50);
+            driveLeft(0.25);
+            driveRight(-0.25);
         } else {
             sweepState = SweepState.SWEEP_LEFT;
         }
diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java
index eb46498..29f434e 100644
--- a/src/main/java/frc/robot/Manipulation.java
+++ b/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);
     }
 
     /**
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5622f57..e3425fd 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -69,7 +69,7 @@ public class Robot extends TimedRobot {
   boolean autoAimToggle = true;
 
   boolean manipulationToggle = true;
-  boolean navXToggle = false;
+  boolean navXToggle = true;
   boolean powercellDetectorToggle = false;
 
   boolean ptoEngaged = false;
@@ -160,16 +160,6 @@ public void robotInit() {
       System.out.println("Shooter disabled. Skipping initialization...");
     }
 
-    if (this.drivetrainToggle) {
-      System.out.print("Initializing drivetrain...");
-      DriveModule leftModule = new DriveModule(new TalonFX(5), new TalonFX(6), new TalonFX(7), new Solenoid(2, 0));
-      DriveModule rightModule = new DriveModule(new TalonFX(8), new TalonFX(9), new TalonFX(10), new Solenoid(2, 1));
-      drive = new Drivetrain(leftModule, rightModule, detector, limelight, gyro);
-      System.out.println("done");
-    } else {
-      System.out.println("Drivetrain disabled. Skipping initialization...");
-    }
-
     if (this.navXToggle) {
       System.out.print("Initializing gyro system (NavX)...");
       gyro = new AHRS(SPI.Port.kMXP);
@@ -180,6 +170,17 @@ public void robotInit() {
       System.out.println("Gyro system (NavX) disabled. Skipping initialization...");
     }
 
+    if (this.drivetrainToggle) {
+      System.out.print("Initializing drivetrain...");
+      Solenoid pto = new Solenoid(2, 2);
+      DriveModule leftModule = new DriveModule(new TalonFX(5), new TalonFX(6), new TalonFX(7), pto);
+      DriveModule rightModule = new DriveModule(new TalonFX(8), new TalonFX(9), new TalonFX(10), pto);
+      drive = new Drivetrain(leftModule, rightModule, detector, gyro);
+      System.out.println("done");
+    } else {
+      System.out.println("Drivetrain disabled. Skipping initialization...");
+    }
+
     if (this.autoAimToggle) {
       System.out.print("Initializing automatic aiming system...");
       aim = new AutoAim(drive, limelight, shooter, gyro);
@@ -194,7 +195,7 @@ public void robotInit() {
     System.out.println("done");
 
     System.out.print("Initializing compressor...");
-    // compressor = new Compressor(2);
+    compressor = new Compressor(2);
     System.out.println("done");
   }
 
@@ -309,13 +310,16 @@ public void teleopPeriodic() {
         aiming = false;
       }
 
-      if (operator.getYButtonPressed() && !aiming) {
+      boolean driverYPressed = driver.getYButtonPressed();
+      if (driverYPressed && !aiming) {
         aiming = true;
         aim.resetState();
-      } else if (operator.getYButtonPressed() && aiming) {
+      } else if (driverYPressed && aiming) {
+        System.out.println("Here");
         aiming = false;
         aim.stopState();
       }
+
       limelight.setLightEnabled(aiming);
       aim.run();
     }

From c46a4cff04fdf94c6d8a5cebc92a30226755cc18 Mon Sep 17 00:00:00 2001
From: Programmer <programmer@redalert1741.org>
Date: Fri, 6 Mar 2020 17:41:44 -0500
Subject: [PATCH 27/28] Limelight tuning

---
 src/main/java/frc/robot/AutoAim.java | 13 ++++++++++---
 src/main/java/frc/robot/Robot.java   | 18 +++++++++++-------
 2 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 6e3eed5..3eb88b4 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -1,6 +1,7 @@
 package frc.robot;
 
 import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class AutoAim {
 
@@ -58,6 +59,10 @@ public void run() {
             case IDLE:
                 break;
         }
+
+        SmartDashboard.putString("AutoAimState", state.toString());
+        SmartDashboard.putNumber("MotorPower", motorPower);
+        SmartDashboard.putNumber("gyroDegrees", gyroDegrees);
     }
 
     /**
@@ -82,12 +87,14 @@ private void SetAngles() {
      * Aims to robot to the X-coordinate of the power port.
      */
     private void AimX() {
-        error = (gyroDegrees - gyro.getYaw()) / 29.8;
-        motorPower = -.2 * error;
+        // error = (gyroDegrees - gyro.getYaw()) / 29.8;
+        error = (gyroDegrees - gyro.getYaw());
+        // motorPower = -.5 * error;
+        motorPower = -error * 1;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);
         if (getWithinTolerance(gyroDegrees, gyro.getYaw(), 1)) {
-            state = AutoAimState.AIM_ANGLE;
+            state = AutoAimState.IDLE;
         }
     }
 
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index e3425fd..9059e72 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -217,11 +217,11 @@ public void teleopInit() {
   public void teleopPeriodic() {
     if (this.limelightToggle) {
       limelight.update();
-      // if (driver.getXButtonPressed()) {
-      // limelight.setLightEnabled(true);
-      // } else if (driver.getYButtonPressed()) {
-      // limelight.setLightEnabled(false);
-      // }
+      if (driver.getXButtonPressed()) {
+        limelight.setLightEnabled(true);
+      } else if (driver.getYButtonPressed()) {
+        limelight.setLightEnabled(false);
+      }
     }
 
     // if (this.powercellDetectorToggle) {
@@ -232,6 +232,11 @@ public void teleopPeriodic() {
       double shooterAngleSpeed = 0;
 
       if (operator.getTriggerAxis(Hand.kRight) > 0.5) {
+        // if (operator.getYButton()) {
+        // speed -= 0.01;
+        // } else if (operator.getXButton()) {
+        // speed += 0.01;
+        // }
         speed = -1 * operator.getY(Hand.kRight);
         shooterAngleSpeed = operator.getY(Hand.kLeft);
       }
@@ -315,12 +320,11 @@ public void teleopPeriodic() {
         aiming = true;
         aim.resetState();
       } else if (driverYPressed && aiming) {
-        System.out.println("Here");
         aiming = false;
         aim.stopState();
       }
 
-      limelight.setLightEnabled(aiming);
+      // limelight.setLightEnabled(aiming);
       aim.run();
     }
   }

From 85f52f7a0cf61ddbb1e086a6adaf9d64cec7ce1e Mon Sep 17 00:00:00 2001
From: ultragamer1010 <31806444+ultragamer1010@users.noreply.github.com>
Date: Sun, 8 Mar 2020 11:16:45 -0400
Subject: [PATCH 28/28] Change robot is aim X and Angle at the same time

---
 src/main/java/frc/robot/AutoAim.java | 27 +++++++++++++++------------
 1 file changed, 15 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/AutoAim.java b/src/main/java/frc/robot/AutoAim.java
index 909a7a6..59c20bb 100644
--- a/src/main/java/frc/robot/AutoAim.java
+++ b/src/main/java/frc/robot/AutoAim.java
@@ -13,8 +13,7 @@ public class AutoAim {
 
     public enum AutoAimState {
         SET_ANGLES,
-        AIM_X,
-        AIM_ANGLE,
+        AIM,
         IDLE;
     }
 
@@ -51,11 +50,8 @@ public void run() {
                 SetAngles();
                 break;
 
-            case AIM_X:
+            case AIM:
                 AimX();
-                break;
-
-            case AIM_ANGLE:
                 AimAngle();
                 break;
 
@@ -72,7 +68,7 @@ private void SetAngles() {
             sweeping = false;
             drive.stopSweep();
             gyroDegrees = gyro.getYaw() + limelight.getTargetXDegrees();
-            state = AutoAimState.AIM_X;
+            state = AutoAimState.AIM;
         } else {
             if (!sweeping) {
                 drive.resetState();
@@ -90,9 +86,6 @@ private void AimX() {
         motorPower = .5 * error;
         drive.driveLeft(motorPower);
         drive.driveRight(-motorPower);
-        if (getWithinTolerance(gyroDegrees, gyro.getYaw(), 1)) {
-            state = AutoAimState.AIM_ANGLE;
-        }
     }
 
     /**
@@ -101,8 +94,18 @@ private void AimX() {
     private void AimAngle() {
         angleDegrees = 1 *limelight.getTargetVertical(); //TODO: Use a table from testing to create an equation to plug this into
         shooter.setAngle(angleDegrees);
-        if (getWithinTolerance(angleDegrees, shooter.getAngleInDegrees(), 1)) {
-            state = AutoAimState.IDLE;
+    }
+
+    /**
+     * 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;
         }
     }