extender and intake changes

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

@@ -145,6 +145,7 @@ public final class Constants {
         false, 10, 0, 0); //Find
     public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration(
         false, 15, 0, 0);
+    public static final double INTAKE_SPEED_MULTIPLIER = 0.4;
   }
 
   public static final class ExtenderConstants {

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

@@ -62,11 +62,11 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.robot.commands.AimToCenter;
-import frc4388.robot.commands.ExtenderIntakeGroup;
 import frc4388.robot.commands.RunMiddleSwitch;
-import frc4388.robot.commands.Shoot;
+import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
-import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.ShooterCommands.AimToCenter;
+import frc4388.robot.commands.ShooterCommands.Shoot;
+import frc4388.robot.commands.ShooterCommands.TrackTarget;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Extender;
@@ -96,13 +96,14 @@ import frc4388.utility.controller.DeadbandedXboxController;
  */
 public class RobotContainer {
   private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
-  /* RobotMap */
+
+  // RobotMap
   public final RobotMap m_robotMap = new RobotMap();
 
   // Subsystems 
   public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
   public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
-  public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotSerializer);
+  public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
   public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
   
   public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
@@ -115,12 +116,12 @@ public class RobotContainer {
   private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31);
   public final Climber m_robotClimber = new Climber(testElbowMotor);
 
-  /* Controllers */
+  // Controllers
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
   private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
 
-  /* Autonomous */
+  // Autonomous
   private PathPlannerTrajectory loadedPathTrajectory = null;
   private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
   private final List<Waypoint> pathPoints = new ArrayList<>();
@@ -275,20 +276,8 @@ 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.kX.value)
-    //     .whileHeld(new RunExtender(m_robotIntake, m_robotExtender, 0.5))
-    //     .whenReleased(new RunCommand(() -> RunExtender.changeDirection(), m_robotIntake, m_robotExtender));
-
     new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
         .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
-        // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection()));
-
-    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-    //     .whenPressed(new ParallelCommandGroup(
-    //                  new RunCommand(() -> m_robotIntake.m_intakeMotor.set(ExtenderIntakeGroup.direction * -0.2), m_robotIntake),
-    //                  new RunCommand(() -> m_robotExtender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 0.5), m_robotExtender)))
-    //     .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection()));
-    
     // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
     //     .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0)));
 
@@ -297,10 +286,14 @@ public class RobotContainer {
     //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-        .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
+        // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
-        // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
+        .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.runExtender(0.0), m_robotExtender));
-        .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0)));
+        // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0)));
+    
+    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.kA.value)
     //     .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));
@@ -321,8 +314,8 @@ public class RobotContainer {
     /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
         .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
       
-      new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+      // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-          .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret));
+      //     .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret));
 
        //B > Shoot with Lime
     new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
@@ -343,7 +336,7 @@ public class RobotContainer {
 
       .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
       .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
-      .whenReleased(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0), m_robotExtender));
+      .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender));
 
     // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
     //     .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true)))

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.ExtenderIntakeCommands;
 
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import frc4388.robot.subsystems.Extender;

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.ExtenderIntakeCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ExtenderConstants;
@@ -13,15 +13,28 @@ public class RunExtender extends CommandBase {
 
   private Extender extender;
 
+  private double error;
+  private double tolerance;
+
   /** Creates a new RunExtender. */
   public RunExtender(Extender extender) {
     // Use addRequirements() here to declare subsystem dependencies.
-
     this.extender = extender;
     
+    updateError();
+    tolerance = 5.0;
+    
     addRequirements(this.extender);
   }
   
+  public void updateError() {
+    if (ExtenderIntakeGroup.direction > 0) {
+      this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT);
+    } else {
+      this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_REVERSE_LIMIT);
+    }
+  }
+  
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {}
@@ -29,7 +42,9 @@ public class RunExtender extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    this.extender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 1.0); // TODO: change to 1.0 for actual speed, 0.5 is just for testing
+    System.out.println("RunExtender is working");
+    this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0);
+    updateError();
   }
 
   // Called once the command ends or is interrupted.
@@ -41,7 +56,9 @@ public class RunExtender extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (Math.abs(this.extender.m_extenderMotor.getEncoder().getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT) < 5) {
+    if (error < tolerance) {
+      System.out.println("RunExtender finished");
+      this.extender.runExtender(0.0);
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java

@@ -2,9 +2,10 @@
 // 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;
+package frc4388.robot.commands.ExtenderIntakeCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.subsystems.Intake;
 
 public class RunIntakeConditionally extends CommandBase {
@@ -28,9 +29,9 @@ public class RunIntakeConditionally extends CommandBase {
   @Override
   public void execute() {
     if (ExtenderIntakeGroup.direction > 0) {
-      this.intake.m_intakeMotor.set(-0.4);
+      this.intake.runAtOutput(-1);
     } else {
-      this.intake.m_intakeMotor.set(0);
+      this.intake.runAtOutput(0);
     }
   }
 

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.ShooterCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.ShooterCommands;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.ShooterCommands;
 
 import java.util.ArrayList;
 

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

@@ -15,13 +15,11 @@ import frc4388.robot.Constants.ExtenderConstants;
 
 public class Extender extends SubsystemBase {
   
-  public CANSparkMax m_extenderMotor;
+  private CANSparkMax m_extenderMotor;
 
   private SparkMaxLimitSwitch m_inLimit;
   private SparkMaxLimitSwitch m_outLimit;
 
-  public boolean toggle;
-
   /** Creates a new Extender. */
   public Extender(CANSparkMax extenderMotor) {
 
@@ -52,30 +50,20 @@ public class Extender extends SubsystemBase {
     SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition());
   }
 
-    /**
-   * Runs The Extender-
-   * @param extended Wether the Extender Is Extended
-   */
-  // public void runExtender(boolean extended) {
-  //   if (!m_serializer.getBeam() && !extended) return;
-  //   double extenderMotorSpeed = extended ? 0.25d : -0.25d;
-  //   m_extenderMotor.set(extenderMotorSpeed);
-  // }
-
   public void runExtender(double input) {
     // if (!m_serializer.getBeam() && input < 0.) return;
     m_extenderMotor.set(input);
   }
 
+  public double getPosition() {
+    return m_extenderMotor.getEncoder().getPosition();
+  }
+
+  public void setEncoder(double position) {
+    m_extenderMotor.getEncoder().setPosition(position);
+  }
+
   public double getCurrent() {
     return m_extenderMotor.getOutputCurrent();
   }
-
-  /**
-   * Toggles The Extender
-  */
-  // public void toggleExtender() {
-  //   toggle = !toggle;
-  //   runExtender(toggle);
-  // }
 }

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

@@ -4,27 +4,20 @@
 
 package frc4388.robot.subsystems;
 
-//Imported Limit switch ONLY
-import com.revrobotics.SparkMaxLimitSwitch;
-
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.commands.ExtenderIntakeGroup;
+import frc4388.robot.Constants.IntakeConstants;
-
+import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
-import com.revrobotics.CANSparkMax;
 
 public class Intake extends SubsystemBase {
 
-  public WPI_TalonFX m_intakeMotor;
+  private WPI_TalonFX m_intakeMotor;
-  private Serializer m_serializer;
 
   /** Creates a new Intake. */
-  public Intake(WPI_TalonFX intakeMotor, Serializer serializer) {
+  public Intake(WPI_TalonFX intakeMotor) {
     m_intakeMotor = intakeMotor;
-    m_serializer = serializer;
   }
 
   @Override
@@ -39,11 +32,19 @@ public class Intake extends SubsystemBase {
    * @param rightTrigger Right Trigger to Run Outward
    */
   public void runWithTriggers(double leftTrigger, double rightTrigger) {
-    m_intakeMotor.set((rightTrigger - leftTrigger) * 0.4);
+    m_intakeMotor.set((rightTrigger - leftTrigger) * IntakeConstants.INTAKE_SPEED_MULTIPLIER);
     SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent());
     SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent());
   }
 
+  public void runAtOutput(double output, double multiplier) {
+    m_intakeMotor.set(output * multiplier);
+  }
+
+  public void runAtOutput(double output) {
+    m_intakeMotor.set(output * IntakeConstants.INTAKE_SPEED_MULTIPLIER);
+  }
+  
   public double getCurrent() {
     return m_intakeMotor.getSupplyCurrent();
   }

src/test/java/frc4388/commands/AimToCenterTest.java

@@ -2,7 +2,8 @@ package frc4388.commands;
 
 import org.junit.Test;
 
-import frc4388.robot.commands.AimToCenter;
+import frc4388.robot.commands.ShooterCommands.AimToCenter;
+
 import org.junit.Assert;
 
 public class AimToCenterTest {