Merge pull request #17 from strykeforce/proto

Initial Proto Bringup

Author: mwitcpalek

src/main/java/frc/robot/RobotContainer.java

@@ -17,11 +17,15 @@
 import frc.robot.commands.coral.EnableEjectBeamCommand;
 import frc.robot.commands.coral.OpenLoopCoralCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
+import frc.robot.commands.drive.ResetGyroCommand;
+import frc.robot.commands.elevator.HoldElevatorCommand;
 import frc.robot.commands.elevator.JogElevatorCommand;
+import frc.robot.commands.elevator.SetElevatorPositionCommand;
 import frc.robot.commands.elevator.ZeroElevatorCommand;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.RobotConstants;
 import frc.robot.controllers.FlyskyJoystick;
+import frc.robot.controllers.FlyskyJoystick.Button;
 import frc.robot.subsystems.algae.AlgaeIOFX;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.coral.CoralIO;
@@ -78,13 +82,17 @@ private void configureTelemetry() {
     telemetryService.register(coralSubsystem);
     telemetryService.register(algaeSubsystem);
     telemetryService.register(elevatorSubsystem);
+    elevatorIO.registerWith(telemetryService);
     telemetryService.start();
   }
 
   private void configureDriverBindings() {
     driveSubsystem.setDefaultCommand(
         new DriveTeleopCommand(
             () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
+
+    // Reset Gyro Command
+    new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
   }
 
   private void configureOperatorBindings() {
@@ -94,23 +102,25 @@ private void configureOperatorBindings() {
 
     // Intake Coral
     new JoystickButton(xboxController, XboxController.Button.kY.value)
-        .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
-        .onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
+        .onTrue(new OpenLoopCoralCommand(coralSubsystem, 0.5))
+        .onTrue(new EnableEjectBeamCommand(true, coralSubsystem));
 
     // Eject Coral
     new JoystickButton(xboxController, XboxController.Button.kA.value)
-        .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
-        .onTrue(new EnableEjectBeamCommand(true, coralSubsystem));
+        .onTrue(new OpenLoopCoralCommand(coralSubsystem, 1))
+        .onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
 
     // Move Elevator
-    new Trigger((() -> xboxController.getRightY() > RobotConstants.kJoystickDeadband))
+    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
         .onTrue(
             new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmount, Rotations)));
-    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kJoystickDeadband))
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
         .onTrue(
             new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(-ElevatorConstants.kJogAmount, Rotations)));
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
 
     // Zero Elevator
     new JoystickButton(xboxController, XboxController.Button.kX.value)
@@ -121,6 +131,25 @@ private void configureOperatorBindings() {
         .onTrue(new OpenLoopAlgaeCommand(algaeSubsystem, 0.5));
     new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
         .onTrue(new OpenLoopAlgaeCommand(algaeSubsystem, -0.5));
+
+    // Elevator setpoint testing
+    new JoystickButton(xboxController, XboxController.Button.kStart.value)
+        .onTrue(
+            new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kFunnelSetpoint));
+    new JoystickButton(xboxController, XboxController.Button.kBack.value)
+        .onTrue(new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kStowSetpoint));
+    (new Trigger(() -> xboxController.getPOV() == 0))
+        .onTrue(
+            new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL1CoralSetpoint));
+    (new Trigger(() -> xboxController.getPOV() == 90))
+        .onTrue(
+            new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL2CoralSetpoint));
+    (new Trigger(() -> xboxController.getPOV() == 180))
+        .onTrue(
+            new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL3CoralSetpoint));
+    (new Trigger(() -> xboxController.getPOV() == 270))
+        .onTrue(
+            new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/algae/OpenLoopAlgaeCommand.java

@@ -10,6 +10,7 @@ public class OpenLoopAlgaeCommand extends InstantCommand {
   public OpenLoopAlgaeCommand(AlgaeSubsystem algaeSubsystem, double pct) {
     this.algaeSubsystem = algaeSubsystem;
     this.pct = pct;
+    addRequirements(algaeSubsystem);
   }
 
   @Override

src/main/java/frc/robot/commands/coral/EnableEjectBeamCommand.java

@@ -5,9 +5,9 @@
 
 public class EnableEjectBeamCommand extends InstantCommand {
   private CoralSubsystem coralSubsystem;
-  private Boolean enable;
+  private boolean enable;
 
-  public EnableEjectBeamCommand(Boolean enable, CoralSubsystem coralSubsystem) {
+  public EnableEjectBeamCommand(boolean enable, CoralSubsystem coralSubsystem) {
     this.coralSubsystem = coralSubsystem;
     this.enable = enable;
   }

src/main/java/frc/robot/commands/coral/OpenLoopCoralCommand.java

@@ -10,6 +10,7 @@ public class OpenLoopCoralCommand extends InstantCommand {
   public OpenLoopCoralCommand(CoralSubsystem coralSubsystem, double pct) {
     this.coralSubsystem = coralSubsystem;
     this.pct = pct;
+    addRequirements(coralSubsystem);
   }
 
   @Override

src/main/java/frc/robot/commands/drive/ResetGyroCommand.java

@@ -0,0 +1,18 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class ResetGyroCommand extends InstantCommand {
+  DriveSubsystem driveSubsystem;
+
+  public ResetGyroCommand(DriveSubsystem driveSubsystem) {
+    this.driveSubsystem = driveSubsystem;
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.teleResetGyro();
+  }
+}

src/main/java/frc/robot/commands/elevator/HoldElevatorCommand.java

@@ -0,0 +1,18 @@
+package frc.robot.commands.elevator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+
+public class HoldElevatorCommand extends InstantCommand {
+  private ElevatorSubsystem elevatorSubsystem;
+
+  public HoldElevatorCommand(ElevatorSubsystem elevatorSubsystem) {
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(elevatorSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    elevatorSubsystem.setPosition(elevatorSubsystem.getPosition());
+  }
+}

src/main/java/frc/robot/commands/elevator/JogElevatorCommand.java

@@ -12,15 +12,21 @@ public class JogElevatorCommand extends Command {
   public JogElevatorCommand(ElevatorSubsystem elevatorSubsystem, Angle positionChange) {
     this.elevatorSubsystem = elevatorSubsystem;
     this.positionChange = positionChange;
+    addRequirements(elevatorSubsystem);
   }
 
   @Override
   public void initialize() {
     elevatorSubsystem.setPosition(elevatorSubsystem.getPosition().plus(positionChange));
   }
 
+  @Override
+  public void execute() {
+    elevatorSubsystem.setPosition(elevatorSubsystem.getPosition().plus(positionChange));
+  }
+
   @Override
   public boolean isFinished() {
-    return elevatorSubsystem.isFinished();
+    return false;
   }
 }

src/main/java/frc/robot/commands/elevator/SetElevatorPositionCommand.java

@@ -12,6 +12,7 @@ public class SetElevatorPositionCommand extends Command {
   public SetElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, Angle position) {
     this.elevatorSubsystem = elevatorSubsystem;
     this.position = position;
+    addRequirements(elevatorSubsystem);
   }
 
   @Override

src/main/java/frc/robot/commands/elevator/ZeroElevatorCommand.java

@@ -2,17 +2,24 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates;
 
 public class ZeroElevatorCommand extends Command {
 
   private ElevatorSubsystem elevatorSubsystem;
 
   public ZeroElevatorCommand(ElevatorSubsystem elevatorSubsystem) {
     this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(elevatorSubsystem);
   }
 
   @Override
   public void initialize() {
     elevatorSubsystem.zero();
   }
+
+  @Override
+  public boolean isFinished() {
+    return elevatorSubsystem.getState() == ElevatorStates.ZEROED;
+  }
 }

src/main/java/frc/robot/constants/BiscuitConstants.java

@@ -22,8 +22,8 @@ public class BiscuitConstants {
   // These are all wrong right now because we don't have any actual info
 
   public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined
-  public static int talonID = 3;
-  public static double kCloseEnough = 2137473647; // This is a little out of wack.
+  public static int talonID = 25;
+  public static double kCloseEnough = 0.05; // This was a little out of wack.
   public static final Angle kMaxFwd = Rotations.of(100);
   public static final Angle kMaxRev = Rotations.of(-100);