diff --git a/.gitmodules b/.gitmodules index 93a08b0c..2d5fdfb8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ [submodule "src/main/java/frc/trigon/lib"] path = src/main/java/frc/trigon/lib url = https://github.com/Programming-TRIGON/TRIGONLib + branch = subsystem-wrapper diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 6732cfae..6b9824d0 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 6732cfae4b692ecb714bd664012a4e68eaa62b43 +Subproject commit 6b9824d047b81a262d6d0bf83412fa779bd61624 diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2e6ec610..e57e392e 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -9,8 +9,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.subsystems.MotorSubsystem; +import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem; +import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystemCommands; import frc.trigon.lib.utilities.flippable.Flippable; -import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; @@ -19,7 +21,8 @@ import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; -import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.flyWheel.FlyWheel; +import frc.trigon.robot.subsystems.flyWheel.FlyWheelConstants; import frc.trigon.robot.subsystems.swerve.Swerve; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -32,6 +35,9 @@ public class RobotContainer { CameraConstants.OBJECT_DETECTION_CAMERA ); public static final Swerve SWERVE = new Swerve(); + // public static final ArmSubsystem ARM = new Arm(); + // public static final ElevatorSubsystem ELEVATOR = new Elevator(); + public static final SimpleMotorSubsystem FLY_WHEEL = new FlyWheel(); private LoggedDashboardChooser autoChooser; public RobotContainer() { @@ -54,12 +60,16 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); + // ARM.setDefaultCommand(ArmSubsystemCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST, ARM)); + // ELEVATOR.setDefaultCommand(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST,ELEVATOR)); + FLY_WHEEL.setDefaultCommand(SimpleMotorSubsystemCommands.getSetTargetStateCommand(FlyWheelConstants.FlyWheelState.REST, FLY_WHEEL)); } private void bindControllerCommands() { - OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); - OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); - OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); +// OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); +// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); +// OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.INTAKE_TRIGGER.whileTrue(SimpleMotorSubsystemCommands.getSetTargetStateCommand(FlyWheelConstants.FlyWheelState.INTAKE, FLY_WHEEL)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 5fbedf61..10e0d465 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -3,8 +3,8 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.lib.subsystems.MotorSubsystem; import java.util.function.BooleanSupplier; @@ -14,7 +14,7 @@ */ public class GeneralCommands { public static Command getFieldRelativeDriveCommand() { - return SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + return SwerveCommands.getOpenLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX()) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index faa1017d..5ceb71e1 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -5,12 +5,13 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.lib.hardware.misc.KeyboardController; import frc.trigon.lib.hardware.misc.XboxController; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import java.util.function.DoubleUnaryOperator; public class OperatorConstants { public static final double DRIVER_CONTROLLER_DEADBAND = 0.07; - private static final int DRIVER_CONTROLLER_PORT = 0; + private static final int DRIVER_CONTROLLER_PORT = 1; private static final int DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 1, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT = 2; @@ -43,5 +44,6 @@ public class OperatorConstants { FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), - BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); + BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), + INTAKE_TRIGGER = DRIVER_CONTROLLER.leftTrigger(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java deleted file mode 100644 index b3fc0856..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ /dev/null @@ -1,171 +0,0 @@ -package frc.trigon.robot.subsystems; - -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; -import frc.trigon.lib.hardware.RobotHardwareStats; - -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.Executor; -import java.util.concurrent.Executors; -import java.util.function.Consumer; - -/** - * A class that represents a subsystem that has motors (rather than something like LEDs). - * This class will automatically stop all the motors when the robot is disabled, and set the motors to brake when the robot is enabled. - * If a subsystem doesn't need to ever brake (i.e. shooter, flywheel, etc.), then it should override the {@link #setBrake(boolean)} method and do nothing. - */ -public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase { - public static boolean IS_BRAKING = true; - private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); - private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); - private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8); - private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation()); - - static { - DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); - DISABLED_TRIGGER.onFalse(new InstantCommand(() -> { - setAllSubsystemsBrakeAsync(true); - IS_BRAKING = true; - }).ignoringDisable(true)); - } - - private final SysIdRoutine sysIDRoutine = createSysIDRoutine(); - - public MotorSubsystem() { - REGISTERED_SUBSYSTEMS.add(this); - } - - /** - * Runs the given consumer on all the subsystem instances. - * - * @param toRun the consumer to run on each registered subsystem - */ - public static void forEach(Consumer toRun) { - REGISTERED_SUBSYSTEMS.forEach(toRun); - } - - /** - * Sets whether the all the subsystems should brake or coast their motors. - * This command will run asynchronously, since the TalonFX's setting of brake/coast is blocking. - * - * @param brake whether the motors should brake or coast - */ - public static void setAllSubsystemsBrakeAsync(boolean brake) { - BRAKE_MODE_EXECUTOR.execute(() -> forEach((subsystem) -> subsystem.setBrake(brake))); - } - - public static boolean isExtensiveLoggingEnabled() { - return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay(); - } - - /** - * Runs periodically, to update the subsystem, and update the mechanism of the subsystem (if there is one). - * This only updates the mechanism if the robot is in replay mode or extensive logging is enabled. - * This function cannot be overridden. Use {@linkplain MotorSubsystem#updatePeriodically} or {@linkplain MotorSubsystem#updateMechanism} (depending on the usage) instead. - */ - @Override - public final void periodic() { - updatePeriodically(); - if (isExtensiveLoggingEnabled()) - updateMechanism(); - } - - /** - * Creates a quasistatic (ramp up) command for characterizing the subsystem's mechanism. - * - * @param direction the direction in which to run the test - * @return the command - * @throws IllegalStateException if the {@link MotorSubsystem#getSysIDConfig()} function wasn't overridden or returns null - */ - public final Command getQuasistaticCharacterizationCommand(SysIdRoutine.Direction direction) throws IllegalStateException { - if (sysIDRoutine == null) - throw new IllegalStateException("Subsystem " + getName() + " doesn't have a SysID routine!"); - return sysIDRoutine.quasistatic(direction); - } - - /** - * Creates a dynamic (constant "step up") command for characterizing the subsystem's mechanism. - * - * @param direction the direction in which to run the test - * @return the command - * @throws IllegalStateException if the {@link MotorSubsystem#getSysIDConfig()} function wasn't overridden or returns null - */ - public final Command getDynamicCharacterizationCommand(SysIdRoutine.Direction direction) throws IllegalStateException { - if (sysIDRoutine == null) - throw new IllegalStateException("Subsystem " + getName() + " doesn't have a SysID routine!"); - return sysIDRoutine.dynamic(direction); - } - - /** - * Drives the motor with the given voltage for characterizing. - * - * @param targetDrivePower the target drive power, unitless. This can be amps, volts, etc. Depending on the characterization type - */ - public void sysIDDrive(double targetDrivePower) { - } - - /** - * Updates the SysId log of the motor states for characterizing. - * - * @param log the log to update - */ - public void updateLog(SysIdRoutineLog log) { - } - - public SysIdRoutine.Config getSysIDConfig() { - return null; - } - - /** - * Sets whether the subsystem's motors should brake or coast. - * If a subsystem doesn't need to ever brake (i.e. shooter, flywheel, etc.), don't implement this method. - * - * @param brake whether the motors should brake or coast - */ - public void setBrake(boolean brake) { - } - - /** - * Updates periodically. Anything that should be updated periodically but isn't related to the mechanism (or shouldn't always be logged, in order to save resources) should be put here. - */ - public void updatePeriodically() { - } - - /** - * Updates the mechanism of the subsystem periodically if the robot is in replay mode, or if {@linkplain MotorSubsystem#ENABLE_EXTENSIVE_LOGGING) is true. - * This doesn't always run in order to save resources. - */ - public void updateMechanism() { - } - - public void changeDefaultCommand(Command newDefaultCommand) { - final Command currentDefaultCommand = getDefaultCommand(); - if (currentDefaultCommand != null) - currentDefaultCommand.cancel(); - setDefaultCommand(newDefaultCommand); - } - - public abstract void stop(); - - private SysIdRoutine createSysIDRoutine() { - if (getSysIDConfig() == null) - return null; - - return new SysIdRoutine( - getSysIDConfig(), - new SysIdRoutine.Mechanism( - (voltage) -> sysIDDrive(voltage.in(Units.Volts)), - this::updateLog, - this, - getName() - ) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java new file mode 100644 index 00000000..d9a3f009 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -0,0 +1,12 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Pose3d; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.subsystems.arm.ArmSubsystem; +import frc.trigon.lib.subsystems.arm.ArmSubsystemConfiguration; + +public class Arm extends ArmSubsystem { + public Arm() { + super(ArmConstants.MASTER_MOTOR, ArmConstants.ARM_CONFIG, new Pose3d()); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java new file mode 100644 index 00000000..90f46b5e --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,171 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.*; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder; +import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.subsystems.arm.ArmSubsystem; +import frc.trigon.lib.subsystems.arm.ArmSubsystemConfiguration; + +public class ArmConstants { + private static final int + MASTER_MOTOR_ID = 11, + FOLLOWER_MOTOR_ID = 12, + ENCODER_ID = 11; + private static final String + MASTER_MOTOR_NAME = "ArmMasterMotor", + FOLLOWER_MOTOR_NAME = "ArmFollowerMotor", + ENCODER_NAME = "ArmEncoder"; + static final TalonFXMotor + MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), + FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); + private static final CANcoderEncoder ENCODER = new CANcoderEncoder(ENCODER_ID, ENCODER_NAME); + + private static final String NAME = "Arm"; + private final static double + GEAR_RATIO = 40, + MAXIMUM_VELOCITY = 20, + MAXIMUM_ACCELERATION = 7, + MAXIMUM_JERK = 1, + ARM_LENGTH_METERS = 1, + ARM_MASS_KILOGRAMS = 10, + SYS_ID_RAMP_RATE = 1, + SYS_ID_STEP_VOLTAGE = 5, + VISUALIZATION_OFFSET = 0; + private final static Rotation2d + MINIMUM_ANGLE = Rotation2d.fromDegrees(0), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(360), + ANGLE_TOLERANCE = Rotation2d.fromDegrees(3); + private final static boolean FOC_ENABLED = true; + private final static boolean SHOULD_SIMULATE_GRAVITY = true; + private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2); + + static ArmSubsystemConfiguration ARM_CONFIG = new ArmSubsystemConfiguration(); + + static { + configureSubsystem(); + configureMasterMotor(); + configureFollowerMotor(); + configureAngleEncoder(); + } + + private static void configureSubsystem() { + ARM_CONFIG.name = NAME; + ARM_CONFIG.gearRatio = GEAR_RATIO; + ARM_CONFIG.maximumVelocity = MAXIMUM_VELOCITY; + ARM_CONFIG.maximumAcceleration = MAXIMUM_ACCELERATION; + ARM_CONFIG.maximumJerk = MAXIMUM_JERK; + ARM_CONFIG.lengthMeters = ARM_LENGTH_METERS; + ARM_CONFIG.massKilograms = ARM_MASS_KILOGRAMS; + ARM_CONFIG.minimumAngle = MINIMUM_ANGLE; + ARM_CONFIG.maximumAngle = MAXIMUM_ANGLE; + ARM_CONFIG.angleTolerance = ANGLE_TOLERANCE; + ARM_CONFIG.shouldSimulateGravity = SHOULD_SIMULATE_GRAVITY; + ARM_CONFIG.sysIDRampRate = SYS_ID_RAMP_RATE; + ARM_CONFIG.sysIDStepVoltage = SYS_ID_STEP_VOLTAGE; + ARM_CONFIG.focEnabled = FOC_ENABLED; + ARM_CONFIG.gearbox = GEAR_BOX; + ARM_CONFIG.visualizationOffset = VISUALIZATION_OFFSET; + } + + private static void configureMasterMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.Feedback.FeedbackRemoteSensorID = ENCODER.getID(); + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.Slot0.kP = 10; + config.Slot0.kI = 0; + config.Slot0.kD = 0; + config.Slot0.kS = 0.01529; + config.Slot0.kG = 0.78412; + config.Slot0.kA = 0; + config.Slot0.kV = 4.9117; + + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.MotionMagic.MotionMagicCruiseVelocity = MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + MASTER_MOTOR.applyConfiguration(config); + MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureFollowerMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower FollowerRequest = new Follower(MASTER_MOTOR_ID, false); + FOLLOWER_MOTOR.setControl(FollowerRequest); + } + + private static void configureAngleEncoder() { + final CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + config.MagnetSensor.MagnetOffset = VISUALIZATION_OFFSET; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; + + ENCODER.applyConfiguration(config); + ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); + + ENCODER.registerSignal(CANcoderSignal.POSITION, 100); + ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); + } + + public enum ArmState implements ArmSubsystem.ArmState { + REST(Rotation2d.fromDegrees(0), null, 1), + NINETY(Rotation2d.fromDegrees(90), null, 1), + ONE_EIGHTY(Rotation2d.fromDegrees(180), null, 1); + + private final Rotation2d targetAngle; + private final ArmState prepareState; + private final double speedScalar; + + ArmState(Rotation2d targetAngle, ArmState prepareState, double speedScalar) { + this.targetAngle = targetAngle; + this.prepareState = prepareState; + this.speedScalar = speedScalar; + } + + @Override + public Rotation2d getTargetAngle() { + return targetAngle; + } + + @Override + public ArmSubsystem.ArmState getPrepareState() { + return prepareState; + } + + @Override + public double getSpeedScalar() { + return speedScalar; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java new file mode 100644 index 00000000..10d9f96d --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,12 @@ +package frc.trigon.robot.subsystems.elevator; + +import edu.wpi.first.math.geometry.Pose3d; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.subsystems.elevator.ElevatorSubsystem; +import frc.trigon.lib.subsystems.elevator.ElevatorSubsystemConfiguration; + +public class Elevator extends ElevatorSubsystem { + public Elevator() { + super(ElevatorConstants.MASTER_MOTOR, ElevatorConstants.ELEVATOR_CONFIG, new Pose3d()); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 00000000..8b603e92 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,147 @@ +package frc.trigon.robot.subsystems.elevator; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import edu.wpi.first.math.system.plant.DCMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.subsystems.elevator.ElevatorSubsystem; +import frc.trigon.lib.subsystems.elevator.ElevatorSubsystemConfiguration; + +public class ElevatorConstants { + private static final int + MASTER_MOTOR_ID = 13, + FOLLOWER_MOTOR_ID = 14; + private static final String + MASTER_MOTOR_NAME = "ElevatorMasterMotor", + FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor"; + static final TalonFXMotor + MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), + FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); + + private static final String NAME = "Elevator"; + private final static double + GEAR_RATIO = 4, + MAXIMUM_VELOCITY = 20, + MAXIMUM_ACCELERATION = 7, + MAXIMUM_JERK = 70, + ELEVATOR_MASS_KILOGRAMS = 5, + SYS_ID_RAMP_RATE = 0.01, + SYS_ID_STEP_VOLTAGE = 1, + MINIMUM_HEIGHT_METERS = 0.1, + MAXIMUM_HEIGHT_METERS = 1.5, + HEIGHT_TOLERANCE_METERS = 0.1, + DRUM_RADIUS_METERS = 0.05; + private final static boolean FOC_ENABLED = true; + private final static boolean SHOULD_SIMULATE_GRAVITY = true; + private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2); + + static ElevatorSubsystemConfiguration ELEVATOR_CONFIG = new ElevatorSubsystemConfiguration(); + + static { + configureSubsystem(); + configureMasterMotor(); + configureFollowerMotor(); + } + + private static void configureSubsystem() { + ELEVATOR_CONFIG.name = NAME; + ELEVATOR_CONFIG.gearRatio = GEAR_RATIO; + ELEVATOR_CONFIG.maximumVelocity = MAXIMUM_VELOCITY; + ELEVATOR_CONFIG.maximumAcceleration = MAXIMUM_ACCELERATION; + ELEVATOR_CONFIG.maximumJerk = MAXIMUM_JERK; + ELEVATOR_CONFIG.massKilograms = ELEVATOR_MASS_KILOGRAMS; + ELEVATOR_CONFIG.minimumHeight = MINIMUM_HEIGHT_METERS; + ELEVATOR_CONFIG.maximumHeight = MAXIMUM_HEIGHT_METERS; + ELEVATOR_CONFIG.positionToleranceMeters = HEIGHT_TOLERANCE_METERS; + ELEVATOR_CONFIG.shouldSimulateGravity = SHOULD_SIMULATE_GRAVITY; + ELEVATOR_CONFIG.sysIDRampRate = SYS_ID_RAMP_RATE; + ELEVATOR_CONFIG.sysIDStepVoltage = SYS_ID_STEP_VOLTAGE; + ELEVATOR_CONFIG.focEnabled = FOC_ENABLED; + ELEVATOR_CONFIG.gearbox = GEAR_BOX; + ELEVATOR_CONFIG.drumRadiusMeters = DRUM_RADIUS_METERS; + } + + private static void configureMasterMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.Slot0.kP = 5; + config.Slot0.kI = 0; + config.Slot0.kD = 0; + config.Slot0.kS = 0.027174; + config.Slot0.kG = 3; + config.Slot0.kA = 0; + config.Slot0.kV = 5; + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.MotionMagic.MotionMagicCruiseVelocity = MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + MASTER_MOTOR.applyConfiguration(config); + MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureFollowerMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower FollowerRequest = new Follower(MASTER_MOTOR_ID, false); + FOLLOWER_MOTOR.setControl(FollowerRequest); + } + + public enum ElevatorState implements ElevatorSubsystem.ElevatorState { + REST(0.2, null, 1), + HALF(0.7, null, 1), + FULL(1.5, null, 1); + + private final double targetPosition; + private final ElevatorState prepareState; + private final double speedScalar; + + ElevatorState(double targetPosition, ElevatorState prepareState, double speedScalar) { + this.targetPosition = targetPosition; + this.prepareState = prepareState; + this.speedScalar = speedScalar; + } + + @Override + public double getTargetPositionMeters() { + return 0; + } + + @Override + public ElevatorSubsystem.ElevatorState getPrepareState() { + return prepareState; + } + + @Override + public double getSpeedScalar() { + return speedScalar; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java new file mode 100644 index 00000000..82805553 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java @@ -0,0 +1,9 @@ +package frc.trigon.robot.subsystems.flyWheel; + +import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem; + +public class FlyWheel extends SimpleMotorSubsystem { + public FlyWheel() { + super(FlyWheelConstants.MASTER_MOTOR, FlyWheelConstants.FLY_WHEEL_CONFIG); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java new file mode 100644 index 00000000..93684d50 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java @@ -0,0 +1,125 @@ +package frc.trigon.robot.subsystems.flyWheel; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.system.plant.DCMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem; +import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystemConfiguration; + +public class FlyWheelConstants { + private static final int + MASTER_MOTOR_ID = 9, + FOLLOWER_MOTOR_ID = 10; + private static final String + MASTER_MOTOR_NAME = "FlyWheelMasterMotor", + FOLLOWER_MOTOR_NAME = "FlyWheelFollowerMotor"; + static final TalonFXMotor + MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), + FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); + + private static final String NAME = "FlyWheel"; + private final static double + GEAR_RATIO = 1, + MOMENT_OF_INERTIA = 0.003, + MAXIMUM_VELOCITY = 20, + MAXIMUM_ACCELERATION = 7, + MAXIMUM_JERK = 1, + MAXIMUM_DISPLAYABLE_VELOCITY = 1, + VELOCITY_TO_TOLERANCE = 1, + SYS_ID_RAMP_RATE = 3, + SYS_ID_STEP_VOLTAGE = 1; + private final static boolean FOC_ENABLED = true; + private final static boolean SHOULD_USE_VOLTAGE_CONTROL = true; + private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2); + + static SimpleMotorSubsystemConfiguration FLY_WHEEL_CONFIG = new SimpleMotorSubsystemConfiguration(); + + static { + configureSubsystem(); + configureMasterMotor(); + configureFollowerMotor(); + } + + private static void configureSubsystem() { + FLY_WHEEL_CONFIG.name = NAME; + FLY_WHEEL_CONFIG.gearRatio = GEAR_RATIO; + FLY_WHEEL_CONFIG.momentOfInertia = MOMENT_OF_INERTIA; + FLY_WHEEL_CONFIG.maximumVelocity = MAXIMUM_VELOCITY; + FLY_WHEEL_CONFIG.maximumAcceleration = MAXIMUM_ACCELERATION; + FLY_WHEEL_CONFIG.maximumJerk = MAXIMUM_JERK; + FLY_WHEEL_CONFIG.maximumDisplayableVelocity = MAXIMUM_DISPLAYABLE_VELOCITY; + FLY_WHEEL_CONFIG.velocityTolerance = VELOCITY_TO_TOLERANCE; + FLY_WHEEL_CONFIG.sysIDRampRate = SYS_ID_RAMP_RATE; + FLY_WHEEL_CONFIG.sysIDStepVoltage = SYS_ID_STEP_VOLTAGE; + FLY_WHEEL_CONFIG.focEnabled = FOC_ENABLED; + FLY_WHEEL_CONFIG.shouldUseVoltageControl = SHOULD_USE_VOLTAGE_CONTROL; + FLY_WHEEL_CONFIG.gearbox = GEAR_BOX; + } + + private static void configureMasterMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.Slot0.kP = 0; + config.Slot0.kI = 0; + config.Slot0.kD = 0; + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kA = 0; + config.Slot0.kV = 0; + + MASTER_MOTOR.applyConfiguration(config); + MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureFollowerMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower FollowerRequest = new Follower(MASTER_MOTOR_ID, false); + FOLLOWER_MOTOR.setControl(FollowerRequest); + } + + public enum FlyWheelState implements SimpleMotorSubsystem.SimpleMotorState { + REST(0, null), + INTAKE(6, null); + + private final double targetVoltage; + private final FlyWheelState prepareState; + + FlyWheelState(double targetVoltage, FlyWheelState prepareState) { + this.targetVoltage = targetVoltage; + this.prepareState = prepareState; + } + + @Override + public double getTargetUnit() { + return targetVoltage; + } + + @Override + public SimpleMotorSubsystem.SimpleMotorState getPrepareState() { + return prepareState; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index f6dae857..b027865a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -12,13 +12,13 @@ import frc.trigon.lib.hardware.phoenix6.Phoenix6SignalThread; import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Signal; +import frc.trigon.lib.subsystems.MotorSubsystem; import frc.trigon.lib.utilities.flippable.Flippable; import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.lib.utilities.flippable.FlippableRotation2d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; -import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; import org.littletonrobotics.junction.AutoLogOutput; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index d04cce6d..bae85e8c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -4,6 +4,7 @@ import com.pathplanner.lib.config.PIDConstants; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -19,7 +20,7 @@ public class SwerveConstants { private static final int GYRO_ID = 0; private static final String GYRO_NAME = "SwerveGyro"; - static final Pigeon2Gyro GYRO = new Pigeon2Gyro(GYRO_ID, GYRO_NAME, RobotConstants.CANIVORE_NAME); + static final Pigeon2Gyro GYRO = new Pigeon2Gyro(GYRO_ID, GYRO_NAME); public static final int FRONT_LEFT_ID = 1, @@ -27,10 +28,10 @@ public class SwerveConstants { REAR_LEFT_ID = 3, REAR_RIGHT_ID = 4; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0, - FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = 0, - REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0, - REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = 0; + FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = -0.441, + FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = -0.285, + REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0.430, + REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = -0.038; private static final double//TODO:Calibrate FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2, FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2, @@ -102,9 +103,9 @@ public class SwerveConstants { private static void configureGyro() { final Pigeon2Configuration config = new Pigeon2Configuration(); //TODO:Calibrate - config.MountPose.MountPoseYaw = 0; - config.MountPose.MountPosePitch = 0; - config.MountPose.MountPoseRoll = 0; + config.MountPose.MountPoseYaw = Rotation2d.fromDegrees(-11.623535).getRotations(); + config.MountPose.MountPosePitch = Rotation2d.fromDegrees(-0.395508).getRotations(); + config.MountPose.MountPoseRoll = Rotation2d.fromDegrees(-0.043945).getRotations(); GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond());//IMPORTANT: Leave as lambda expression, method reference will crash code diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index e6d83a7d..910e7934 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -8,14 +8,13 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import frc.trigon.robot.constants.RobotConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; -import frc.trigon.robot.subsystems.swerve.SwerveConstants; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal; import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; import frc.trigon.lib.utilities.Conversions; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; public class SwerveModule { private final TalonFXMotor @@ -40,18 +39,16 @@ public class SwerveModule { * @param wheelDiameter the diameter of the wheel */ public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter) { - driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); - steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); - steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive"); + steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer"); + steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder"); this.wheelDiameter = wheelDiameter; configureHardware(offsetRotations); } public void setTargetState(SwerveModuleState targetState) { - if (willOptimize(targetState)) { - targetState.optimize(getCurrentSteerAngle()); - } + targetState.optimize(getCurrentSteerAngle()); this.targetState = targetState; setTargetSteerAngle(targetState.angle); @@ -132,9 +129,10 @@ public SwerveModulePosition getOdometryPosition(int odometryUpdateIndex) { ); } - private boolean willOptimize(SwerveModuleState state) { + private boolean + willOptimize(SwerveModuleState state) { final Rotation2d angularDelta = state.angle.minus(getCurrentSteerAngle()); - return Math.abs(angularDelta.getRadians()) > Math.PI / 2; + return true; } /** diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 5a0b67c1..d996529d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -15,8 +15,9 @@ public class SwerveModuleConstants { private static final double - DRIVE_MOTOR_GEAR_RATIO = 6.03,//R1: 7.03, R2: 6.03, R3: 5.27 - STEER_MOTOR_GEAR_RATIO = 287.0 / 11.0; + DRIVE_MOTOR_GEAR_RATIO = 6.03, + STEER_MOTOR_GEAR_RATIO = 26.09; + static final boolean ENABLE_FOC = true; private static final double @@ -65,7 +66,7 @@ static TalonFXConfiguration generateDriveMotorConfiguration() { config.Audio.BeepOnConfig = false; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.Feedback.SensorToMechanismRatio = DRIVE_MOTOR_GEAR_RATIO; final double driveMotorSlipCurrent = AutonomousConstants.ROBOT_CONFIG.moduleConfig.driveCurrentLimit;