drivetrain + autoBuilder/chooser + tournerAZero/180
This commit is contained in:
Antoine PerreaultE
@@ -10,10 +10,15 @@ import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
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.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
@@ -44,74 +49,31 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
|
||||
/* Keep track if we've ever applied the operator perspective before or not */
|
||||
private boolean m_hasAppliedOperatorPerspective = false;
|
||||
|
||||
/* Swerve requests to apply during SysId characterization */
|
||||
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
|
||||
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
|
||||
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
|
||||
|
||||
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
|
||||
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
|
||||
new SysIdRoutine.Config(
|
||||
null, // Use default ramp rate (1 V/s)
|
||||
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
|
||||
null, // Use default timeout (10 s)
|
||||
// Log state with SignalLogger class
|
||||
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
|
||||
),
|
||||
new SysIdRoutine.Mechanism(
|
||||
output -> setControl(m_translationCharacterization.withVolts(output)),
|
||||
null,
|
||||
this
|
||||
)
|
||||
);
|
||||
|
||||
/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
|
||||
private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
|
||||
new SysIdRoutine.Config(
|
||||
null, // Use default ramp rate (1 V/s)
|
||||
Volts.of(7), // Use dynamic voltage of 7 V
|
||||
null, // Use default timeout (10 s)
|
||||
// Log state with SignalLogger class
|
||||
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
|
||||
),
|
||||
new SysIdRoutine.Mechanism(
|
||||
volts -> setControl(m_steerCharacterization.withVolts(volts)),
|
||||
null,
|
||||
this
|
||||
)
|
||||
);
|
||||
|
||||
/*
|
||||
* SysId routine for characterizing rotation.
|
||||
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
|
||||
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
|
||||
*/
|
||||
private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
|
||||
new SysIdRoutine.Config(
|
||||
/* This is in radians per second², but SysId only supports "volts per second" */
|
||||
Volts.of(Math.PI / 6).per(Second),
|
||||
/* This is in radians per second, but SysId only supports "volts" */
|
||||
Volts.of(Math.PI),
|
||||
null, // Use default timeout (10 s)
|
||||
// Log state with SignalLogger class
|
||||
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
|
||||
),
|
||||
new SysIdRoutine.Mechanism(
|
||||
output -> {
|
||||
/* output is actually radians per second, but SysId only supports "volts" */
|
||||
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
|
||||
/* also log the requested output for SysId */
|
||||
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
|
||||
},
|
||||
null,
|
||||
this
|
||||
)
|
||||
);
|
||||
|
||||
/* The SysId routine to test */
|
||||
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
|
||||
|
||||
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
|
||||
private void configureAutoBuilder() {
|
||||
try {
|
||||
RobotConfig config = RobotConfig.fromGUISettings();
|
||||
AutoBuilder.configure(
|
||||
() -> getState().Pose,
|
||||
this::resetPose,
|
||||
() -> getState().Speeds,
|
||||
(speeds, feedforwards) -> setControl(
|
||||
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
|
||||
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
|
||||
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
|
||||
),
|
||||
new PPHolonomicDriveController(
|
||||
new PIDConstants(10, 0, 0),
|
||||
new PIDConstants(7, 0, 0)
|
||||
),
|
||||
config,
|
||||
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
||||
this
|
||||
);
|
||||
} catch (Exception ex) {
|
||||
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
||||
}
|
||||
}
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
* <p>
|
||||
@@ -130,6 +92,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
if (Utils.isSimulation()) {
|
||||
startSimThread();
|
||||
}
|
||||
configureAutoBuilder();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -154,6 +117,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
if (Utils.isSimulation()) {
|
||||
startSimThread();
|
||||
}
|
||||
configureAutoBuilder();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -186,6 +150,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
if (Utils.isSimulation()) {
|
||||
startSimThread();
|
||||
}
|
||||
configureAutoBuilder();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -198,28 +163,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
return run(() -> this.setControl(request.get()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the SysId Quasistatic test in the given direction for the routine
|
||||
* specified by {@link #m_sysIdRoutineToApply}.
|
||||
*
|
||||
* @param direction Direction of the SysId Quasistatic test
|
||||
* @return Command to run
|
||||
*/
|
||||
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
|
||||
return m_sysIdRoutineToApply.quasistatic(direction);
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the SysId Dynamic test in the given direction for the routine
|
||||
* specified by {@link #m_sysIdRoutineToApply}.
|
||||
*
|
||||
* @param direction Direction of the SysId Dynamic test
|
||||
* @return Command to run
|
||||
*/
|
||||
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
|
||||
return m_sysIdRoutineToApply.dynamic(direction);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
/*
|
||||
|
||||
Reference in New Issue
Block a user