Skip to content

Commit

Permalink
add phoenix swerve functionality, lots of placeholders
Browse files Browse the repository at this point in the history
  • Loading branch information
@JayK445
JayK445 committed Jan 10, 2024
1 parent 50daab4 commit 53c2567
Show file tree
Hide file tree
Showing 3 changed files with 122 additions and 14 deletions.
28 changes: 27 additions & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@

import static frc.util.MacUtil.IS_COMP_BOT;

import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
import edu.wpi.first.hal.HALUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
Expand Down Expand Up @@ -48,7 +51,7 @@ public static final class Config {
public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;

/** turn this off! only use on practice eboard testing. */
public static final boolean DISABLE_SWERVE_MODULE_INIT = false;
public static final boolean DISABLE_SWERVE_INIT = false;

/** def turn this off unless you are using it, generates in excess of 100k rows for a match. */
public static final boolean WRITE_APRILTAG_DATA = false;
Expand All @@ -60,6 +63,9 @@ public static final class Config {
}

public static final class Drive {
public static final int PIGEON_PORT = 0; // FIXME placeholder
public static final String SWERVE_CANBUS = "rio"; // placeholder

// max voltage delivered to drivebase
// supposedly useful to limit speed for testing
public static final double MAX_VOLTAGE = 12.0;
Expand Down Expand Up @@ -124,6 +130,26 @@ public static final class Dims {
*/

public static final class Modules {
public static final class Params {
// FIXME ALL PLACEHOLDERS
public static final double WHEEL_RADIUS = 0;
public static final double COUPLING_GEAR_RATIO = 0;
public static final double DRIVE_GEAR_RATIO = 0;
public static final double STEER_GEAR_RATIO = 0;
public static final Slot0Configs DRIVE_MOTOR_GAINS =
new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0);
public static final Slot0Configs STEER_MOTOR_GAINS =
new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0);
public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT =
ClosedLoopOutputType.Voltage;
public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT =
ClosedLoopOutputType.Voltage; // also paywall, but not important?
public static final SteerFeedbackType FEEDBACK_SOURCE =
SteerFeedbackType.RemoteCANcoder; // other types locked by paywall
public static final double SPEED_TWELVE_VOLTS = 0;
public static final double SLIP_CURRENT = 0;
}

public static final class Module1 { // historically front right
public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ public RobotContainer() {

SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT);
SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA);
SmartDashboard.putBoolean("don't init swerve modules", Config.DISABLE_SWERVE_MODULE_INIT);
SmartDashboard.putBoolean("don't init swerve modules", Config.DISABLE_SWERVE_INIT);

// Configure the button bindings
configureButtonBindings();
Expand Down
106 changes: 94 additions & 12 deletions src/main/java/frc/robot/subsystems/DrivebaseSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,11 @@

import static frc.robot.Constants.Drive.*;

import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.ApplyChassisSpeeds;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
Expand All @@ -17,7 +22,6 @@
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.SPI;
Expand Down Expand Up @@ -58,6 +62,8 @@ public class DrivebaseSubsystem extends SubsystemBase {
// Back right
new Translation2d(-Dims.TRACKWIDTH_METERS / 2.0, -Dims.WHEELBASE_METERS / 2.0));

private final SwerveDrivetrain swerveDrivetrain;

/** The SwerveDriveOdometry class allows us to estimate the robot's "pose" over time. */
private final SwerveDrivePoseEstimator swervePoseEstimator;

Expand All @@ -71,6 +77,8 @@ public class DrivebaseSubsystem extends SubsystemBase {
/** The current ChassisSpeeds goal for the drivetrain */
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); // defaults to zeros

private ApplyChassisSpeeds chassisSpeedRequest = new ApplyChassisSpeeds();

/** The modes of the drivebase subsystem */
public enum Modes {
DRIVE,
Expand All @@ -84,8 +92,6 @@ public enum Modes {
private Field2d field = new Field2d();

/** Contains each swerve module. Order: FR, FL, BL, BR. Or in Quadrants: I, II, III, IV */
// sds -> private final SwerveModule[] swerveModules;

private final PIDController rotController;

private double targetAngle = 0; // default target angle to zero
Expand All @@ -101,9 +107,73 @@ public enum Modes {
public DrivebaseSubsystem(VisionSubsystem visionSubsystem) {
this.visionSubsystem = visionSubsystem;

// FIXME determine if this should be kept
if (!Config.DISABLE_SWERVE_MODULE_INIT) {
if (!Config.DISABLE_SWERVE_INIT) {
final SwerveDrivetrainConstants swerveDrivetrainConstants =
new SwerveDrivetrainConstants().withPigeon2Id(PIGEON_PORT).withCANbusName(SWERVE_CANBUS);

final SwerveModuleConstantsFactory swerveModuleConstantsFactory =
new SwerveModuleConstantsFactory()
.withWheelRadius(Modules.Params.WHEEL_RADIUS)
.withCouplingGearRatio(Modules.Params.COUPLING_GEAR_RATIO)
.withDriveMotorGearRatio(Modules.Params.DRIVE_GEAR_RATIO)
.withSteerMotorGearRatio(Modules.Params.STEER_GEAR_RATIO)
.withDriveMotorGains(Modules.Params.DRIVE_MOTOR_GAINS)
.withSteerMotorGains(Modules.Params.STEER_MOTOR_GAINS)
.withDriveMotorClosedLoopOutput(Modules.Params.DRIVE_CLOSED_LOOP_OUTPUT)
.withSteerMotorClosedLoopOutput(Modules.Params.STEER_CLOSED_LOOP_OUTPUT)
.withFeedbackSource(Modules.Params.FEEDBACK_SOURCE)
.withSpeedAt12VoltsMps(Modules.Params.SPEED_TWELVE_VOLTS)
.withSlipCurrent(Modules.Params.SLIP_CURRENT);

// module wheel positions taken from kinematics object
final SwerveModuleConstants frontLeft =
swerveModuleConstantsFactory.createModuleConstants(
Modules.Module4.STEER_MOTOR,
Modules.Module4.DRIVE_MOTOR,
Modules.Module4.STEER_ENCODER,
Modules.Module4.STEER_OFFSET,
Dims.TRACKWIDTH_METERS / 2.0,
Dims.TRACKWIDTH_METERS / 2.0,
false);

// module wheel positions taken from kinematics object
final SwerveModuleConstants frontRight =
swerveModuleConstantsFactory.createModuleConstants(
Modules.Module3.STEER_MOTOR,
Modules.Module3.DRIVE_MOTOR,
Modules.Module3.STEER_ENCODER,
Modules.Module3.STEER_OFFSET,
Dims.TRACKWIDTH_METERS / 2.0,
-Dims.TRACKWIDTH_METERS / 2.0,
false);

// module wheel positions taken from kinematics object
final SwerveModuleConstants backLeft =
swerveModuleConstantsFactory.createModuleConstants(
Modules.Module1.STEER_MOTOR,
Modules.Module1.DRIVE_MOTOR,
Modules.Module1.STEER_ENCODER,
Modules.Module1.STEER_OFFSET,
-Dims.TRACKWIDTH_METERS / 2.0,
Dims.TRACKWIDTH_METERS / 2.0,
false);

// module wheel positions taken from kinematics object
final SwerveModuleConstants backRight =
swerveModuleConstantsFactory.createModuleConstants(
Modules.Module2.STEER_MOTOR,
Modules.Module2.DRIVE_MOTOR,
Modules.Module2.STEER_ENCODER,
Modules.Module2.STEER_OFFSET,
-Dims.TRACKWIDTH_METERS / 2.0,
-Dims.TRACKWIDTH_METERS / 2.0,
false);

swerveDrivetrain =
new SwerveDrivetrain(
swerveDrivetrainConstants, frontLeft, frontRight, backLeft, backRight);
} else {
swerveDrivetrain = null;
}

rotController = new PIDController(0.03, 0.001, 0.003);
Expand Down Expand Up @@ -152,9 +222,20 @@ public ChassisSpeeds getChassisSpeeds() {
return chassisSpeeds;
}

// FIXME placeholder return value
private SwerveModulePosition[] getSwerveModulePositions() {
return new SwerveModulePosition[4];
return Config.DISABLE_SWERVE_INIT
? new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
}
: new SwerveModulePosition[] {
swerveDrivetrain.getModule(0).getPosition(false),
swerveDrivetrain.getModule(1).getPosition(false),
swerveDrivetrain.getModule(2).getPosition(false),
swerveDrivetrain.getModule(3).getPosition(false),
};
}

private Rotation2d driverGyroOffset = Rotation2d.fromDegrees(0);
Expand Down Expand Up @@ -207,7 +288,7 @@ public void resetOdometryToPose(Pose2d pose) {
* @return The current angle of the robot, relative to boot position.
*/
public Rotation2d getConsistentGyroscopeRotation() {
return Rotation2d.fromDegrees(Util.normalizeDegrees(-navx.getAngle()));
return Rotation2d.fromDegrees(Util.normalizeDegrees(-swerveDrivetrain.getPigeon2().getAngle()));
}

/**
Expand All @@ -223,7 +304,7 @@ public Rotation2d getDriverGyroscopeRotation() {

// We have to invert the angle of the NavX so that rotating the robot counter-clockwise makes
// the angle increase.
double angle = Util.normalizeDegrees(-navx.getAngle());
double angle = Util.normalizeDegrees(-swerveDrivetrain.getPigeon2().getAngle());

// We need to subtract the offset here so that the robot drives forward based on auto
// positioning or manual reset
Expand Down Expand Up @@ -286,8 +367,8 @@ private void odometryPeriodic() {
}

private void drivePeriodic() {
SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VELOCITY_METERS_PER_SECOND);
chassisSpeedRequest.withSpeeds(chassisSpeeds);
swerveDrivetrain.setControl(chassisSpeedRequest);
}

// called in drive to angle mode
Expand Down Expand Up @@ -317,6 +398,7 @@ private void driveAnglePeriodic() {

@SuppressWarnings("java:S1121")
private void defensePeriodic() {

// No need to call odometry periodic
}

Expand Down Expand Up @@ -345,7 +427,7 @@ public RollPitch getRollPitch() {
* @param mode The mode to use (should use the current mode value)
*/
public void updateModules(Modes mode) {
if (Config.DISABLE_SWERVE_MODULE_INIT) return;
if (Config.DISABLE_SWERVE_INIT) return;
switch (mode) {
case DRIVE -> drivePeriodic();
case DRIVE_ANGLE -> driveAnglePeriodic();
Expand Down

0 comments on commit 53c2567

Please sign in to comment.