diff --git a/simgui-ds.json b/simgui-ds.json index 78037a0..7c47bfc 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -94,7 +99,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5b2c31f..6ab9d4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,12 +11,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; 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.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; @@ -26,11 +24,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelIO; -import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelSubsystem; -import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelTalonFX; -import frc.robot.subsystems.flywheel.FlywheelIO; -import frc.robot.subsystems.flywheel.FlywheelIOTalonFX; +import frc.robot.subsystems.pivot.PivotIO; +import frc.robot.subsystems.pivot.PivotPos; +import frc.robot.subsystems.pivot.PivotSubsystem; +import frc.robot.subsystems.pivot.PivotTalonFX; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -42,8 +39,7 @@ public class RobotContainer { // Subsystems private final Drive drive; - private IntakeFeederwheelSubsystem intakeFeederwheel; - private final FlywheelIO flywheel; + public final PivotSubsystem pivot; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -58,6 +54,8 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations // ModuleIOTalonFX is intended for modules with TalonFX drive, TalonFX turn, and a // CANcoder + pivot = new PivotSubsystem(new PivotTalonFX()); + drive = new Drive( new GyroIOPigeon2(), @@ -80,14 +78,11 @@ public RobotContainer() { // new ModuleIOTalonFXS(TunerConstants.FrontRight), // new ModuleIOTalonFXS(TunerConstants.BackLeft), // new ModuleIOTalonFXS(TunerConstants.BackRight)); - - intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelTalonFX()); - - flywheel = new FlywheelIOTalonFX(); break; case SIM: - // Sim robot, instantiate physics sim IO implementations + // Sim robot, instantiate physics sim IO implementation + pivot = new PivotSubsystem(new PivotTalonFX()); drive = new Drive( new GyroIO() {}, @@ -95,14 +90,11 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - - intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelTalonFX()); - - flywheel = new FlywheelIOTalonFX(); break; default: // Replayed robot, disable IO implementations + pivot = new PivotSubsystem(new PivotIO() {}); drive = new Drive( new GyroIO() {}, @@ -110,10 +102,6 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - - intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelIO() {}); - - flywheel = new FlywheelIO() {}; break; } @@ -146,10 +134,12 @@ public RobotContainer() { /** * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link Joystick} or {@link - * XboxController}), and then passing it to a {@link JoystickButton}. + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + // Default command, normal field-relative drive drive.setDefaultCommand( DriveCommands.joystickDrive( @@ -157,7 +147,6 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - // Lock to 0° when A button is held controller .a() @@ -184,15 +173,17 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); - controller.rightTrigger().onTrue(intakeFeederwheel.rollIn()); - controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1))); + controller + .leftBumper() + .onTrue(Commands.run(() -> pivot.setPosition(PivotPos.PivotDown), pivot)); + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ public Command getAutonomousCommand() { return autoChooser.get(); } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index f087335..3c5b0d6 100755 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -15,4 +15,6 @@ */ public final class Constants { public static final int PRIMARY_CONTROLLER_PORT = 0; + + public static final String RIO_BUS = "rio"; } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index b989c01..fc46081 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -2,7 +2,6 @@ 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; diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotConfigs.java b/src/main/java/frc/robot/subsystems/pivot/PivotConfigs.java new file mode 100644 index 0000000..81f7936 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotConfigs.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.pivot; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +public class PivotConfigs { + public static final int pivotMotorID = 1; + public static final int pivotCANcoderID = 2; + // values may change as tuning progresses + public static final CANcoderConfiguration canCoderConfig = + new CANcoderConfiguration() + .withMagnetSensor( + new com.ctre.phoenix6.configs.MagnetSensorConfigs() + .withSensorDirection( + SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(0.0)); + public static TalonFXConfiguration motorConfig = + new TalonFXConfiguration() + .withSlot0( + new Slot0Configs() + .withGravityType(GravityTypeValue.Arm_Cosine) + .withKD(0.0) + .withKP(0.0) + .withKI(0.0) + .withKG(0.0) + .withKS(0.0) + .withKV(0.0)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(0.0) + .withSupplyCurrentLimit(0.0) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true)); +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java new file mode 100644 index 0000000..92b1359 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.pivot; + +import org.littletonrobotics.junction.AutoLog; + +public interface PivotIO { + @AutoLog + class PivotIOinput { + public double pos = 0.0; + public double velocity = 0.0; + } + + public default void updateInputs(PivotIOinput inputs) {} + + public default void setPosition(double position) {} + + public default void applyPower(double power) {} + + public default void stop() {} +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotPos.java b/src/main/java/frc/robot/subsystems/pivot/PivotPos.java new file mode 100644 index 0000000..5d4bcf0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotPos.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.pivot; + +import edu.wpi.first.units.Units; + +public enum PivotPos { + PivotDown(0.0), + PivotUp(0.0); + + public final double position; + + PivotPos(double rotation) { + this.position = Units.Rotations.of(rotation).magnitude(); + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java new file mode 100644 index 0000000..b9092e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class PivotSubsystem extends SubsystemBase { + private final PivotIO io; + private final PivotIOinputAutoLogged inputs = new PivotIOinputAutoLogged(); + + public PivotSubsystem(PivotIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Pivot", inputs); + } + + public void setPosition(PivotPos pivotPos) { + io.setPosition(pivotPos.position); + } + + public Command stopCommand() { + return runOnce(io::stop); + } + + public Command setPositionCommand(PivotPos pivotPos) { + return run(() -> setPosition(pivotPos)).finallyDo(io::stop); + } + + public Command applyPowerCommand(double power) { + return run(() -> io.applyPower(power)).finallyDo(io::stop); + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotTalonFX.java b/src/main/java/frc/robot/subsystems/pivot/PivotTalonFX.java new file mode 100644 index 0000000..5405082 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotTalonFX.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.pivot; + +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.util.PhysicsSim; + +public class PivotTalonFX implements PivotIO { + private final MotionMagicTorqueCurrentFOC motionMagicReq = new MotionMagicTorqueCurrentFOC(0); + private final TalonFX pivotMotor; + private final CANcoder pivotCan; + + public PivotTalonFX() { + + this.pivotMotor = new TalonFX(PivotConfigs.pivotMotorID); + this.pivotCan = new CANcoder(PivotConfigs.pivotCANcoderID); + this.pivotMotor.getConfigurator().apply(PivotConfigs.motorConfig); + this.pivotCan.getConfigurator().apply(PivotConfigs.canCoderConfig); + if (RobotBase.isSimulation()) { + PhysicsSim.getInstance().addTalonFX(pivotMotor, pivotCan); + } + } + + @Override + public void updateInputs(PivotIOinput inputs) { + inputs.pos = (double) pivotCan.getAbsolutePosition().getValueAsDouble(); + inputs.velocity = (double) pivotMotor.getVelocity().getValueAsDouble(); + } + + @Override + public void setPosition(double positionRad) { + pivotMotor.setControl(motionMagicReq.withPosition(positionRad)); + } + + @Override + public void applyPower(double power) { + pivotMotor.setControl(new DutyCycleOut(power)); + } + + @Override + public void stop() { + pivotMotor.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/util/PhysicsSim.java b/src/main/java/frc/robot/util/PhysicsSim.java new file mode 100644 index 0000000..0df6ed9 --- /dev/null +++ b/src/main/java/frc/robot/util/PhysicsSim.java @@ -0,0 +1,68 @@ +package frc.robot.util; + +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/TalonFXSimProfile.java b/src/main/java/frc/robot/util/TalonFXSimProfile.java new file mode 100644 index 0000000..e31056b --- /dev/null +++ b/src/main/java/frc/robot/util/TalonFXSimProfile.java @@ -0,0 +1,68 @@ +package frc.robot.util; + +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.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.getAngularPosition(); + // final double velocity_rps = + // Units.radiansToRotations(motorSim.getAngularVelocity()); + // + // 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); + } +}