From 74f60f0e853bb54f22c214740c79f655fcac3be9 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 7 Mar 2020 15:24:25 -0700
Subject: [PATCH] Add Gyro for Turret Angle

Co-Authored-By: Keenan D. Buckley <hfocus@users.noreply.github.com>
---
 .../frc4388/robot/subsystems/Shooter.java     | 17 +++-----
 .../frc4388/robot/subsystems/ShooterAim.java  | 40 +++++++++++++++++++
 2 files changed, 46 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index c2186f0..7ed9f10 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -83,21 +83,16 @@ public class Shooter extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     try{
-    SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
+      SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
 
-    SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
+      SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
 
-    SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
+      SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
 
-    SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
+      SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
 
-    SmartDashboard.putBoolean("Drum Ready", m_isDrumReady);
-
-    //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
-    }
-
-    catch(Exception e)
-    {
+      SmartDashboard.putBoolean("Drum Ready", m_isDrumReady);
+    } catch(Exception e) {
       
     }
   }
diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
index 6433d52..3709d90 100644
--- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java
+++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java
@@ -17,6 +17,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.ControlType;
 
+import edu.wpi.first.wpilibj.GyroBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ShooterConstants;
@@ -28,6 +29,7 @@ public class ShooterAim extends SubsystemBase {
   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 boolean m_isAimReady = false;
 
@@ -42,6 +44,8 @@ public class ShooterAim extends SubsystemBase {
     //resetGyroShooterRotate();
     m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
 
+    m_turretGyro = getGyroInterface();
+
     m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
     m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
     m_shooterRightLimit.enableLimitSwitch(true);
@@ -61,8 +65,44 @@ public class ShooterAim extends SubsystemBase {
     SmartDashboard.putNumber("Turret Angle Raw", getShooterRotatePosition());
 
     SmartDashboard.putBoolean("Aim Ready", m_isAimReady);
+
+    SmartDashboard.putData("Turret Angle", m_turretGyro);
   }
 
+  public GyroBase getGyroInterface() {
+    return new GyroBase(){
+    
+      @Override
+      public void close() throws Exception {
+        // TODO Auto-generated method stub
+        
+      }
+    
+      @Override
+      public void reset() {
+        // TODO Auto-generated method stub
+        
+      }
+    
+      @Override
+      public double getRate() {
+        // TODO Auto-generated method stub
+        return 0;
+      }
+    
+      @Override
+      public double getAngle() {
+        // TODO Auto-generated method stub
+        return getShooterRotatePosition();
+      }
+    
+      @Override
+      public void calibrate() {
+        // TODO Auto-generated method stub
+        
+      }
+    };
+  }
   /**
    * Passes subsystem needed.
    * @param subsystem Subsystem needed.