From 41597c99de0af470f7fe7e43dc62f17e671a654f Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Tue, 10 Mar 2020 18:41:56 -0600
Subject: [PATCH] Shooter Aim Angles

---
 src/main/java/frc4388/robot/Constants.java    |  2 +-
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../robot/commands/shooter/TrackTarget.java   |  1 +
 .../frc4388/robot/subsystems/ShooterAim.java  | 36 ++++++++++++++++---
 4 files changed, 34 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 3da8fc8..47e2c81 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -138,7 +138,7 @@ public final class Constants {
         public static final int TURRET_LEFT_SOFT_LIMIT = -55;
         public static final double TURRET_SPEED_MULTIPLIER = 0.3;
         public static final double TURRET_CALIBRATE_SPEED = 0.075;
-        public static final double TURRET_MOTOR_ROTS_PER_ROT = 1; //TODO: Find
+        public static final double TURRET_MOTOR_ROTS_PER_ROT = 89.56696;
         public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find
 
         public static final int HOOD_UP_SOFT_LIMIT = 33;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index bbe5ff6..91c05ab 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -100,7 +100,7 @@ public class RobotContainer {
 
         m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
         m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
-        m_robotShooterAim.passRequiredSubsystem(m_robotShooter);
+        m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive);
 
         m_robotLeveler.passRequiredSubsystem(m_robotClimber);
 
diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
index 6d35f49..7e61550 100644
--- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
@@ -103,6 +103,7 @@ public class TrackTarget extends CommandBase {
       
       m_shooter.m_fireVel = fireVel;
       m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
+      m_shooterAim.m_targetDistance = distance;
     }
   }
 
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index ae3f5c5..c1c4ebc 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -25,12 +25,15 @@ import frc4388.utility.Gains;
 
 public class ShooterAim extends SubsystemBase {
   public Shooter m_shooterSubsystem;
+  public Drive m_driveSubsystem;
   
   public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
   public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
   CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
   public GyroBase m_turretGyro;
 
+  public double m_targetDistance = 0;
+
   public boolean m_isAimReady = false;
 
   // Configure PID Controllers
@@ -73,8 +76,9 @@ public class ShooterAim extends SubsystemBase {
    * Passes subsystem needed.
    * @param subsystem Subsystem needed.
    */
-  public void passRequiredSubsystem(Shooter subsystem) {
-    m_shooterSubsystem = subsystem;
+  public void passRequiredSubsystem(Shooter subsystem0, Drive subsystem1) {
+    m_shooterSubsystem = subsystem0;
+    m_driveSubsystem = subsystem1;
   }
 
   public void runShooterWithInput(double input) {
@@ -109,11 +113,33 @@ public class ShooterAim extends SubsystemBase {
     return m_shooterRotateEncoder.getPosition();
   }
 
-  public double getShooterRotatePositionDegrees()
-  {
+  public double getShooterAngleDegrees() {
     return (m_shooterRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
   }
 
+  /**
+   * Gets the angle of the Shooter relative to the target.
+   */
+  public double getTargetAngleDegrees() {
+    return m_driveSubsystem.getHeading() + getShooterAngleDegrees();
+  }
+
+  public double getTargetXDisplacement() {
+    return m_targetDistance * Math.cos(getTargetAngleDegrees());
+  }
+
+  public double getTargetYDisplacement() {
+    return m_targetDistance * Math.sin(getTargetAngleDegrees());
+  }
+
+  /**
+   * Gets the angle of the Shooter relative to the inner port.
+   * Use for tuning the Shooter Displacement
+   */
+  public double getInnerPortAngleDegrees() {
+    return Math.atan( getTargetYDisplacement() / (getTargetXDisplacement() + 29.25) );
+  }
+
   public GyroBase getGyroInterface() {
     return new GyroBase(){
     
@@ -138,7 +164,7 @@ public class ShooterAim extends SubsystemBase {
       @Override
       public double getAngle() {
         // TODO Auto-generated method stub
-        return getShooterRotatePositionDegrees();
+        return getShooterAngleDegrees();
       }
     
       @Override