From 5cdef72c879826bbb17052b462a79970533245bf Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 19 Mar 2022 21:31:34 -0600
Subject: [PATCH] sought

---
 .../robot/commands/ShooterCommands/Seek.java  | 29 ++++++++++++++
 .../robot/commands/ShooterCommands/Shoot.java | 40 +++++++++++--------
 2 files changed, 53 insertions(+), 16 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
new file mode 100644
index 0000000..a706512
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.ShooterCommands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.VisionOdometry;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class Seek extends SequentialCommandGroup {
+
+  /** Seeks. 
+   * Seeking -> Sought
+   * @author Aarav Shah
+   * @blame Aarav Shah (thomas did this)
+  */
+  public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(new Shoot(swerve, drum, turret, hood), new TrackTarget(turret, drum, hood, visionOdometry));
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
index f12e518..d172256 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
@@ -25,6 +25,8 @@ public class Shoot extends CommandBase {
   private Turret turret;
   private Hood hood;
 
+  private boolean toShoot;
+
   // given
   private double gyroAngle;
   private double odoX;
@@ -67,13 +69,15 @@ public class Shoot extends CommandBase {
    * 
    * @author Aarav Shah
    */
-  public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
+  public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, boolean toShoot) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.swerve = swerve;
     this.drum = drum;
     this.turret = turret;
     this.hood = hood;
-    
+
+    this.toShoot = toShoot;
+
     addRequirements(this.swerve, this.drum, this.turret, this.hood);
 
     kP = gains.kP;
@@ -85,7 +89,7 @@ public class Shoot extends CommandBase {
     derivative = 0;
     time = 0.02;
 
-    tolerance = 5.0;
+    tolerance = 10.0;
     isAimedInTolerance = false;
 
     if (simMode) {
@@ -96,6 +100,10 @@ public class Shoot extends CommandBase {
     }
   }
 
+  public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
+    this(swerve, drum, turret, hood, false);
+  }
+
   /**
    * Updates error for custom PID.
    */
@@ -131,21 +139,19 @@ public class Shoot extends CommandBase {
     targetHood = drum.getHood(distance);
 
     targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
-    // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
 
     // deadzone processing
     if (AimToCenter.isDeadzone(targetAngle)) {}
     
     // initial error
     updateError();
-    // System.out.println("Error: " + error);
     prevError = error;
   }
   /**
    * Run custom PID.
    */
   public void runPID() {
-    if (error > 180){
+    if (error > 180) {
       error = 360 - error;
       inverted = -1;
     }
@@ -156,10 +162,10 @@ public class Shoot extends CommandBase {
     updateError();
     
     proportional = error;
-    integral = integral + error * time;
+    integral = integral + (error * time);
     derivative = (error - prevError) / time;
     output = kP * proportional + kI * integral + kD * derivative;
-    normOutput = output/360 * inverted;
+    normOutput = (output / 360) * inverted;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -178,28 +184,30 @@ public class Shoot extends CommandBase {
     }
 
     runPID();
+
     SmartDashboard.putNumber("Error", this.error);
     SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
     SmartDashboard.putNumber("Normalized Output", normOutput);
-    swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
-                                                 // entire swerve drive or its the line below
-    // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
-    
-    // m_hood.runAngleAdjustPID(m_targetHood);
-    // m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
+
+    swerve.driveWithInput(0, 0, normOutput, true);
+    turret.m_boomBoomRotateMotor.set(normOutput);
+
+    if (this.toShoot) {
+      this.hood.runAngleAdjustPID(this.targetHood);
+      this.drum.runDrumShooterVelocityPID(this.targetVel);
+    }
 
     if (simMode) {
       turretDummy.apply(normOutput);
       System.out.println("Turret Dummy: " + turretDummy.get());
     }
-    turret.m_boomBoomRotateMotor.set(normOutput);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     // return to initial swerve rotation
-    swerve.driveWithInput(0, 0, initialSwerveRotation, true);
+    // swerve.driveWithInput(0, 0, initialSwerveRotation, true);
   }
 
   // Returns true when the command should end.