diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07a6ddc..8f0addb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,25 +10,53 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.constants.OIConstants; import frc.robot.constants.ElevatorConstants.ElevatorPositions; +import frc.robot.constants.OIConstants.RobotOperatingMode; import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.ElevatorSysID; public class RobotContainer { private Elevator elevator; private CommandXboxController driver; + private RobotOperatingMode operatingMode; + public RobotContainer() { - elevator = new Elevator(); + operatingMode = RobotOperatingMode.kNormal; + + switch(operatingMode) { + case kNormal: + case kElevatorSetpointDebug: + default: + elevator = new Elevator(); + break; + case kElevatorSysIDCharacterization: + elevator = new ElevatorSysID(); + break; + } driver = new CommandXboxController(OIConstants.kDriverUSB); - configureBindings(); + switch(operatingMode) { + case kNormal: + default: + configureNormalBindings(); + break; + case kElevatorSetpointDebug: + configureElevatorSetpointBindings(); + break; + case kElevatorSysIDCharacterization: + configureElevatorSysIDBindings(); + break; + } + configureShuffleboard(); } - private void configureBindings() { + private void configureNormalBindings() { elevator.setDefaultCommand( elevator.assistedManualControl(driver::getRightY) ); @@ -39,6 +67,30 @@ public class RobotContainer { driver.y().onTrue(elevator.goToPosition(ElevatorPositions.kTop, 3)); } + private void configureElevatorSetpointBindings() { + elevator.setDefaultCommand( + elevator.debugSetpoints() + ); + } + + private void configureElevatorSysIDBindings() { + driver.a().and(driver.rightBumper()).whileTrue( + ((ElevatorSysID)elevator).sysIdQuasistatic(Direction.kForward) + ); + + driver.b().and(driver.rightBumper()).whileTrue( + ((ElevatorSysID)elevator).sysIdQuasistatic(Direction.kReverse) + ); + + driver.a().and(driver.leftBumper()).whileTrue( + ((ElevatorSysID)elevator).sysIdDynamic(Direction.kForward) + ); + + driver.b().and(driver.leftBumper()).whileTrue( + ((ElevatorSysID)elevator).sysIdDynamic(Direction.kReverse) + ); + } + private void configureShuffleboard() { ShuffleboardTab tab = Shuffleboard.getTab("MyTab"); diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index 54d25ac..921b545 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -1,5 +1,11 @@ package frc.robot.constants; public class OIConstants { + public enum RobotOperatingMode { + kNormal, + kElevatorSetpointDebug, + kElevatorSysIDCharacterization + } + public static final int kDriverUSB = 0; } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 87d1be2..86dc554 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -16,10 +16,10 @@ import frc.robot.constants.ElevatorConstants; import frc.robot.constants.ElevatorConstants.ElevatorPositions; public class Elevator extends SubsystemBase { - private WPI_VictorSPX motor1; - private WPI_VictorSPX motor2; + protected WPI_VictorSPX motor1; + protected WPI_VictorSPX motor2; - private Encoder encoder; + protected Encoder encoder; private PIDController positionalController; private PIDController velocityController; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/ElevatorSysID.java new file mode 100644 index 0000000..0e52bc8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSysID.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class ElevatorSysID extends Elevator { + private MutableMeasure appliedVoltage; + private MutableMeasure distance; + private MutableMeasure> velocity; + + private SysIdRoutine sysIDRoutine; + + public ElevatorSysID() { + super(); + + appliedVoltage = MutableMeasure.mutable(Units.Volts.of(0)); + distance = MutableMeasure.mutable(Units.Meters.of(0)); + velocity = MutableMeasure.mutable(Units.MetersPerSecond.of(0)); + + sysIDRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + motor1.setVoltage(volts.in(Units.Volts)); + }, + (log) -> { + log.motor("elevator") + .voltage( + appliedVoltage.mut_replace( + motor1.get() * RobotController.getBatteryVoltage(), + Units.Volts + ) + ) + .linearPosition( + distance.mut_replace(encoder.getDistance(), Units.Meters) + ) + .linearVelocity( + velocity.mut_replace(encoder.getRate(), Units.MetersPerSecond) + ); + }, + this + ) + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIDRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIDRoutine.dynamic(direction); + } +}