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

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

@@ -183,7 +183,7 @@ public final class Constants {
         true, 27, 0, 0);
     public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21;
     public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22;
-    public static final double TURRET_SPEED_MULTIPLIER = 0.4d;
+    public static final double TURRET_SPEED_MULTIPLIER = 0.6;
     public static final double DEGREES_PER_ROT = 180.0/105.45445251464844;
     public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
     public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;

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

@@ -175,6 +175,8 @@ public class RobotContainer {
     m_robotTurret.setDefaultCommand(
         new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), 
         m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
+
+
     m_robotHood.setDefaultCommand(
        new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
     // m_robotTurret.setDefaultCommand(
@@ -270,26 +272,25 @@ public class RobotContainer {
         .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
         .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
 
+    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+        .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
+        .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender))
+        .whenReleased(new RunCommand(() -> m_robotExtender.switchDirection(), m_robotExtender));
+
     // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
     //     .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
     //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
-    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-    //     .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
-    //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
+        .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
     // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
     //     .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));
 
-    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-        .whileHeld(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-180.0), m_robotTurret));
-
     // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
     //     .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom));
 
-    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-    //     .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
-    //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
       // Right Bumper > Storage In
     // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
     //     .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
@@ -304,8 +305,9 @@ public class RobotContainer {
         .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
 
        //B > Shoot with Lime
-    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-        .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+    //     .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+
   
     /* Button Box Buttons */
 

src/main/java/frc4388/robot/commands/RunExtenderOut.java

@@ -0,0 +1,46 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Extender;
+import frc4388.robot.subsystems.Intake;
+
+public class RunExtenderOut extends CommandBase {
+
+  private Intake intake;
+  private Extender extender;
+  private int direction;
+
+  /** Creates a new RunExtenderOut. */
+  public RunExtenderOut(Intake intake, Extender extender) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.intake = intake;
+    this.extender = extender;
+
+    this.direction = 1;
+
+    addRequirements(this.intake, this.extender);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/TrackTarget.java

@@ -23,7 +23,6 @@ import frc4388.utility.DesmosServer;
 public class TrackTarget extends CommandBase {
   /** Creates a new TrackTarget. */
   Turret m_turret;
-  SwerveDrive m_drive;
   VisionOdometry m_visionOdometry;
   BoomBoom m_boomBoom;
   Hood m_hood;
@@ -45,15 +44,14 @@ public class TrackTarget extends CommandBase {
 
   // public static Gains m_aimGains;
 
-  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) {
+  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_turret = turret;
-    m_drive = drive;
     m_boomBoom = boomBoom;
     m_hood = hood;
     m_visionOdometry = visionOdometry;
 
-    addRequirements(m_turret, m_boomBoom, m_hood, m_drive, m_visionOdometry);
+    addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
   }
 
   // Called when the command is initially scheduled.
@@ -102,6 +100,7 @@ public class TrackTarget extends CommandBase {
     }
     catch (Exception e){
       e.printStackTrace();
+      m_turret.runshooterRotatePID(180);
       // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java");
     }
     // vel = m_boomBoom.getVelocity(distance);

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

@@ -17,6 +17,7 @@ public class Extender extends SubsystemBase {
   private SparkMaxLimitSwitch m_outLimit;
 
   public boolean toggle;
+  public int direction = 1;
 
   /** Creates a new Extender. */
   public Extender(CANSparkMax extenderMotor) {
@@ -46,7 +47,12 @@ public class Extender extends SubsystemBase {
 
   public void runExtender(double input) {
     // if (!m_serializer.getBeam() && input < 0.) return;
-    m_extenderMotor.set(input);
+    if (this.direction > 0) {}
+    m_extenderMotor.set(this.direction * input);
+  }
+
+  public void switchDirection() {
+    this.direction = this.direction * -1;
   }
 
   public double getCurrent() {