From 92777c105d3bc9c555ddbd48abeed13813b3a734 Mon Sep 17 00:00:00 2001
From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com>
Date: Sat, 19 Feb 2022 15:05:49 -0700
Subject: [PATCH] updated aim to center

---
 .../frc4388/robot/commands/AimToCenter.java   | 14 +++++++++-
 .../java/frc4388/robot/subsystems/Turret.java | 28 ++++++-------------
 2 files changed, 22 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java
index a284d8a..b63c5c5 100644
--- a/src/main/java/frc4388/robot/commands/AimToCenter.java
+++ b/src/main/java/frc4388/robot/commands/AimToCenter.java
@@ -36,7 +36,19 @@ public class AimToCenter extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x);
+    if (x > 0) {
+      m_targetAngle = 180 + Math.atan(y/x) - m_drive.gyro.getAngle();
+    } 
+    if (x < 0) {
+      m_targetAngle = 360 + Math.atan(y/x) - m_drive.gyro.getAngle();
+    } 
+    if (x == 0 && y > 0) {
+      m_targetAngle = 270 - m_drive.gyro.getAngle(); 
+    }
+    if (x == 0 && y < 0) {
+      m_targetAngle = 90 - m_drive.gyro.getAngle();
+    }
+    
     m_turret.runshooterRotatePID(m_targetAngle);
   }
   
diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java
index 5f785c5..530de78 100644
--- a/src/main/java/frc4388/robot/subsystems/Turret.java
+++ b/src/main/java/frc4388/robot/subsystems/Turret.java
@@ -42,8 +42,6 @@ public class Turret extends SubsystemBase {
   SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController();
   public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder();
   
-  
-  
   //Variables
   public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument
 
@@ -62,17 +60,18 @@ public class Turret extends SubsystemBase {
     m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
     m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit
     m_boomBoomRotateMotor.setInverted(false);
-    
+
+    m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
+    m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
+    m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
+    m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
+    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
+    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
   }
 
 
   @Override
   public void periodic() {
-    // SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition());
-
-    // SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro);
-
-    // SmartDashboard.putBoolean("Turret Aimed", m_isAimReady);
     // This method will be called once per scheduler run
   }
 
@@ -81,20 +80,11 @@ public class Turret extends SubsystemBase {
     m_sDriveSubsystem = subsystem1;
   }
 
-  public void runTurretWithInput(double input) { //Rename to turret
+  public void runTurretWithInput(double input) {
     m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
   }
 
-  public void runshooterRotatePID(double targetAngle) { //Split into configure and run
-    m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
-    m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
-    m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
-    m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
-    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
-
-
-    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
-
+  public void runshooterRotatePID(double targetAngle) {
     targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
     m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition);
   }