Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/Constants.java

@@ -34,6 +34,7 @@ import frc4388.utility.LEDPatterns;
  */
 public final class Constants {
   public static final double TICKS_PER_ROTATION_FX = 2048;
+  public static final double LOOP_TIME = 0.02;
 
   public static final class SwerveDriveConstants {
     public static final double ROTATION_SPEED = 3.0;

src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java

@@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.BoomBoom;
@@ -21,44 +22,33 @@ public class Shoot extends CommandBase {
 
   // subsystems
   private SwerveDrive swerve;
-  private BoomBoom drum;
   private Turret turret;
+  private BoomBoom drum;
   private Hood hood;
 
   private boolean toShoot;
 
   // given
-  private double gyroAngle;
-  private double odoX;
-  private double odoY;
+  private double odoX, odoY;
   private double distance;
+  private double gyroAngle;
 
   // targets
-  private double targetVel;
-  private double targetHood;
-  private double targetAngle;
-  private Pose2d targetPoint;
+  private double targetAngle, targetVel, targetHood;
 
   // pid
+  private Gains gains = ShooterConstants.SHOOT_GAINS;
   private double error;
   private double prevError;
-  private Gains gains = ShooterConstants.SHOOT_GAINS;
   private double kP, kI, kD;
   private double proportional, integral, derivative;
-  private double time;
-  private double output;
-  private double normOutput;
+  private double output, normOutput;
   private double tolerance;
 
   private boolean isAimedInTolerance;
   private int inverted;
   private double initialSwerveRotation;
 
-  // testing
-  private boolean simMode = false;
-  private DummySensor driveDummy;
-  private DummySensor turretDummy;
-
   /**
    * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
    * 
@@ -87,17 +77,9 @@ public class Shoot extends CommandBase {
     proportional = 0;
     integral = 0;
     derivative = 0;
-    time = 0.02;
 
     tolerance = 10.0;
     isAimedInTolerance = false;
-
-    if (simMode) {
-      driveDummy = new DummySensor(180);
-      turretDummy = new DummySensor(180);
-
-      DummySensor.resetAll();
-    }
   }
 
   public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
@@ -108,115 +90,96 @@ public class Shoot extends CommandBase {
    * Updates error for custom PID.
    */
   public void updateError() {
-    targetPoint = SwerveDriveConstants.HUB_POSE;
-    // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
     targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
-    // error = (m_targetAngle - turretDummy.get() + 360) % 360;
     error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360;
     isAimedInTolerance = (Math.abs(error) <= tolerance);
-
-    if (simMode) {
-      SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
-      System.out.println("Target Angle: " + targetAngle);
-      System.out.println("Error: " + error);
-    }
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
 
-    turret.gotoMidpoint();
+    this.turret.gotoMidpoint();
 
-    odoX = 0;//-m_swerve.getOdometry().getY();
-    odoY = -8;//-m_swerve.getOdometry().getX();
+    this.odoX = 0;//-m_swerve.getOdometry().getY();
+    this.odoY = -8;//-m_swerve.getOdometry().getX();
 
-    gyroAngle = swerve.getRegGyro().getDegrees();
-    initialSwerveRotation = gyroAngle;
+    this.distance = Math.hypot(odoX, odoY);
+
+    this.gyroAngle = this.swerve.getRegGyro().getDegrees();
+    this.initialSwerveRotation = gyroAngle;
 
     // get targets (shooter tables)
-    targetVel = drum.getVelocity(distance);
-    targetHood = drum.getHood(distance);
+    this.targetVel = drum.getVelocity(distance);
+    this.targetHood = drum.getHood(distance);
 
-    targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
-
-    // deadzone processing
-    if (AimToCenter.isDeadzone(targetAngle)) {}
+    this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
     
     // initial error
     updateError();
     prevError = error;
   }
+
   /**
    * Run custom PID.
    */
   public void runPID() {
     if (error > 180) {
       error = 360 - error;
-      inverted = -1;
+      this.inverted = -1;
     }
     else{
-      inverted = 1;
+      this.inverted = 1;
     }
     prevError = error;
     updateError();
     
-    proportional = error;
-    integral = integral + (error * time);
-    derivative = (error - prevError) / time;
-    output = kP * proportional + kI * integral + kD * derivative;
-    normOutput = (output / 360) * inverted;
+    this.proportional = error;
+    this.integral = integral + (error * Constants.LOOP_TIME);
+    this.derivative = (error - prevError) / Constants.LOOP_TIME;
+    this.output = kP * proportional + kI * integral + kD * derivative;
+    this.normOutput = (output / 360) * inverted;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
 
-    
-    if (simMode) {
-      System.out.println("Normalized Output: " + normOutput);
-    }
-    // custom pid
-    
-    if (simMode) {
-      driveDummy.apply(normOutput);
-      System.out.println("Drive Dummy: " + driveDummy.get());
-    }
-
     runPID();
 
     SmartDashboard.putNumber("Error", this.error);
     SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
-    SmartDashboard.putNumber("Normalized Output", normOutput);
+    SmartDashboard.putNumber("Normalized Output", this.normOutput);
 
-    swerve.driveWithInput(0, 0, normOutput, true);
-    turret.m_boomBoomRotateMotor.set(normOutput);
+    this.swerve.driveWithInput(0, 0, normOutput, true);
+    this.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());
-    }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    // return to initial swerve rotation
+    
+    // ? return to initial swerve rotation
     // swerve.driveWithInput(0, 0, initialSwerveRotation, true);
+
+    this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
+    this.turret.m_boomBoomRotateMotor.set(0.0);
+
+    if (this.toShoot) {
+      this.hood.runHood(0.0);
+      this.drum.runDrumShooter(0.0);
+    }
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    // if (simMode) {
     SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance);
     return isAimedInTolerance;
-    // }
-    // return false;
   }
 }