diff --git a/elastic-layout.json b/elastic-layout.json index 2abe1a42..6b22f6e4 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -212,7 +212,139 @@ { "name": "Debug", "grid_layout": { - "layouts": [], + "layouts": [ + { + "title": "Swerve Characterization", + "x": 896.0, + "y": 0.0, + "width": 384.0, + "height": 512.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Swerve SysId Dynamic Reverse", + "x": 512.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Dynamic Reverse", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Swerve SysId Dynamic Forward", + "x": 512.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Dynamic Forward", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Swerve SysId Quasistatic Forward", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Quasistatic Forward", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Swerve SysId Quasistatic Reverse", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Quasistatic Reverse", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Swerve SysId Rotation Mode", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Rotation Mode", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Swerve SysId Steer Mode", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Steer Mode", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Swerve SysId Translation Mode", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Swerve SysId Translation Mode", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Wheel Radius Characterization Command", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Wheel Radius Characterization Command", + "period": 0.06, + "show_type": true + } + }, + { + "title": "WRC Speed", + "x": 768.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "WheelRadiusCharacterization/SpeedRadsPerSec", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + ], "containers": [] } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1db9b789..cdcba59e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,14 +9,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.controller.LogitechController; import frc.lib.controller.ThrustmasterJoystick; import frc.robot.commands.AlignAndDriveToReef; import frc.robot.commands.AlignToPiece; import frc.robot.commands.AlignToReef; +import frc.robot.commands.WheelRadiusCharacterization; import frc.robot.constants.GlobalConstants; import frc.robot.constants.GlobalConstants.ControllerConstants; import frc.robot.constants.TunerConstants; @@ -216,22 +219,33 @@ private void configureBindings() { // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - // operatorController - // .getBack() - // .and(operatorController.getY()) - // .whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - // operatorController - // .getBack() - // .and(operatorController.getX()) - // .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - // operatorController - // .getStart() - // .and(operatorController.getY()) - // .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - // operatorController - // .getStart() - // .and(operatorController.getX()) - // .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + SmartDashboard.putData( + drivetrain + .sysIdDynamic(Direction.kForward) + .withName("Swerve SysId Dynamic Forward")); + SmartDashboard.putData( + drivetrain + .sysIdDynamic(Direction.kReverse) + .withName("Swerve SysId Dynamic Reverse")); + SmartDashboard.putData( + drivetrain + .sysIdQuasistatic(Direction.kForward) + .withName("Swerve SysId Quasistatic Forward")); + SmartDashboard.putData( + drivetrain + .sysIdQuasistatic(Direction.kReverse) + .withName("Swerve SysId Quasistatic Reverse")); + + SmartDashboard.putData( + drivetrain.sysIdRotationMode().withName("Swerve SysId Rotation Mode")); + SmartDashboard.putData(drivetrain.sysIdSteerMode().withName("Swerve SysId Steer Mode")); + SmartDashboard.putData( + drivetrain.sysIdTranslationMode().withName("Swerve SysId Translation Mode")); + + SmartDashboard.putData( + new WheelRadiusCharacterization( + WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain) + .withName("Wheel Radius Characterization Command")); // operatorController // operatorController.getA().onTrue(stateManager.moveToPosition(Position.L4)); diff --git a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java index 5c8db7b7..55b91e4b 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java @@ -62,12 +62,13 @@ public void initialize() { } public void execute() { - drive.driveRobotRelative( - new ChassisSpeeds( - 0, - 0, - omegaLimiter.calculate( - omegaDirection.value * characterizationSpeed.get()))); + drive.setControl( + drive.driveRobotRelative( + new ChassisSpeeds( + 0, + 0, + omegaLimiter.calculate( + omegaDirection.value * characterizationSpeed.get())))); // Get yaw and wheel positions diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 796705c6..8ae682f5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; @@ -489,7 +490,7 @@ public void addVisionMeasurement( * @return Command to run */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); + return defer(() -> m_sysIdRoutineToApply.quasistatic(direction)); } /** @@ -500,7 +501,19 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { * @return Command to run */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); + return defer(() -> m_sysIdRoutineToApply.dynamic(direction)); + } + + public Command sysIdTranslationMode() { + return Commands.runOnce(() -> m_sysIdRoutineToApply = m_sysIdRoutineTranslation); + } + + public Command sysIdSteerMode() { + return Commands.runOnce(() -> m_sysIdRoutineToApply = m_sysIdRoutineSteer); + } + + public Command sysIdRotationMode() { + return Commands.runOnce(() -> m_sysIdRoutineToApply = m_sysIdRoutineRotation); } private double lastSpeed = 0;