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

src/main/java/frc4388/robot/RobotContainer.java

@@ -272,7 +272,7 @@ public class RobotContainer {
         .whenReleased(() -> m_robotSwerveDrive.stopModules());
         
     new JoystickButton(getDriverController(), XboxController.Button.kB.value)
-        .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry))
+        .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry))
         .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
         
     new JoystickButton(getDriverController(), XboxController.Button.kY.value)
@@ -341,12 +341,20 @@ public class RobotContainer {
 
     new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
       .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
+      .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
+      
       .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
+      .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
+      
       .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
       // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber))
-
+      
       .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
+      .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
+      
       .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
+      .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
+
       .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
       // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber))
 

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

@@ -4,6 +4,9 @@
 
 package frc4388.robot.commands.ShooterCommands;
 
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
@@ -15,9 +18,11 @@ import frc4388.robot.subsystems.VisionOdometry;
 public class AimToCenter extends CommandBase {
   /** Creates a new AimWithOdometry. */
   Turret m_turret;
-  SwerveDrive m_drive;
   VisionOdometry m_visionOdometry;
 
+  Supplier<Pose2d> supplier;
+  Pose2d odo;
+
   // use odometry to find x and y later
   double x;
   double y;
@@ -25,28 +30,35 @@ public class AimToCenter extends CommandBase {
 
   // public static Gains m_aimGains;
 
-  public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) {
+  public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier<Pose2d> supplier) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_turret = turret;
-    m_drive = drive;
     m_visionOdometry = visionOdometry;
-    addRequirements(m_turret, m_drive, m_visionOdometry);
+
+    this.supplier = supplier;
+
+    addRequirements(m_turret, m_visionOdometry);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    x = -m_drive.getOdometry().getY();
-    y = -m_drive.getOdometry().getX();
-
+    odo = this.supplier.get();
+    // ! Yes I realize this stupid, yes it works I promise, coordinate system is funky
+    x = -odo.getY();
+    y = -odo.getX();
+    
     SmartDashboard.putNumber("trans x", x);
     SmartDashboard.putNumber("trans y", y);
   }
-
+  
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360;
+    
+    odo = this.supplier.get(); // * update odometry using really cool supplier -aarav
+
+    m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360;
     SmartDashboard.putNumber("Target Angle", m_targetAngle);
     m_turret.runShooterRotatePID(m_targetAngle);
 

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));
+  }
+}

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

@@ -20,22 +20,24 @@ import frc4388.utility.Gains;
 public class Shoot extends CommandBase {
 
   // subsystems
-  private SwerveDrive m_swerve;
-  private BoomBoom m_boomBoom;
-  private Turret m_turret;
-  private Hood m_hood;
+  private SwerveDrive swerve;
+  private BoomBoom drum;
+  private Turret turret;
+  private Hood hood;
+
+  private boolean toShoot;
 
   // given
-  private double m_gyroAngle;
-  private double m_odoX;
-  private double m_odoY;
-  private double m_distance;
+  private double gyroAngle;
+  private double odoX;
+  private double odoY;
+  private double distance;
 
   // targets
-  private double m_targetVel;
-  private double m_targetHood;
-  private double m_targetAngle;
-  private Pose2d m_targetPoint;
+  private double targetVel;
+  private double targetHood;
+  private double targetAngle;
+  private Pose2d targetPoint;
 
   // pid
   private double error;
@@ -59,20 +61,24 @@ public class Shoot extends CommandBase {
 
   /**
    * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
-   * TODO: Velocity Correction
-   * @param sDrive Drive Train
-   * @param sShooter Shooter Drum
-   * @param sTurret Shooter Turret
-   * @param sHood Shooter Hood
+   * 
+   * @param swerve Drive Train
+   * @param drum Shooter Drum
+   * @param turret Shooter Turret
+   * @param hood Shooter Hood
+   * 
+   * @author Aarav Shah
    */
-  public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
+  public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, boolean toShoot) {
     // Use addRequirements() here to declare subsystem dependencies.
-    m_swerve = sDrive;
-    m_boomBoom = sShooter;
-    m_turret = sTurret;
-    m_hood = sHood;
-    
-    addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
+    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;
     kI = gains.kI;
@@ -83,7 +89,7 @@ public class Shoot extends CommandBase {
     derivative = 0;
     time = 0.02;
 
-    tolerance = 5.0;
+    tolerance = 10.0;
     isAimedInTolerance = false;
 
     if (simMode) {
@@ -94,20 +100,24 @@ 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.
    */
   public void updateError() {
-    m_targetPoint = SwerveDriveConstants.HUB_POSE;
+    targetPoint = SwerveDriveConstants.HUB_POSE;
     // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
-    m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
+    targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
     // error = (m_targetAngle - turretDummy.get() + 360) % 360;
-    error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
+    error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360;
     isAimedInTolerance = (Math.abs(error) <= tolerance);
 
     if (simMode) {
       SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
-      System.out.println("Target Angle: " + m_targetAngle);
+      System.out.println("Target Angle: " + targetAngle);
       System.out.println("Error: " + error);
     }
   }
@@ -116,34 +126,32 @@ public class Shoot extends CommandBase {
   @Override
   public void initialize() {
 
-    m_turret.gotoMidpoint();
+    turret.gotoMidpoint();
 
-    m_odoX = 0;//-m_swerve.getOdometry().getY();
-    m_odoY = -8;//-m_swerve.getOdometry().getX();
+    odoX = 0;//-m_swerve.getOdometry().getY();
+    odoY = -8;//-m_swerve.getOdometry().getX();
 
-    m_gyroAngle = m_swerve.getRegGyro().getDegrees();
-    initialSwerveRotation = m_gyroAngle;
+    gyroAngle = swerve.getRegGyro().getDegrees();
+    initialSwerveRotation = gyroAngle;
 
     // get targets (shooter tables)
-    m_targetVel = m_boomBoom.getVelocity(m_distance);
-    m_targetHood = m_boomBoom.getHood(m_distance);
+    targetVel = drum.getVelocity(distance);
+    targetHood = drum.getHood(distance);
 
-    m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
-    // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
+    targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
 
     // deadzone processing
-    if (AimToCenter.isDeadzone(m_targetAngle)) {}
+    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;
     }
@@ -154,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.
@@ -174,30 +182,32 @@ public class Shoot extends CommandBase {
       driveDummy.apply(normOutput);
       System.out.println("Drive Dummy: " + driveDummy.get());
     }
-    
+
     runPID();
+
     SmartDashboard.putNumber("Error", this.error);
-    SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle);
+    SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
     SmartDashboard.putNumber("Normalized Output", normOutput);
-    m_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());
     }
-    m_turret.m_boomBoomRotateMotor.set(normOutput);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     // return to initial swerve rotation
-    m_swerve.driveWithInput(0, 0, initialSwerveRotation, true);
+    // swerve.driveWithInput(0, 0, initialSwerveRotation, true);
   }
 
   // Returns true when the command should end.

src/main/java/frc4388/robot/subsystems/Hood.java

@@ -35,6 +35,7 @@ public class Hood extends SubsystemBase {
   public double m_fireAngle;
   
   public double speedLimiter;
+  public double calibrationSpeed = 1.0;
 
   /** Creates a new Hood. */
   public Hood(CANSparkMax angleAdjusterMotor) {
@@ -63,23 +64,30 @@ public class Hood extends SubsystemBase {
     SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
 
     // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
-    double currentPos = this.getEncoderPosition();
-    double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
-    double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
 
-    if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
-    }
+    if (areSoftLimitsEnabled()) {
+      double currentPos = this.getEncoderPosition();
+      double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
+      double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
 
-    if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
-    }
+      if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
+        this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
+      }
 
-    if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
-      this.speedLimiter = 1.0;
+      if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
+        this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
+      }
+
+      if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
+        this.speedLimiter = 1.0;
+      }
     }
   }
 
+  public boolean areSoftLimitsEnabled() {
+    return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
+  }
+
     /**
    * Set status of hood motor soft limits.
    * @param set Boolean to set soft limits to.
@@ -107,7 +115,7 @@ public class Hood extends SubsystemBase {
   * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle)
   */
   public void runHood(double input) {
-    m_angleAdjusterMotor.set(input * this.speedLimiter);
+    m_angleAdjusterMotor.set(input * this.speedLimiter * this.calibrationSpeed);
   }
 
   /**

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -46,7 +46,8 @@ public class Turret extends SubsystemBase {
   long leftCurrentTime;
   long leftElapsedTime;
 
-  double speedLimiter;
+  public double speedLimiter;
+  public double calibrationSpeed = 1.0;
 
   public Turret(CANSparkMax boomBoomRotateMotor) {
 
@@ -148,23 +149,31 @@ public class Turret extends SubsystemBase {
   
   
     // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
-    double currentPos = this.getEncoderPosition();
-    double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
-    double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
 
-    if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
-    }
+    if (areSoftLimitsEnabled()) {
 
-    if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
-      this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
-    }
+      double currentPos = this.getEncoderPosition();
+      double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
+      double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
 
-    if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {
-      this.speedLimiter = 1.0;
+      if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
+        this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
+      }
+
+      if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
+        this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
+      }
+
+      if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {
+        this.speedLimiter = 1.0;
+      }
     }
   }
 
+  public boolean areSoftLimitsEnabled() {
+    return this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
+  }
+
   /**
    * Set status of turret motor soft limits.
    * @param set Boolean to set soft limits to.
@@ -192,7 +201,7 @@ public class Turret extends SubsystemBase {
    * @param input from -1.0 to 1.0, positive is clockwise
    */
   public void runTurretWithInput(double input) {
-    m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter);
+    m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed);
   }
 
   public void runShooterRotatePID(double targetAngle) {