Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Swerve Code works #10

Merged
merged 6 commits into from
Jan 7, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .vscode/settings.json
Original file line number Diff line number Diff line change
Expand Up @@ -56,5 +56,6 @@
"edu.wpi.first.math.proto.*",
"edu.wpi.first.math.**.proto.*",
"edu.wpi.first.math.**.struct.*",
]
],
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
}
9 changes: 8 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,12 @@

import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.ResetGyro;
import frc.robot.constants.SWERVE;
import frc.robot.constants.USB;
import frc.robot.generated.TunerConstants;
Expand Down Expand Up @@ -50,12 +52,17 @@ public class RobotContainer {
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
m_swerveDrive.registerTelemetry(m_telemetry::telemeterize);

initSmartDashboard();
// Configure the trigger bindings
initializeSubSystems();
configureBindings();
}

private void initSmartDashboard() {

SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive));
}

private void initializeSubSystems() {
m_swerveDrive.setDefaultCommand(
// Drivetrain will execute this command periodically
Expand Down
46 changes: 46 additions & 0 deletions src/main/java/frc/robot/commands/ResetGyro.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Controls;

/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class ResetGyro extends Command {
/** Creates a new ResetGyro. */
private final CommandSwerveDrivetrain m_swerveDrive;

public ResetGyro(CommandSwerveDrivetrain swervedrive) {
m_swerveDrive = swervedrive;
// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_swerveDrive);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
if (Controls.isRedAlliance()) {
m_swerveDrive.resetGyro(0);
} else {
m_swerveDrive.resetGyro(180);
}
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return true;
}
}
101 changes: 45 additions & 56 deletions src/main/java/frc/robot/generated/TunerConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,9 @@
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.*;
import frc.robot.constants.CAN;
import frc.robot.constants.SWERVE;
import frc.robot.subsystems.CommandSwerveDrivetrain;

// Generated by the Tuner X Swerve Project Generator
// Generated by the Tuner X Swerve Project Generator for Software Bot/AlphaBot as of 1/6/2025
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
public class TunerConstants {
// Both sets of gains need to be tuned to your individual robot.
Expand All @@ -25,23 +24,17 @@ public class TunerConstants {
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs steerGains =
new Slot0Configs()
.withKP(SWERVE.kTurnKP)
.withKI(SWERVE.kTurnKI)
.withKD(SWERVE.kTurnKD)
.withKS(SWERVE.kTurnKS)
.withKV(SWERVE.kTurnKV)
.withKA(SWERVE.kTurnKA)
.withKP(100)
.withKI(0)
.withKD(0.5)
.withKS(0.1)
.withKV(1.59)
.withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains =
new Slot0Configs()
.withKP(SWERVE.kDriveKP)
.withKI(SWERVE.kDriveKI)
.withKD(SWERVE.kDriveKD)
.withKS(SWERVE.kDriveKS)
.withKV(SWERVE.kDriveKV)
.withKA(SWERVE.kDriveKA);
new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);

// The closed-loop output type to use for the steer motors;
// This affects the PID/FF gains for the steer motors
Expand All @@ -59,11 +52,11 @@ public class TunerConstants {

// The remote sensor feedback type to use for the steer motors;
// When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder;

// The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot
private static final Current kSlipCurrent = Amps.of(SWERVE.kSlipCurrent);
private static final Current kSlipCurrent = Amps.of(120.0);

// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
Expand All @@ -75,38 +68,39 @@ public class TunerConstants {
// Swerve azimuth does not require much torque output, so we can set a relatively
// low
// stator current limit to help avoid brownouts without impacting performance.
.withStatorCurrentLimit(Amps.of(SWERVE.kTurnSatorCurrentLimit))
.withStatorCurrentLimit(Amps.of(60))
.withStatorCurrentLimitEnable(true));
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null;

// CAN bus that the devices are located on;
// All swerve devices must share the same CAN bus
public static final CANBus kCANBus = new CANBus(CAN.driveBaseCanbus, "./logs/example.hoot");
public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");

// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
public static final LinearVelocity kSpeedAt12Volts =
MetersPerSecond.of(SWERVE.kMaxSpeedMetersPerSecond);
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96);

// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
private static final double kCoupleRatio = SWERVE.kCoupleRatio;
private static final double kCoupleRatio = 3.125;

private static final double kDriveGearRatio = 5.357142857142857;
private static final double kSteerGearRatio = 12.8;
private static final Distance kWheelRadius = Inches.of(2);

private static final double kDriveGearRatio = SWERVE.kDriveMotorGearRatio;
private static final double kSteerGearRatio = SWERVE.kTurnMotorGearRatio;
private static final Distance kWheelRadius = Inches.of(SWERVE.kWheelRadiusInches);
private static final boolean kInvertLeftSide = true;
private static final boolean kInvertRightSide = false;

private static final int kPigeonId = CAN.pigeon;
private static final int kPigeonId = 9;

// These are only used for simulation
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(SWERVE.kTurnInertia);
private static final MomentOfInertia kDriveInertia =
KilogramSquareMeters.of(SWERVE.kDriveInertia);
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
// Simulated voltage necessary to overcome friction
private static final Voltage kSteerFrictionVoltage = Volts.of(SWERVE.kTurnFrictionVoltage);
private static final Voltage kDriveFrictionVoltage = Volts.of(SWERVE.kDriveFrictionVoltage);
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);

public static final SwerveDrivetrainConstants DrivetrainConstants =
new SwerveDrivetrainConstants()
Expand Down Expand Up @@ -144,50 +138,45 @@ public class TunerConstants {
private static final int kFrontLeftDriveMotorId = CAN.frontLeftDriveMotor;
private static final int kFrontLeftSteerMotorId = CAN.frontLeftTurnMotor;
private static final int kFrontLeftEncoderId = CAN.frontLeftCanCoder;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(SWERVE.kFrontLeftEncoderOffset);
private static final boolean kFrontLeftSteerMotorInverted = SWERVE.kTurnInversions[0];
private static final boolean kFrontLeftDriveMotorInverted = SWERVE.kDriveInversions[0];
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.263916015625);
private static final boolean kFrontLeftSteerMotorInverted = false;
private static final boolean kFrontLeftEncoderInverted = false;

private static final Distance kFrontLeftXPos = Inches.of(SWERVE.kFrontLeftPosition.getX());
private static final Distance kFrontLeftYPos = Inches.of(SWERVE.kFrontLeftPosition.getY());
private static final Distance kFrontLeftXPos = Inches.of(13.75);
private static final Distance kFrontLeftYPos = Inches.of(13.75);

// Front Right
private static final int kFrontRightDriveMotorId = CAN.frontRightDriveMotor;
private static final int kFrontRightSteerMotorId = CAN.frontRightTurnMotor;
private static final int kFrontRightEncoderId = CAN.frontRightCanCoder;
private static final Angle kFrontRightEncoderOffset =
Rotations.of(SWERVE.kFrontRightEncoderOffset);
private static final boolean kFrontRightSteerMotorInverted = SWERVE.kTurnInversions[1];
private static final boolean kFrontRightDriveMotorInverted = SWERVE.kDriveInversions[1];
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.485595703125);
private static final boolean kFrontRightSteerMotorInverted = false;
private static final boolean kFrontRightEncoderInverted = false;

private static final Distance kFrontRightXPos = Inches.of(SWERVE.kFrontRightPosition.getX());
private static final Distance kFrontRightYPos = Inches.of(SWERVE.kFrontRightPosition.getY());
private static final Distance kFrontRightXPos = Inches.of(13.75);
private static final Distance kFrontRightYPos = Inches.of(-13.75);

// Back Left
private static final int kBackLeftDriveMotorId = CAN.backLeftDriveMotor;
private static final int kBackLeftSteerMotorId = CAN.backLeftTurnMotor;
private static final int kBackLeftEncoderId = CAN.backLeftCanCoder;
private static final Angle kBackLeftEncoderOffset = Rotations.of(SWERVE.kBackLeftEncoderOffset);
private static final boolean kBackLeftSteerMotorInverted = SWERVE.kTurnInversions[2];
private static final boolean kBackLeftDriveMotorInverted = SWERVE.kDriveInversions[2];
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0556640625);
private static final boolean kBackLeftSteerMotorInverted = false;
private static final boolean kBackLeftEncoderInverted = false;

private static final Distance kBackLeftXPos = Inches.of(SWERVE.kBackLeftPosition.getX());
private static final Distance kBackLeftYPos = Inches.of(SWERVE.kBackLeftPosition.getY());
private static final Distance kBackLeftXPos = Inches.of(-13.75);
private static final Distance kBackLeftYPos = Inches.of(13.75);

// Back Right
private static final int kBackRightDriveMotorId = CAN.backRightDriveMotor;
private static final int kBackRightSteerMotorId = CAN.backRightTurnMotor;
private static final int kBackRightEncoderId = CAN.backRightCanCoder;
private static final Angle kBackRightEncoderOffset = Rotations.of(SWERVE.kBackRightEncoderOffset);
private static final boolean kBackRightSteerMotorInverted = SWERVE.kTurnInversions[3];
private static final boolean kBackRightDriveMotorInverted = SWERVE.kDriveInversions[3];
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.201171875);
private static final boolean kBackRightSteerMotorInverted = false;
private static final boolean kBackRightEncoderInverted = false;

private static final Distance kBackRightXPos = Inches.of(SWERVE.kBackRightPosition.getX());
private static final Distance kBackRightYPos = Inches.of(SWERVE.kBackRightPosition.getY());
private static final Distance kBackRightXPos = Inches.of(-13.75);
private static final Distance kBackRightYPos = Inches.of(-13.75);

public static final SwerveModuleConstants<
TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
Expand All @@ -199,7 +188,7 @@ public class TunerConstants {
kFrontLeftEncoderOffset,
kFrontLeftXPos,
kFrontLeftYPos,
kFrontLeftDriveMotorInverted,
kInvertLeftSide,
kFrontLeftSteerMotorInverted,
kFrontLeftEncoderInverted);
public static final SwerveModuleConstants<
Expand All @@ -212,7 +201,7 @@ public class TunerConstants {
kFrontRightEncoderOffset,
kFrontRightXPos,
kFrontRightYPos,
kFrontRightDriveMotorInverted,
kInvertRightSide,
kFrontRightSteerMotorInverted,
kFrontRightEncoderInverted);
public static final SwerveModuleConstants<
Expand All @@ -225,7 +214,7 @@ public class TunerConstants {
kBackLeftEncoderOffset,
kBackLeftXPos,
kBackLeftYPos,
kBackLeftDriveMotorInverted,
kInvertLeftSide,
kBackLeftSteerMotorInverted,
kBackLeftEncoderInverted);
public static final SwerveModuleConstants<
Expand All @@ -238,7 +227,7 @@ public class TunerConstants {
kBackRightEncoderOffset,
kBackRightXPos,
kBackRightYPos,
kBackRightDriveMotorInverted,
kInvertRightSide,
kBackRightSteerMotorInverted,
kBackRightEncoderInverted);

Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ public CommandSwerveDrivetrain(
if (Utils.isSimulation()) {
startSimThread();
}
configureAutoBuilder();
// configureAutoBuilder();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

will need to add this back in later to run autos

}

/**
Expand All @@ -150,7 +150,7 @@ public CommandSwerveDrivetrain(
if (Utils.isSimulation()) {
startSimThread();
}
configureAutoBuilder();
// configureAutoBuilder();
}

/**
Expand Down Expand Up @@ -183,7 +183,7 @@ public CommandSwerveDrivetrain(
if (Utils.isSimulation()) {
startSimThread();
}
configureAutoBuilder();
// configureAutoBuilder();
}

// TODO: Re-implement
Expand Down Expand Up @@ -252,6 +252,10 @@ public CommandSwerveDrivetrain(
// });
// }

public void resetGyro(double angle) {
getPigeon2().setYaw(angle);
}

private void configureAutoBuilder() {
try {
var config = RobotConfig.fromGUISettings();
Expand Down
33 changes: 33 additions & 0 deletions src/main/java/frc/robot/subsystems/Controls.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Controls extends SubsystemBase {
private CommandSwerveDrivetrain m_swervedrive;
private static DriverStation.Alliance m_allianceColor = DriverStation.Alliance.Red;

/** Creates a new Controls. */
public Controls() {}

public static DriverStation.Alliance getAllianceColor() {
return m_allianceColor;
}

public static boolean isRedAlliance() {
return (m_allianceColor == DriverStation.Alliance.Red);
}

public static boolean isBlueAlliance() {
return (m_allianceColor == DriverStation.Alliance.Blue);
}

@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
Loading