diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68c00ed..a670d71 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -137,4 +137,3 @@ public void simulationInit() {} @Override public void simulationPeriodic() {} } - diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b2016e..1ac7323 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,8 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.flywheel.FlywheelIO; +import frc.robot.subsystems.flywheel.FlywheelIOTalonFX; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -35,6 +37,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final FlywheelIO flywheel; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -71,6 +74,8 @@ public RobotContainer() { // new ModuleIOTalonFXS(TunerConstants.FrontRight), // new ModuleIOTalonFXS(TunerConstants.BackLeft), // new ModuleIOTalonFXS(TunerConstants.BackRight)); + + flywheel = new FlywheelIOTalonFX(); break; case SIM: @@ -82,6 +87,8 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); + + flywheel = new FlywheelIOTalonFX(); break; default: @@ -93,6 +100,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + + flywheel = new FlywheelIO() {}; break; } @@ -163,6 +172,7 @@ private void configureButtonBindings() { Rotation2d.kZero)), drive) .ignoringDisable(true)); + controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1))); } /** diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java new file mode 100644 index 0000000..4bb6523 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.flywheel; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Flywheel extends SubsystemBase { + // Auto logged classes for AKit + private final FlywheelIO io; + private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Flywheel", inputs); + } + + public Flywheel(FlywheelIO io) { + this.io = io; + } + + public Command setRoller(double velocity) { + return runEnd( + () -> { + io.setRoller(velocity); + }, + () -> { + io.setRoller(0); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java new file mode 100644 index 0000000..1870998 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.flywheel; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; + +public class FlywheelConfigs { + static final int leftMotorID = 41; + static final int rightMotorID = 67; + + static TalonFXConfiguration rollerConfig = + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java new file mode 100644 index 0000000..83ea527 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + + @AutoLog + public static class FlywheelIOInputs { + public double leftMotorVelocityRPM = 0; + public double leftMotorSpeed = 0; + + public double rightMotorVelocityRPM = 0; + public double rightMotorSpeed = 0; + } + + public default void updateInputs(FlywheelIOInputs inputs) {} + + public default void setRoller(double velocity) {} +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 0000000..b989c01 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.flywheel; + +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.Robot; +import frc.robot.util.sim.PhysicsSim; + +public class FlywheelIOTalonFX implements FlywheelIO { + private final TalonFX leftMotor; + private final TalonFX rightMotor; + + private final MotionMagicVelocityVoltage motionRequest = new MotionMagicVelocityVoltage(0); + + public FlywheelIOTalonFX() { + leftMotor = new TalonFX(FlywheelConfigs.leftMotorID); + rightMotor = new TalonFX(FlywheelConfigs.rightMotorID); + // make left a follower of right + leftMotor.setControl(new Follower(rightMotor.getDeviceID(), false)); + leftMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); + rightMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig); + if (Robot.isSimulation()) { + PhysicsSim.getInstance().addTalonFX(leftMotor); + PhysicsSim.getInstance().addTalonFX(rightMotor); + } + } + + @Override + public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) { + inputs.leftMotorVelocityRPM = leftMotor.getVelocity().getValueAsDouble(); + inputs.leftMotorSpeed = leftMotor.getDutyCycle().getValueAsDouble(); + + inputs.rightMotorVelocityRPM = rightMotor.getVelocity().getValueAsDouble(); + inputs.rightMotorSpeed = rightMotor.getDutyCycle().getValueAsDouble(); + } + + @Override + public void setRoller(double velocity) { + leftMotor.setControl(motionRequest.withVelocity(velocity)); + } +} diff --git a/src/main/java/frc/robot/util/sim/PhysicsSim.java b/src/main/java/frc/robot/util/sim/PhysicsSim.java new file mode 100644 index 0000000..8316ddd --- /dev/null +++ b/src/main/java/frc/robot/util/sim/PhysicsSim.java @@ -0,0 +1,68 @@ +package frc.robot.util.sim; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import java.util.ArrayList; + +/** Manages physics simulation for CTRE products. */ +public class PhysicsSim { + private static final PhysicsSim sim = new PhysicsSim(); + private final ArrayList simProfiles = new ArrayList<>(); + + /** Gets the robot simulator instance. */ + public static PhysicsSim getInstance() { + return sim; + } + + /** + * Adds a TalonFX controller to the simulator. + * + * @param talonFX The TalonFX device + */ + public void addTalonFX(TalonFX talonFX) { + if (talonFX != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, 0.001); + simProfiles.add(simTalonFX); + } + } + + public void addTalonFX(TalonFX talonFX, CANcoder cancoder) { + if (talonFX != null && cancoder != null) { + TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, 0.001, cancoder); + simProfiles.add(simTalonFX); + } + } + + /** Runs the simulator: - enable the robot - simulate sensors */ + public void run() { + // Simulate devices + for (SimProfile simProfile : simProfiles) { + simProfile.run(); + } + } + + /** Holds information about a simulated device. */ + static class SimProfile { + private double _lastTime; + private boolean _running = false; + + /** Runs the simulation profile. Implemented by device-specific profiles. */ + public void run() {} + + /** Returns the time since last call, in seconds. */ + protected double getPeriod() { + // set the start time if not yet running + if (!_running) { + _lastTime = Utils.getCurrentTimeSeconds(); + _running = true; + } + + double now = Utils.getCurrentTimeSeconds(); + final double period = now - _lastTime; + _lastTime = now; + + return period; + } + } +} diff --git a/src/main/java/frc/robot/util/sim/TalonFXSimProfile.java b/src/main/java/frc/robot/util/sim/TalonFXSimProfile.java new file mode 100644 index 0000000..67a98f2 --- /dev/null +++ b/src/main/java/frc/robot/util/sim/TalonFXSimProfile.java @@ -0,0 +1,67 @@ +package frc.robot.util.sim; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** Holds information about a simulated TalonFX. */ +class TalonFXSimProfile extends PhysicsSim.SimProfile { + private static final double MOTOR_RESISTANCE = + 0.002; // Assume 2mOhm resistance for voltage drop calculation + + private final DCMotorSim motorSim; + private final TalonFXSimState talonFXSim; + private CANcoderSimState cancoderSimState; + + /** + * Creates a new simulation profile for a TalonFX device. + * + * @param talonFX The TalonFX device + * @param rotorInertia Rotational Inertia of the mechanism at the rotor + */ + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia) { + var gearbox = DCMotor.getKrakenX60Foc(1); + this.motorSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox); + this.talonFXSim = talonFX.getSimState(); + } + + public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia, CANcoder cancoder) { + this(talonFX, rotorInertia); + cancoderSimState = cancoder.getSimState(); + } + + /** + * Runs the simulation profile. + * + *

This uses very rudimentary physics simulation and exists to allow users to test features + * of our products in simulation using our examples out of the box. Users may modify this to + * utilize more accurate physics simulation. + */ + public void run() { + /// DEVICE SPEED SIMULATION + + motorSim.setInputVoltage(talonFXSim.getMotorVoltage()); + + motorSim.update(getPeriod()); + + /// SET SIM PHYSICS INPUTS + final double position_rot = motorSim.getAngularPositionRotations(); + final double velocity_rps = Units.radiansToRotations(motorSim.getAngularVelocityRPM()); + + if (cancoderSimState != null) { + cancoderSimState.setRawPosition(position_rot); + cancoderSimState.setVelocity(velocity_rps); + } + talonFXSim.setRawRotorPosition(position_rot); + talonFXSim.setRotorVelocity(velocity_rps); + + talonFXSim.setSupplyVoltage(12 - talonFXSim.getSupplyCurrent() * MOTOR_RESISTANCE); + } +}