Skip to content

Commit

Permalink
sysid for drivetrain and better lookahead for wrist
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearBoxFox
GearBoxFox committed Mar 8, 2024
1 parent 609a1bb commit 861761d
Show file tree
Hide file tree
Showing 4 changed files with 80 additions and 124 deletions.
159 changes: 42 additions & 117 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants.DriveConstants;
import frc.robot.commands.DriveCommands;
import frc.robot.commands.FeedForwardCharacterization;
Expand Down Expand Up @@ -145,12 +146,18 @@ public RobotContainer() {
autoChooser = new SendableChooser<>();

// Set up feedforward characterization
// autoChooser.addOption(
// "Drive FF Characterization",
// new FeedForwardCharacterization(
// m_driveSubsystem,
// m_driveSubsystem::runCharacterizationVolts,
// m_driveSubsystem::getCharacterizationVelocity));
autoChooser.addOption(
"Drive Quasistatic Forward",
m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kForward));
autoChooser.addOption(
"Drive Quasistatic Backwards",
m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kReverse));
autoChooser.addOption(
"Drive Dynamic Forward",
m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kForward));
autoChooser.addOption(
"Drive Dynamic Backwards",
m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kReverse));

// Configure the button bindings
configureButtonBindings();
Expand All @@ -170,131 +177,49 @@ private void configureButtonBindings() {
Trigger spinUpTrigger = controller.leftTrigger().and(controller.rightTrigger().negate());
Trigger shootTrigger = controller.leftTrigger().and(controller.rightTrigger());

// intakeTrigger.whileTrue(new IntakeControlCommand(m_armSubsystem, m_shooter));
intakeTrigger.whileTrue(m_shooter.intakeCommand(0.95, 0.5, 0.1)
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE)));

// spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false)
// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)));
// shootTrigger.whileTrue(m_shooter.runShooterVelocity(true)
// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)));

// controller.x().whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
// controller.y().whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5),
// () -> m_shooter.setKickerPower(0.0),
// m_shooter));

double centerDistance = 1.34 - Units.inchesToMeters(3.0);

// m_driveSubsystem.setDefaultCommand(
// DriveCommands.joystickDrive(
// m_driveSubsystem,
// () -> -controller.getLeftY(),
// () -> -controller.getLeftX(),
// () -> -controller.getRightX()));
// controller
// .start()
// .onTrue(
// Commands.runOnce(
// () ->
// m_driveSubsystem.setPose(AllianceFlipUtil.apply(
// new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(240.25), 5.55),
// Rotation2d.fromDegrees(180.0)))),
// m_driveSubsystem)
// .ignoringDisable(true));
spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false)
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)));
shootTrigger.whileTrue(m_shooter.runShooterVelocity(true)
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)));

controller.x().whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
controller.y().whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5),
() -> m_shooter.setKickerPower(0.0),
m_shooter));

m_driveSubsystem.setDefaultCommand(
DriveCommands.joystickDrive(
m_driveSubsystem,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller
.start()
.onTrue(
Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(m_driveSubsystem.getVisionPose().getTranslation(),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
}

/**
* Use this method to configure any named commands needed for PathPlanner autos
*/
private void configureNamedCommands() {
// NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5)));
// NamedCommands.registerCommand("Run Shooter", Commands.run(m_shooter::runShooterVelocity));
NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5)));
}

private void configureDashboard() {
ShuffleboardTab commandTab = Shuffleboard.getTab("Commads");

// commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false));
// commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true));
/*
commandTab.add("Center Robot Pose", Commands.runOnce(
() ->
m_driveSubsystem.setPose(
new Pose2d(
FieldConstants.FIELD_LENGTH / 2.0,
FieldConstants.FIELD_WIDTH / 2.0,
new Rotation2d())),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("CenterToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(1.34, 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
double centerDistance = 1.34 - Units.inchesToMeters(3.0);
commandTab.add("2InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(2.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("6InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(6.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("12InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(12.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("24InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(24.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("48InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(48.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("96InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(96.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
commandTab.add("192InchesToSpeaker", Commands.runOnce(
() ->
m_driveSubsystem.setPose(AllianceFlipUtil.apply(
new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(192.0), 5.55),
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));
*/
commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false));
commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true));
}

/**
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,10 +93,10 @@ public void periodic() {

if (m_desiredState != ArmState.DISABLED) {
// check to see if the wrist is currently too close to the rest of the arm
m_io.setWristAngle(m_desiredWristPoseDegs, m_wristVelocityMult);
double predictedUnderGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue()
- (m_desiredArmPoseDegs + m_desiredWristPoseDegs), 0, 180);

double underGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue()
- (m_inputs.armPositionDegs + m_inputs.wristPositionDegs), 0, 180);
m_io.setWristAngle(m_desiredWristPoseDegs + predictedUnderGap, m_wristVelocityMult);

// set the arms angle
m_io.setArmAngle(m_desiredArmPoseDegs, m_armVelocityMult);
Expand All @@ -110,8 +110,8 @@ public void periodic() {
public void handleState() {
switch(m_desiredState) {
case STOW -> {
if (m_inputs.armPositionDegs > 60 && m_currentState == ArmState.AMP) {
m_wristVelocityMult = 0.0;
if (m_inputs.armPositionDegs > 65 && m_currentState == ArmState.AMP) {
m_wristVelocityMult = 0.25;
m_armVelocityMult = 1.0;
} else {
m_currentState = ArmState.STOW;
Expand Down
31 changes: 31 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

package frc.robot.subsystems.drive;

import com.ctre.phoenix6.SignalLogger;
import com.gos.lib.properties.pid.PidProperty;
import com.gos.lib.properties.pid.WpiPidPropertyBuilder;
import com.pathplanner.lib.auto.AutoBuilder;
Expand All @@ -27,12 +28,16 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.*;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants.DriveConstants;
import frc.robot.subsystems.drive.module.Module;
import frc.robot.subsystems.drive.module.ModuleIO;
Expand All @@ -46,6 +51,8 @@
import lib.logger.DataLogUtil.DataLogTable;
import lib.utils.PoseEstimator;

import static edu.wpi.first.units.Units.Volts;

public class DriveSubsystem extends SubsystemBase {

public static final Lock odometryLock = new ReentrantLock();
Expand Down Expand Up @@ -76,6 +83,8 @@ public class DriveSubsystem extends SubsystemBase {

private final DataLogTable m_logTable = DataLogUtil.getTable("Swerve");

private final SysIdRoutine m_sysId;

private final SwerveModuleState[] m_measureStates = new SwerveModuleState[] {
new SwerveModuleState(),
new SwerveModuleState(),
Expand Down Expand Up @@ -165,6 +174,20 @@ public DriveSubsystem(
// turn on logging
setupLogging();
SmartDashboard.putData("Field",m_field);

m_sysId = new SysIdRoutine(
new SysIdRoutine.Config(
null,
null,
null,
(SysIdRoutineLog.State state) -> SignalLogger.writeString("Drive/SysidState", state.toString())
),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> runCharacterizationVolts(volts.in(Volts)),
null,
this
)
);
}

@Override
Expand Down Expand Up @@ -365,6 +388,14 @@ public static Translation2d[] getModuleTranslations() {
};
}

public Command runSysidQuasistatic(SysIdRoutine.Direction direction) {
return m_sysId.quasistatic(direction);
}

public Command runSysidDynamic(SysIdRoutine.Direction direction) {
return m_sysId.dynamic(direction);
}

public void setupLogging() {
for (int i = 0; i < modules.length; i++) {
Module module = modules[i];
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ private PhoenixOdometryThread() {

@Override
public void start() {
if (timestampQueues.size() > 0) {
if (!timestampQueues.isEmpty()) {
super.start();
}
}
Expand Down Expand Up @@ -138,4 +138,4 @@ public void run() {
}
}
}
}
}

0 comments on commit 861761d

Please sign in to comment.