Skip to content

Commit

Permalink
163 swerve tuning from dashboard (#164)
Browse files Browse the repository at this point in the history 
* add back the swerve tuning protocols

* remembered to format
  • Loading branch information
@mmilunicmobile
mmilunicmobile authored Feb 4, 2025
1 parent 4d0b2bc commit 8cf3dc1
Show file tree
Hide file tree
Showing 4 changed files with 185 additions and 25 deletions.
134 changes: 133 additions & 1 deletion elastic-layout.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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": []
}
}
Expand Down
46 changes: 30 additions & 16 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand Down
13 changes: 7 additions & 6 deletions src/main/java/frc/robot/commands/WheelRadiusCharacterization.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
17 changes: 15 additions & 2 deletions src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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));
}

/**
Expand All @@ -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;
Expand Down

0 comments on commit 8cf3dc1

Please sign in to comment.