Preferred operator controls

Added preferred operator controls
Added manual controls
Renamed misspelled items

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

@@ -237,7 +237,7 @@ public final class Constants {
             public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
             public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
             public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
-            public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0);
+            public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
 
             public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
             public static final int LIDAR_DIO_CHANNEL = 7;
@@ -430,21 +430,21 @@ public final class Constants {
         public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
         //Max for endefector = 60%
 
-        public static final double L2_SCORE_ENDEFECTOR = -19;
+        public static final double L2_SCORE_ENDEFFECTOR = -19;
 
-        public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
-        public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
+        public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
-        public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
-        public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
-        public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
+        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
-        public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
 
         public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
             .withKP(1)
             .withKI(0)
             .withKD(0);
         
-        public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs()
+        public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
             .withKP(1)
             .withKI(0)
             .withKD(0);

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

@@ -18,6 +18,7 @@ import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.utility.controller.XboxController;
@@ -83,7 +84,7 @@ public class RobotContainer {
     // private final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
     public final Lidar m_lidar = new Lidar();
-    public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch);
+    public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
@@ -452,7 +453,7 @@ public class RobotContainer {
 
             // Cancel button
             new JoystickButton(getButtonBox(), ButtonBox.White)
-                .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
+                .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
 
             // Score prep        
             // Prime 4
@@ -508,9 +509,12 @@ public class RobotContainer {
                 .onTrue(AprilLidarAlignL2Right);
 
             //Controller Lower Algae Removal
-            
+            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
+                .onTrue(leftAlgaeRemove);
 
             //Controller Upper Algae Removal
+            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
+                .onTrue(rightAlgaeRemove);
             
             //Button Box Coral Scoring
             new JoystickButton(getButtonBox(), ButtonBox.Five)
@@ -541,7 +545,14 @@ public class RobotContainer {
 
             // Cancel button
             new JoystickButton(getButtonBox(), ButtonBox.White)
-                .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
+                .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
+
+            //Manual Controls
+            m_robotElevator.setDefaultCommand(new RunCommand(() -> {
+                m_robotElevator.manualEndeffectorVel(getDeadbandedOperatorController().getLeftY());
+                m_robotElevator.manualElevatorVel(getDeadbandedOperatorController().getRightY());
+            }, m_robotElevator)
+            .withName("Default Manual Controls"));
         
         }
 

src/main/java/frc4388/robot/RobotMap.java

@@ -57,7 +57,7 @@ public class RobotMap {
     public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
 
     public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
-    public final DigitalInput endefectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+    public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
 
     void configureDriveMotorControllers() {}
         

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

@@ -31,6 +31,6 @@ public class waitElevatorRefrence extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevator.elevatorAtRefrence();
+    return elevator.elevatorAtReference();
   }
 }

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

@@ -31,6 +31,6 @@ public class waitEndefectorRefrence extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevator.endefectorAtRefrence();
+    return elevator.endeffectorAtReference();
   }
 }

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

@@ -24,20 +24,20 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
 public class Elevator extends SubsystemBase {
   /** Creates a new Elevator. */
   private TalonFX elevatorMotor;
-  private TalonFX endefectorMotor;
+  private TalonFX endeffectorMotor;
 
   private long wait = 0;
   private long maxWait = 1000;
 
   private double elevatorRefrence = 0;
-  private double endefectorRefrence = 0;
+  private double endeffectorRefrence = 0;
 
   private DigitalInput basinBeamBreak;
-  private DigitalInput endefectorLimitSwitch;
+  private DigitalInput endeffectorLimitSwitch;
 
   public enum CoordinationState {
     Waiting, // for coral into the though
-    WatingBeamTriped, //once the beam break trips
+    WatingBeamTripped, //once the beam break trips
     Ready, // Has coral in endefector
     Hovering, // Has coral in endefector
     L2Score,
@@ -53,18 +53,18 @@ public class Elevator extends SubsystemBase {
 
   private CoordinationState currentState;
 
-  public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) {
+  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch) {
     elevatorMotor = elevatorTalonFX;
-    endefectorMotor = endefectorTalonFX;
+    endeffectorMotor = endeffectorTalonFX;
 
     this.basinBeamBreak = basinLimitSwitch;
-    this.endefectorLimitSwitch = endefectorLimitSwitch;
+    this.endeffectorLimitSwitch = endeffectorLimitSwitch;
 
     elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
-    endefectorMotor.setNeutralMode(NeutralModeValue.Brake);
+    endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
     
     elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
-    endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID);
+    endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
     currentState = CoordinationState.Ready;
   }
 
@@ -72,7 +72,7 @@ public class Elevator extends SubsystemBase {
 
   private void PIDPosition(TalonFX motor, double position) {
     if (motor == elevatorMotor) elevatorRefrence = position;
-    else endefectorRefrence = position;
+    else endeffectorRefrence = position;
 
     var request = new PositionDutyCycle(position);
     motor.setControl(request);
@@ -82,8 +82,8 @@ public class Elevator extends SubsystemBase {
     elevatorMotor.set(0);
   }
 
-  public void endefectorStop() {
+  public void endeffectorStop() {
-    endefectorMotor.set(0);
+    endeffectorMotor.set(0);
   }
 
   public void transitionState(CoordinationState state) {
@@ -92,79 +92,79 @@ public class Elevator extends SubsystemBase {
       case Waiting: {
         wait = System.currentTimeMillis() + maxWait;
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         break;
       }
 
-      case WatingBeamTriped: {
+      case WatingBeamTripped: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         break;
       }
 
       case Ready: {
         PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         break;
       }
 
       case Hovering: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         break;
       }
 
       case L2Score: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
       
       case PrimedFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
         break;
       }
 
       case ScoringFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
       case PrimedThree: {
         PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
         break;
       }
       
       case ScoringThree: {
         PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
       case BallRemoverL2Primed: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
         break;
       }
 
       case BallRemoverL2Go: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
       case BallRemoverL3Primed: {
         PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
         break;
       }
 
       case BallRemoverL3Go: {
         PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
         break;
       }
 
@@ -175,7 +175,7 @@ public class Elevator extends SubsystemBase {
 
   }
 
-  public boolean elevatorAtRefrence() {
+  public boolean elevatorAtReference() {
     // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
     double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
     boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5;
@@ -187,11 +187,11 @@ public class Elevator extends SubsystemBase {
     return atPos;
   }
 
-  public boolean endefectorAtRefrence() {
+  public boolean endeffectorAtReference() {
     // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
-    double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
+    double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
 
-    return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
+    return Math.abs(endeffectorRefrence - endeffectorPosition) <= 0.2;
   }
   // public void driveElevatorStick(Translation2d stick) {
   //   if (stick.getNorm() > 0.05) {
@@ -210,12 +210,20 @@ public class Elevator extends SubsystemBase {
   // }
   
   private void periodicReady() {
-    if (elevatorAtRefrence())
+    if (elevatorAtReference())
       transitionState(CoordinationState.Hovering);
   }
 
   private void periodicScoring() {
-    if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
+    if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
+  }
+
+  public void manualElevatorVel(double velocity) {
+    elevatorMotor.set(velocity);
+  }
+
+  public void manualEndeffectorVel(double velocity) {
+    endeffectorMotor.set(velocity);
   }
 
   @Override
@@ -223,12 +231,12 @@ public class Elevator extends SubsystemBase {
 
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
-    SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0);
+    SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
     SmartDashboard.putString("State", currentState.toString());
 
     if (currentState == CoordinationState.Waiting) {
       // periodicWaiting();
-    } else if (currentState == CoordinationState.WatingBeamTriped) {
+    } else if (currentState == CoordinationState.WatingBeamTripped) {
       // periodicWaitingTripped();
     } else if (currentState == CoordinationState.Ready) {
       periodicReady();