diff --git a/build.gradle b/build.gradle index 1114ba98..48935bc8 100644 --- a/build.gradle +++ b/build.gradle @@ -111,7 +111,12 @@ sonar { } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui() +// +// The sim GUI is *disabled* by default to support running +// AdvantageKit log replay from the command line. Set the +// value to "true" to enable the sim GUI by default (this +// is the standard WPILib behavior). +wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') diff --git a/ctre_sim/CANCoder vers. H - 011 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 011 - 0 - ext.dat new file mode 100644 index 00000000..845e4a3a Binary files /dev/null and b/ctre_sim/CANCoder vers. H - 011 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 02 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 02 - 0 - ext.dat new file mode 100644 index 00000000..400651b3 Binary files /dev/null and b/ctre_sim/CANCoder vers. H - 02 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 05 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 05 - 0 - ext.dat new file mode 100644 index 00000000..b309d556 Binary files /dev/null and b/ctre_sim/CANCoder vers. H - 05 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 08 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 08 - 0 - ext.dat new file mode 100644 index 00000000..2bbef49a Binary files /dev/null and b/ctre_sim/CANCoder vers. H - 08 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat new file mode 100644 index 00000000..12476bd6 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat new file mode 100644 index 00000000..9d792a2c Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat new file mode 100644 index 00000000..3ea2106a Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat new file mode 100644 index 00000000..674afba4 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat new file mode 100644 index 00000000..57fed237 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat new file mode 100644 index 00000000..c91e9d7f Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat new file mode 100644 index 00000000..fe3f4272 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat new file mode 100644 index 00000000..71119452 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat differ diff --git a/logs/Log_24-01-19_18-35-38.wpilog b/logs/Log_24-01-19_18-35-38.wpilog new file mode 100644 index 00000000..226c1f3a Binary files /dev/null and b/logs/Log_24-01-19_18-35-38.wpilog differ diff --git a/networktables.json b/networktables.json index fe51488c..6cc7f9ce 100644 --- a/networktables.json +++ b/networktables.json @@ -1 +1,130 @@ -[] +[ + { + "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + } +] diff --git a/networktables.json.bck b/networktables.json.bck new file mode 100644 index 00000000..4dc34172 --- /dev/null +++ b/networktables.json.bck @@ -0,0 +1,98 @@ +[ + { + "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + } +] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d81ef09..d1b2241a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,7 +24,7 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 617eac14..11eb7266 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; @@ -34,7 +33,6 @@ import frc.robot.subsystems.shooter.ShooterIOPrototype; import frc.robot.subsystems.shooter.ShooterSubsystem; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -52,52 +50,43 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - switch (Constants.currentMode) { - case REAL -> { - // Real robot, instantiate hardware IO implementations - m_driveSubsystem = new DriveSubsystem( - new GyroIOPigeon1(12, true), - new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS), - new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS), - new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), - new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS)); - m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); - } - case SIM -> { - // Sim robot, instantiate physics sim IO implementations - m_driveSubsystem = - new DriveSubsystem( - new GyroIO() { - }, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); - } - default -> { - // Replayed robot, disable IO implementations - m_driveSubsystem = - new DriveSubsystem( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }); - m_shooter = new ShooterSubsystem(new ShooterIO() { - }); - } + switch (Constants.currentMode) { + case REAL -> { + // Real robot, instantiate hardware IO implementations + m_driveSubsystem = new DriveSubsystem( + new GyroIOPigeon1(12, true), + new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS), + new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS), + new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), + new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS)); + m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); } + case SIM -> { + // Sim robot, instantiate physics sim IO implementations + m_driveSubsystem = + new DriveSubsystem( + new GyroIO() {}, + new ModuleIOSim(Constants.DriveConstants.FL_MOD_CONSTANTS), + new ModuleIOSim(Constants.DriveConstants.FR_MOD_CONSTANTS), + new ModuleIOSim(Constants.DriveConstants.BL_MOD_CONSTANTS), + new ModuleIOSim(Constants.DriveConstants.BR_MOD_CONSTANTS)); + m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); + } + default -> { + // Replayed robot, disable IO implementations + m_driveSubsystem = + new DriveSubsystem( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + m_shooter = new ShooterSubsystem(new ShooterIO() {}); + } + } // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -106,7 +95,9 @@ public RobotContainer() { autoChooser.addOption( "Drive FF Characterization", new FeedForwardCharacterization( - m_driveSubsystem, m_driveSubsystem::runCharacterizationVolts, m_driveSubsystem::getCharacterizationVelocity)); + m_driveSubsystem, + m_driveSubsystem::runCharacterizationVolts, + m_driveSubsystem::getCharacterizationVelocity)); // Configure the button bindings configureButtonBindings(); @@ -146,6 +137,6 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand();//autoChooser.get(); + return autoChooser.get(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 3f4a466a..ca759931 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -45,7 +45,7 @@ public class Module { public Module(ModuleIO io) { this.m_io = io; this.m_moduleConstants = m_io.getModuleConstants(); - this.m_index = m_moduleConstants.MODULE_INDEX; + this.m_index = m_moduleConstants.MODULE_INDEX(); // delay to initialize all hardware Timer.delay(0.5); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java index 5d37d05e..9d766357 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java @@ -3,135 +3,57 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -public class ModuleConstants { - - public enum GearRatios { - L1((14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0)), - L2((14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0)), - L3((14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0)) - ; - - GearRatios(double ratio) { - this.ratio = ratio; - } - - public final double ratio; - } +public record ModuleConstants( // Physical constants - public final double DRIVE_GEAR_RATIO; - public final double TURNING_GEAR_RATIO; - - public final double WHEEL_RADIUS_METERS; - public final double WHEEL_CURCUMFERENCE_METERS; - - public final boolean TURN_MOTOR_INVERTED; - public final boolean DRIVE_MOTOR_INVERTED; - public final boolean ENCODER_INVERTED; - - public final Rotation2d ENCODER_OFFSET; - - public final int MODULE_INDEX; - + double DRIVE_GEAR_RATIO, double TURNING_GEAR_RATIO, + double WHEEL_RADIUS_METERS, double WHEEL_CURCUMFERENCE_METERS, + boolean TURN_MOTOR_INVERTED, boolean DRIVE_MOTOR_INVERTED, boolean ENCODER_INVERTED, + Rotation2d ENCODER_OFFSET, + int MODULE_INDEX, // can ID's - public final int DRIVE_MOTOR_ID; - public final int TURN_MOTOR_ID; - public final int ENCODER_ID; - + int DRIVE_MOTOR_ID, int TURN_MOTOR_ID, int ENCODER_ID, // Drive loop gains - public final double DRIVE_KV; - public final double DRIVE_KS; - public final double DRIVE_KA; - - public final double DRIVE_KP; - public final double DRIVE_KI; - public final double DRIVE_KD; - + double DRIVE_KV, double DRIVE_KS, double DRIVE_KA, + double DRIVE_KP, double DRIVE_KI, double DRIVE_KD, // Turning loop gains - public final double TURN_KP; - public final double TURN_KI; - public final double TURN_KD; - - public ModuleConstants(int id, - int[] ids, - double[] driveFF, - double[] driveFB, - double[] turnFB, - double offsetDegs, - boolean inverted, - GearRatios driveRatio) { - - double defaultTurnRatio = (150.0 / 7.0); - double defaultWheelRadiusMeters = Units.inchesToMeters(2.0); - - MODULE_INDEX = id; + double TURN_KP, double TURN_KI, double TURN_KD) { + public static final double DEFAULT_WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0); - DRIVE_MOTOR_ID = ids[0]; - TURN_MOTOR_ID = ids[1]; - ENCODER_ID = ids[2]; + public enum GearRatios { + TURN(150.0 / 7.0), + L1((14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0)), + L2((14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0)), + L3((14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0)); - DRIVE_KV = driveFF[0]; - DRIVE_KS = driveFF[1]; - DRIVE_KA = driveFF[2]; - - DRIVE_KP = driveFB[0]; - DRIVE_KI = driveFB[1]; - DRIVE_KD = driveFB[2]; - - TURN_KP = turnFB[0]; - TURN_KI = turnFB[1]; - TURN_KD = turnFB[2]; - - DRIVE_MOTOR_INVERTED = !inverted; - TURN_MOTOR_INVERTED = inverted; - ENCODER_INVERTED = false; - - ENCODER_OFFSET = Rotation2d.fromDegrees(offsetDegs); - - DRIVE_GEAR_RATIO = driveRatio.ratio; - TURNING_GEAR_RATIO = defaultTurnRatio; - - WHEEL_RADIUS_METERS = defaultWheelRadiusMeters; - WHEEL_CURCUMFERENCE_METERS = WHEEL_RADIUS_METERS * Math.PI * 2; + GearRatios(double ratio) { + this.ratio = ratio; } - public ModuleConstants (int id, - int[] ids, - double[] driveFF, - double[] driveFB, - double[] turnFB, - boolean[] inversions, - double offsetDegs, - GearRatios driveRatio, - double turningRatio, - double wheelRadiusMeter) { - MODULE_INDEX = id; - - DRIVE_MOTOR_ID = ids[0]; - TURN_MOTOR_ID = ids[1]; - ENCODER_ID = ids[2]; - - DRIVE_KV = driveFF[0]; - DRIVE_KS = driveFF[1]; - DRIVE_KA = driveFF[2]; - - DRIVE_KP = driveFB[0]; - DRIVE_KI = driveFB[1]; - DRIVE_KD = driveFB[2]; - - TURN_KP = turnFB[0]; - TURN_KI = turnFB[1]; - TURN_KD = turnFB[2]; - - DRIVE_MOTOR_INVERTED = inversions[0]; - TURN_MOTOR_INVERTED = inversions[1]; - ENCODER_INVERTED = false; - - ENCODER_OFFSET = Rotation2d.fromDegrees(offsetDegs); - - DRIVE_GEAR_RATIO = driveRatio.ratio; - TURNING_GEAR_RATIO = turningRatio; - - WHEEL_RADIUS_METERS = wheelRadiusMeter; - WHEEL_CURCUMFERENCE_METERS = WHEEL_RADIUS_METERS * Math.PI * 2; + public final double ratio; } + + public ModuleConstants(int id, + int[] ids, + double[] driveFF, + double[] driveFB, + double[] turnFB, + double offsetDegs, + boolean inverted, + GearRatios driveRatio) { + + this ( + driveRatio.ratio, GearRatios.TURN.ratio, + DEFAULT_WHEEL_RADIUS_METERS, + DEFAULT_WHEEL_RADIUS_METERS * 2 * Math.PI, + inverted, + !inverted, + !inverted, + Rotation2d.fromDegrees(offsetDegs), + id, + ids[0], ids[1], ids[2], + driveFF[0], driveFF[1], driveFF[2], + driveFB[0], driveFB[1], driveFB[2], + turnFB[0], turnFB[1], turnFB[2] + ); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index 5b9d49bc..5fb2d65d 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -28,29 +28,36 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; - private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); - private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); + private final DCMotorSim m_driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); + private final DCMotorSim m_turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); + + private final ModuleConstants m_moduleConstants; // use this mainly for module id + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; + public ModuleIOSim(ModuleConstants moduleConstants) { + m_moduleConstants = moduleConstants; + } + @Override public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(LOOP_PERIOD_SECS); - turnSim.update(LOOP_PERIOD_SECS); + m_driveSim.update(LOOP_PERIOD_SECS); + m_turnSim.update(LOOP_PERIOD_SECS); - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.drivePositionRad = m_driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = m_driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.driveCurrentAmps = new double[] {Math.abs(m_driveSim.getCurrentDrawAmps())}; inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + new Rotation2d(m_turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = new Rotation2d(m_turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = m_turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnCurrentAmps = new double[] {Math.abs(m_turnSim.getCurrentDrawAmps())}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; @@ -59,12 +66,17 @@ public void updateInputs(ModuleIOInputs inputs) { @Override public void setDriveVoltage(double volts) { driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - driveSim.setInputVoltage(driveAppliedVolts); + m_driveSim.setInputVoltage(driveAppliedVolts); } @Override public void setTurnVoltage(double volts) { turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - turnSim.setInputVoltage(turnAppliedVolts); + m_turnSim.setInputVoltage(turnAppliedVolts); + } + + @Override + public ModuleConstants getModuleConstants() { + return m_moduleConstants; } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 45cd6f50..ed183e4e 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -25,6 +25,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.gos.lib.phoenix6.properties.pid.Phoenix6TalonPidPropertyBuilder; +import com.gos.lib.properties.pid.PidProperty; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import java.util.Queue; @@ -44,7 +46,10 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFX m_driveTalon; private final TalonFX m_turnTalon; - private final CANcoder m_cancoder; + + private final PidProperty m_drivePid; + private final PidProperty m_turnPid; + private final ModuleConstants m_moduleConstants; private final StatusSignal m_drivePosition; @@ -60,53 +65,60 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal m_turnAppliedVolts; private final StatusSignal m_turnCurrent; - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final Rotation2d absoluteEncoderOffset; - public ModuleIOTalonFX(ModuleConstants moduleConstants) { - m_driveTalon = new TalonFX(moduleConstants.DRIVE_MOTOR_ID); - m_turnTalon = new TalonFX(moduleConstants.TURN_MOTOR_ID); - m_cancoder = new CANcoder(moduleConstants.ENCODER_ID); + m_driveTalon = new TalonFX(moduleConstants.DRIVE_MOTOR_ID()); + m_turnTalon = new TalonFX(moduleConstants.TURN_MOTOR_ID()); + CANcoder cancoder = new CANcoder(moduleConstants.ENCODER_ID()); m_moduleConstants = moduleConstants; - absoluteEncoderOffset = moduleConstants.ENCODER_OFFSET; - // run configs on drive motor var driveConfig = new TalonFXConfiguration(); driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = - moduleConstants.DRIVE_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive + moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; m_driveTalon.getConfigurator().apply(driveConfig); setDriveBrakeMode(true); // setup pid gains for drive motor - + m_drivePid = new Phoenix6TalonPidPropertyBuilder( + "Drive/Module" + m_moduleConstants.MODULE_INDEX() + "/Drive Pid Property", + false, + m_driveTalon, + 0 + ).addP(m_moduleConstants.DRIVE_KP()).build(); // run configs on turning motor var turnConfig = new TalonFXConfiguration(); turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; turnConfig.MotorOutput.Inverted = - moduleConstants.TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive + moduleConstants.TURN_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; m_turnTalon.getConfigurator().apply(turnConfig); setTurnBrakeMode(true); + m_turnPid = new Phoenix6TalonPidPropertyBuilder( + "Drive/Module" + m_moduleConstants.MODULE_INDEX() + "/Turn Pid Property", + false, + m_turnTalon, + 0) + .addP(m_moduleConstants.TURN_KP()) + .addI(m_moduleConstants.TURN_KI()) + .addD(m_moduleConstants.TURN_KD()) + .build(); + // run factory default on cancoder var encoderConfig = new CANcoderConfiguration(); encoderConfig.MagnetSensor.SensorDirection = - moduleConstants.ENCODER_INVERTED ? SensorDirectionValue.Clockwise_Positive + moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - m_cancoder.getConfigurator().apply(encoderConfig); + cancoder.getConfigurator().apply(encoderConfig); - // Fancy multi-threaded odometry update stuff + // Fancy multithreaded odometry update stuff // setup drive values m_driveTalon.setPosition(0.0); m_drivePosition = m_driveTalon.getPosition(); @@ -118,7 +130,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { // setup turn values m_turnTalon.setPosition(0.0); - m_turnAbsolutePosition = m_cancoder.getAbsolutePosition(); + m_turnAbsolutePosition = cancoder.getAbsolutePosition(); m_turnPosition = m_turnTalon.getPosition(); m_turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(m_turnTalon, m_turnTalon.getPosition()); @@ -156,29 +168,29 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnCurrent); inputs.drivePositionRad = - Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()}; inputs.turnAbsolutePosition = Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); + .minus(m_moduleConstants.ENCODER_OFFSET()); inputs.turnPosition = - Rotation2d.fromRotations((m_turnPosition.getValueAsDouble() / TURN_GEAR_RATIO)); + Rotation2d.fromRotations((m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO())); inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO(); inputs.turnAppliedVolts = m_turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = new double[] {m_turnCurrent.getValueAsDouble()}; inputs.odometryDrivePositionsRad = m_drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO()) .toArray(); inputs.odometryTurnPositions = m_turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO())) .toArray(Rotation2d[]::new); m_drivePositionQueue.clear(); m_turnPositionQueue.clear(); @@ -198,7 +210,7 @@ public void setTurnVoltage(double volts) { public void setDriveBrakeMode(boolean enable) { var config = new MotorOutputConfigs(); config.Inverted = - m_moduleConstants.DRIVE_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive + m_moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; m_driveTalon.getConfigurator().apply(config); @@ -208,7 +220,7 @@ public void setDriveBrakeMode(boolean enable) { public void setTurnBrakeMode(boolean enable) { var config = new MotorOutputConfigs(); config.Inverted = - m_moduleConstants.TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive + m_moduleConstants.TURN_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; m_turnTalon.getConfigurator().apply(config); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 4e4ca1d4..366c94a5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -5,8 +5,7 @@ public interface ShooterIO { @AutoLog - public class ShooterIOInputs { - + class ShooterIOInputs { } default void setMotorVoltageTL(double voltage) {} default void setMotorVoltageTR(double voltage) {} diff --git a/vendordeps/SnobotSim.json b/vendordeps/SnobotSim.json new file mode 100644 index 00000000..42051560 --- /dev/null +++ b/vendordeps/SnobotSim.json @@ -0,0 +1,43 @@ +{ + "fileName": "SnobotSim.json", + "name": "SnobotSim", + "version": "2024.01.09-03", + "uuid": "f34e80c0-aea1-11ee-a96e-5761b95fc5a8", + "frcYear": "2024", + "mavenUrls": ["https://raw.githubusercontent.com/snobotsim/maven_repo/master/release"], + "jsonUrl": "http://raw.githubusercontent.com/snobotsim/maven_repo/master/release/SnobotSim.json", + "javaDependencies": [ + { + "groupId": "org.snobotv2", + "artifactId": "snobot_sim_java_base", + "version": "2024.01.09-03" + }, + { + "groupId": "org.snobotv2", + "artifactId": "snobot_sim_java_navx", + "version": "2024.01.09-03" + }, + { + "groupId": "org.snobotv2", + "artifactId": "snobot_sim_java_phoenix5", + "version": "2024.01.09-03" + }, + { + "groupId": "org.snobotv2", + "artifactId": "snobot_sim_java_phoenix6", + "version": "2024.01.09-03" + }, + { + "groupId": "org.snobotv2", + "artifactId": "snobot_sim_java_revlib", + "version": "2024.01.09-03" + }, + { + "groupId": "org.snobotv2", + "artifactId": "snobot_swerve_sim", + "version": "2024.01.09-03" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file