Merge pull request #7 from strykeforce/elevator

Initial Elevator

Author: mwitcpalek

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

@@ -4,43 +4,60 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Rotations;
+
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
 import frc.robot.commands.coral.OpenLoopCoralCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
+import frc.robot.commands.elevator.JogElevatorCommand;
+import frc.robot.commands.elevator.ZeroElevatorCommand;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.RobotConstants;
 import frc.robot.controllers.FlyskyJoystick;
 import frc.robot.subsystems.coral.CoralIO;
 import frc.robot.subsystems.coral.CoralIOFX;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.Swerve;
+import frc.robot.subsystems.elevator.ElevatorIO;
+import frc.robot.subsystems.elevator.ElevatorIOFX;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import org.strykeforce.telemetry.TelemetryController;
 import org.strykeforce.telemetry.TelemetryService;
 
 public class RobotContainer {
   private Swerve swerve;
   private DriveSubsystem driveSubsystem;
 
+  private ElevatorIO elevatorIO;
+  private ElevatorSubsystem elevatorSubsystem;
+
+  private final CoralIO coralIO;
+  private final CoralSubsystem coralSubsystem;
+
   private final XboxController xboxController = new XboxController(1);
   private final Joystick driveJoystick = new Joystick(0);
 
   private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
   private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new);
 
-  private final CoralSubsystem coralSubsystem;
-  private final CoralIO coralIO;
-
   public RobotContainer() {
     swerve = new Swerve();
     driveSubsystem = new DriveSubsystem(swerve);
 
     coralIO = new CoralIOFX();
     coralSubsystem = new CoralSubsystem(coralIO);
 
+    elevatorIO = new ElevatorIOFX();
+    elevatorSubsystem = new ElevatorSubsystem(elevatorIO);
+
     configureTelemetry();
     configureDriverBindings();
     configureOperatorBindings();
@@ -72,6 +89,20 @@ private void configureOperatorBindings() {
     new JoystickButton(xboxController, XboxController.Button.kA.value)
         .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
         .onTrue(new EnableEjectBeamCommand(true, coralSubsystem));
+
+    // Move Elevator
+    new Trigger((() -> xboxController.getRightY() > RobotConstants.kJoystickDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmount, Rotations)));
+    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kJoystickDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(-ElevatorConstants.kJogAmount, Rotations)));
+
+    // Zero Elevator
+    new JoystickButton(xboxController, XboxController.Button.kX.value)
+        .onTrue(new ZeroElevatorCommand(elevatorSubsystem));
   }
 
   public Command getAutonomousCommand() {

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

@@ -0,0 +1,26 @@
+package frc.robot.commands.elevator;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+
+public class JogElevatorCommand extends Command {
+
+  private ElevatorSubsystem elevatorSubsystem;
+  private Angle positionChange;
+
+  public JogElevatorCommand(ElevatorSubsystem elevatorSubsystem, Angle positionChange) {
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.positionChange = positionChange;
+  }
+
+  @Override
+  public void initialize() {
+    elevatorSubsystem.setPosition(elevatorSubsystem.getPosition().plus(positionChange));
+  }
+
+  @Override
+  public boolean isFinished() {
+    return elevatorSubsystem.isFinished();
+  }
+}

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

@@ -0,0 +1,26 @@
+package frc.robot.commands.elevator;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+
+public class SetElevatorPositionCommand extends Command {
+
+  private ElevatorSubsystem elevatorSubsystem;
+  private Angle position;
+
+  public SetElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, Angle position) {
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.position = position;
+  }
+
+  @Override
+  public void initialize() {
+    elevatorSubsystem.setPosition(position);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return elevatorSubsystem.isFinished();
+  }
+}

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

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

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

@@ -1,3 +1,110 @@
 package frc.robot.constants;
 
-public class ElevatorConstants {}
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
+import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
+import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
+
+public class ElevatorConstants {
+
+  public static final double kCloseEnoughRotations = 0.0083;
+  public static final double kMaxFwd = 0; // TODO all of these fields need to be filled out
+  public static final double kMaxRev = 0;
+  public static final int kZeroMultiple =
+      0; // some constant to multiply, add by to turn the analog input into a position
+  public static final double kZeroSpeed = -.05;
+  public static final int kZeroCounter = 3;
+  public static final double kZeroedThreshhold = .0001;
+
+  public static final int heightAnalogID = 0;
+  public static final int kFxIDMain = 20;
+  public static final int kFxIDFollow = 21;
+
+  public static final double kJogAmount = 0.1;
+
+  public static TalonFXConfiguration getBothFXConfig() {
+    TalonFXConfiguration fxConfig = new TalonFXConfiguration();
+
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs()
+            .withStatorCurrentLimitEnable(false)
+            .withStatorCurrentLimit(20)
+            .withSupplyCurrentLimit(10)
+            .withSupplyCurrentLowerLimit(8)
+            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimitEnable(true);
+    fxConfig.CurrentLimits = current;
+
+    HardwareLimitSwitchConfigs hwLimit =
+        new HardwareLimitSwitchConfigs()
+            .withForwardLimitAutosetPositionEnable(false)
+            .withForwardLimitEnable(false)
+            .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
+            .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
+            .withReverseLimitAutosetPositionEnable(false)
+            .withReverseLimitEnable(false)
+            .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
+            .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
+    fxConfig.HardwareLimitSwitch = hwLimit;
+
+    SoftwareLimitSwitchConfigs swLimit =
+        new SoftwareLimitSwitchConfigs()
+            .withForwardSoftLimitEnable(false)
+            .withForwardSoftLimitThreshold(kMaxFwd)
+            .withReverseSoftLimitEnable(false)
+            .withReverseSoftLimitThreshold(kMaxRev);
+    fxConfig.SoftwareLimitSwitch = swLimit;
+
+    Slot0Configs slot0 =
+        new Slot0Configs()
+            .withKP(0.4)
+            .withKI(0.1)
+            .withKD(0)
+            .withGravityType(GravityTypeValue.Elevator_Static)
+            .withKG(0)
+            .withKS(0)
+            .withKV(0.12)
+            .withKA(0);
+    fxConfig.Slot0 = slot0;
+
+    MotionMagicConfigs motionMagic =
+        new MotionMagicConfigs()
+            .withMotionMagicAcceleration(130)
+            .withMotionMagicCruiseVelocity(0)
+            .withMotionMagicExpo_kA(0)
+            .withMotionMagicExpo_kV(0)
+            .withMotionMagicJerk(1000);
+    fxConfig.MotionMagic = motionMagic;
+
+    MotorOutputConfigs motorOut =
+        new MotorOutputConfigs()
+            .withDutyCycleNeutralDeadband(0.01)
+            .withNeutralMode(NeutralModeValue.Coast)
+            .withInverted(InvertedValue.CounterClockwise_Positive);
+    fxConfig.MotorOutput = motorOut;
+
+    return fxConfig;
+  }
+
+  public static CurrentLimitsConfigs getZeroingCurrentLimitsConfigs() {
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs() // TODO actually have correct limits for zeroing
+            .withStatorCurrentLimitEnable(false)
+            .withStatorCurrentLimit(20)
+            .withSupplyCurrentLimit(10)
+            .withSupplyCurrentLowerLimit(8)
+            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimitEnable(true);
+    return current;
+  }
+}

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

@@ -33,7 +33,6 @@ public static TalonFXConfiguration getFXConfig() {
 
     CurrentLimitsConfigs current =
         new CurrentLimitsConfigs()
-            .withStatorCurrentLimit(10)
             .withStatorCurrentLimitEnable(false)
             .withStatorCurrentLimit(20)
             .withSupplyCurrentLimit(10)

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

@@ -2,4 +2,6 @@
 
 public class RobotConstants {
   public static final int kTalonConfigTimeout = 10; // ms
+
+  public static final double kJoystickDeadband = 0.1;
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java

@@ -1,3 +1,33 @@
 package frc.robot.subsystems.elevator;
 
-public interface ElevatorIO {}
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import edu.wpi.first.units.measure.Angle;
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface ElevatorIO {
+
+  @AutoLog
+  public static class ElevatorIOInputs {
+    public double position = 0.0;
+    public double velocity = 0.0;
+    // something about height and IO layer?
+  }
+
+  public default void updateInputs(ElevatorIOInputs inputs) {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+
+  public default void setPosition(Angle position) {}
+
+  public default void setVelocityOpenLoop(double dutyCycleOut) {}
+
+  public default void setCurrentLimitConfig(CurrentLimitsConfigs config) {}
+
+  public default void zero() {}
+}
+
+/*
+ * To-do: resolve all the comments, implement zero, find out if follower actually means every LOC for one motor does the same
+ * thing to the other?, positionrequest never used in IOFX
+ */