diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f0411143..135bab2f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Units; /** @@ -50,15 +51,19 @@ public static class ElevatorConstants { public static final int COUNTS_PER_REV = 42; // may need to be updated public static final double INITIAL_POSITION = 0; - public static final double METERS_PER_ROTATION = 0.0; + public static final double METERS_PER_ROTATION = Units.Meters.convertFrom(1.751 * Math.PI/5, Units.Inches); // temp public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; + + public static final double POSITION_LOWER_LIMIT = 0.01; + public static final double POSITION_UPPER_LIMIT = 0.71; + public static final int MOTOR_STATOR_LIMIT = 60; // needs to be tuned // not tuned public static final double kS = 0; - public static final double kG = 0; + public static final double kG = 0.2; public static final double kV = 12/MAX_SPEED; // may need to be updated public static final double kA = 0; @@ -69,6 +74,7 @@ public static class ElevatorConstants { public static final double kMaxV = 0; public static final double kMaxA = 0; + public static final double MANUAL_MOVE_MOTOR_SPEED = 2.0; } public static class ArmConstants { @@ -78,7 +84,7 @@ public static class ArmConstants { public static final double INITIAL_POSITION = 0; // facing down public static final double POSITION_OFFSET = Math.PI/2; // difference between pid 0 (horizontal) and our 0 (down) - public static final double DEGREES_PER_ROTATION = 0.0; + public static final double DEGREES_PER_ROTATION = 1; // temp public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; @@ -95,6 +101,8 @@ public static class ArmConstants { public static final double kMaxV = 0; public static final double kMaxA = 0; + + public static final double MANUAL_MOVE_MOTOR_SPEED = 3.0; } public static class CollarConstants { @@ -107,6 +115,8 @@ public static class CollarConstants { public static final double MAX_SPEED = 0.0; public static final double SPEED_LOWER_LIMIT = 0.0; public static final double SPEED_UPPER_LIMIT = 0.0; + + public static final double OUTTAKE_MOTOR_SPEED = 4.0; } public static class RampConstants { diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b176a1db..ddfdbd72 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -7,7 +7,7 @@ public class Controls { public static JoystickVals adjustInputs(double x, double y, boolean slowmode) { - return adjustSlowmode(inputShape(x, y), slowmode); + return adjustSlowmode(inputShape(x, y), slowmode); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d10c4a60..5e315e54 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,17 +43,21 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; import frc.robot.commands.RotateToFaceReefCommand; -import frc.robot.generated.TunerConstants; +import frc.robot.generated.TunerConstantsDynamene; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.subsystems.collar.CollarSubsystem; +import frc.robot.subsystems.lifter.ArmSubsystem; +import frc.robot.subsystems.lifter.ElevatorIOHardware; +import frc.robot.subsystems.lifter.ElevatorSubsystem; import frc.robot.subsystems.lifter.LifterCommandFactory; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.commands.AutoAlignCommand; + public class RobotContainer { record JoystickVals(double x, double y) { } @@ -61,7 +65,7 @@ record JoystickVals(double x, double y) { } private RobotState currentState = RobotState.INTAKE; private RobotState nextState = RobotState.INTAKE; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12VoltsMps desired top speed *0.3 for pid tuning 9/15 + private double MaxSpeed = TunerConstantsDynamene.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12VoltsMps desired top speed *0.3 for pid tuning 9/15 private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity /* Setting up bindings for necessary control of the swerve drive platform */ @@ -70,7 +74,7 @@ record JoystickVals(double x, double y) { } private final CommandXboxController testJoystick = new CommandXboxController(OperatorConstants.kTestControllerPort); // test joystick - private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); // My drivetrain + private final CommandSwerveDrivetrain drivetrain = TunerConstantsDynamene.createDrivetrain(); // My drivetrain private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); private final VisionSubsystem m_vision = new VisionSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); @@ -78,6 +82,9 @@ record JoystickVals(double x, double y) { } private final RampSubsystem m_ramp = new RampSubsystem(); private final LifterCommandFactory m_lifter = new LifterCommandFactory(); + private final ElevatorSubsystem m_elevator = new ElevatorSubsystem(); + + private final ArmSubsystem m_arm = new ArmSubsystem(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband @@ -153,31 +160,61 @@ private void configureBindings() { // set next state, change LED colors accordingly - copilotJoystick.leftTrigger().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L4_CORAL;}), - m_ledSubsystem.runSolidRed())); - copilotJoystick.rightTrigger().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L3_CORAL;}), - m_ledSubsystem.runSolidOrange())); - copilotJoystick.leftBumper().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L2_CORAL;}), - m_ledSubsystem.runSolidYellow())); - copilotJoystick.rightBumper().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}), - m_ledSubsystem.runSolidWhite())); - copilotJoystick.a().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}), - m_ledSubsystem.runSolidPurple())); - copilotJoystick.y().onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}), - m_ledSubsystem.runSolidPink())); + //copilotJoystick.leftTrigger().onTrue( + //new ParallelCommandGroup( + //new InstantCommand(() -> {nextState = RobotState.L4_CORAL;}), + //m_ledSubsystem.runSolidRed())); + + //copilotJoystick.rightTrigger().onTrue( + //new ParallelCommandGroup( + //new InstantCommand(() -> {nextState = RobotState.L3_CORAL;}), + //m_ledSubsystem.runSolidOrange())); + + // copilotJoystick.leftBumper().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L2_CORAL;}), + // m_ledSubsystem.runSolidYellow())); + + // copilotJoystick.rightBumper().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}), + // m_ledSubsystem.runSolidWhite())); + + // copilotJoystick.a().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}), + // m_ledSubsystem.runSolidPurple())); + + // copilotJoystick.y().onTrue( + // new ParallelCommandGroup( + // new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}), + // m_ledSubsystem.runSolidPink())); + + //open loop control testing: + copilotJoystick.leftTrigger().whileTrue( + m_elevator.manualMoveCommand()); + + copilotJoystick.rightTrigger().whileTrue( + m_arm.manualMoveCommand()); + copilotJoystick.leftBumper().whileTrue( + m_elevator.manualMoveBackwardCommand()); + + copilotJoystick.rightBumper().whileTrue( + m_arm.manualMoveBackwardCommand()); + + copilotJoystick.a().whileTrue( + m_collar.runCollar()); + + copilotJoystick.y().whileTrue( + m_collar.runCollarBackward()); + + m_elevator.setDefaultCommand(m_elevator.keepInPlaceCommand()); + m_arm.setDefaultCommand(m_arm.keepInPlaceCommand()); + m_collar.setDefaultCommand(m_collar.runCollarOff()); + + + // run command runSolidGreen continuously if robot isWithinTarget() m_vision.isWithinTargetTrigger().whileTrue(m_ledSubsystem.runSolidGreen()); @@ -194,6 +231,19 @@ private void configureBindings() { testJoystick.leftBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); testJoystick.rightBumper().and(testJoystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); testJoystick.rightBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + //sysID routines for mechanisms + //(comment out other mechanisms while running one of them since they are all bound + //to the same buttons ) + //or change the bindings buttons haha + //run sysID routines for collar + + //run sysID routines for elevator + testJoystick.a().whileTrue(m_elevator.sysIdDynamic(Direction.kForward)); + testJoystick.b().whileTrue(m_elevator.sysIdDynamic(Direction.kReverse)); + testJoystick.x().whileTrue(m_elevator.sysIdQuasistatic(Direction.kForward)); + testJoystick.y().whileTrue(m_elevator.sysIdQuasistatic(Direction.kReverse)); + if (Utils.isSimulation()) { drivetrain.resetPose(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstantsCeridwen.java similarity index 99% rename from src/main/java/frc/robot/generated/TunerConstants.java rename to src/main/java/frc/robot/generated/TunerConstantsCeridwen.java index 17650848..f4d96625 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstantsCeridwen.java @@ -20,7 +20,7 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { +public class TunerConstantsCeridwen { // Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with the diff --git a/src/main/java/frc/robot/generated/TunerConstantsDynamene.java b/src/main/java/frc/robot/generated/TunerConstantsDynamene.java new file mode 100644 index 00000000..951f9c44 --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstantsDynamene.java @@ -0,0 +1,286 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstantsDynamene { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(2.66).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.746031746031747; + private static final double kSteerGearRatio = 21.428571428571427; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 13; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 2; + private static final int kFrontLeftSteerMotorId = 1; + private static final int kFrontLeftEncoderId = 3; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.284912109375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.375); + private static final Distance kFrontLeftYPos = Inches.of(10.375); + + // Front Right + private static final int kFrontRightDriveMotorId = 11; + private static final int kFrontRightSteerMotorId = 10; + private static final int kFrontRightEncoderId = 12; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.26171875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.375); + private static final Distance kFrontRightYPos = Inches.of(-10.375); + + // Back Left + private static final int kBackLeftDriveMotorId = 5; + private static final int kBackLeftSteerMotorId = 4; + private static final int kBackLeftEncoderId = 6; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.493408203125); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.375); + private static final Distance kBackLeftYPos = Inches.of(10.375); + + // Back Right + private static final int kBackRightDriveMotorId = 8; + private static final int kBackRightSteerMotorId = 7; + private static final int kBackRightEncoderId = 9; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.03662109375); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.375); + private static final Distance kBackRightYPos = Inches.of(-10.375); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index fe0d251c..26a69ebc 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -41,9 +41,8 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DrivetrainConstants; -import frc.robot.generated.TunerConstants; -import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.generated.TunerConstantsDynamene.TunerSwerveDrivetrain; /** diff --git a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java index 44c275e7..0f963878 100644 --- a/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java +++ b/src/main/java/frc/robot/subsystems/collar/CollarSubsystem.java @@ -1,7 +1,13 @@ package frc.robot.subsystems.collar; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; + +import frc.robot.Constants.CollarConstants; import frc.robot.RobotState; public class CollarSubsystem extends SubsystemBase { @@ -15,4 +21,25 @@ public CollarSubsystem() { public Command getCommand(RobotState targetRobotState) { return new WaitCommand(0); } + + public Command runCollarOff() { + return new RunCommand( + () -> m_IO.setVoltage(0), + this + ); + } + + public Command runCollar() { + return new RunCommand( + () -> m_IO.setVoltage(CollarConstants.OUTTAKE_MOTOR_SPEED), + this + ); + } + + public Command runCollarBackward() { + return new RunCommand( + () -> m_IO.setVoltage(-CollarConstants.OUTTAKE_MOTOR_SPEED), + this + ); + } } diff --git a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java index c66bc120..e2b044b6 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ArmSubsystem.java @@ -3,8 +3,11 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ArmConstants; @@ -36,6 +39,26 @@ public ArmSubsystem() { } // commands + public Command keepInPlaceCommand() { + return new RunCommand( + () -> m_IO.setVoltage(0), + this + ); + } + + public Command manualMoveCommand() { + return new RunCommand( + () -> m_IO.setVoltage(ArmConstants.MANUAL_MOVE_MOTOR_SPEED), + this + ); + } + public Command manualMoveBackwardCommand() { + return new RunCommand( + () -> m_IO.setVoltage(-ArmConstants.MANUAL_MOVE_MOTOR_SPEED), + this + ); + } + public Command moveToCommand(double targetPosition) { return moveToCommand(new TrapezoidProfile.State(targetPosition, 0)); } @@ -70,4 +93,10 @@ private void executeMoveTo() { m_IO.setVoltage(feedForwardPower + PIDPower); } + + @Override + public void periodic() { + SmartDashboard.putNumber("arm/position", m_IO.getPosition()); + SmartDashboard.putNumber("arm/velocity", m_IO.getVelocity()); + } } diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java index 1ac2d252..8089f4b1 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorIOHardware.java @@ -1,8 +1,11 @@ package frc.robot.subsystems.lifter; import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.StatusSignal; @@ -52,7 +55,20 @@ public void resetPosition() { } public void setVoltage(double value) { + value = MathUtil.clamp(value, -3, 3); + + // if position is too low, only run the elevator up + if (this.getPosition() < ElevatorConstants.POSITION_LOWER_LIMIT) { + value = MathUtil.clamp(value, 0, 1000); + } + // if position is too high, only run the elevator down (or in placd) + if (this.getPosition() > ElevatorConstants.POSITION_UPPER_LIMIT) { + value = MathUtil.clamp(value, -1000, ElevatorConstants.kG); + + } + m_elevatorMotor.setControl(new VoltageOut(value)); + SmartDashboard.putNumber("elevator/voltage", value); } public void requestClosedLoopPosition(double value) { diff --git a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java index db98b1f7..5b56998b 100644 --- a/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lifter/ElevatorSubsystem.java @@ -1,14 +1,36 @@ package frc.robot.subsystems.lifter; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.ElevatorConstants; + + public class ElevatorSubsystem extends SubsystemBase{ private final ElevatorIOHardware m_IO = new ElevatorIOHardware(); private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( @@ -35,6 +57,27 @@ public ElevatorSubsystem() { } // commands + public Command keepInPlaceCommand() { + return new RunCommand( + () -> m_IO.setVoltage(ElevatorConstants.kG), + this + ); + } + + public Command manualMoveCommand() { + return new RunCommand( + () -> m_IO.setVoltage(ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), + this + ); + } + + public Command manualMoveBackwardCommand() { + return new RunCommand( + () -> m_IO.setVoltage(-ElevatorConstants.MANUAL_MOVE_MOTOR_SPEED), + this + ); + } + public Command moveToCommand(double targetPosition) { return moveToCommand(new TrapezoidProfile.State(targetPosition, 0)); } @@ -69,4 +112,44 @@ private void executeMoveTo() { m_IO.setVoltage(feedForwardPower + PIDPower); } + + @Override + public void periodic() { + SmartDashboard.putNumber("elevator/position", m_IO.getPosition()); + SmartDashboard.putNumber("elevator/velocity", m_IO.getVelocity()); + } + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + + + private final SysIdRoutine m_sysIDRoutineElevatorRoutine = new SysIdRoutine( + new SysIdRoutine.Config(Volts.per(Seconds).of(0.5), Volts.of(3), Seconds.of(10)), + new SysIdRoutine.Mechanism( + output -> m_IO.setVoltage(output.in(Volts)), + log -> { + // Record a frame for the shooter motor. + log.motor("elevatorsysID") + .voltage( + m_appliedVoltage.mut_replace( + m_IO.getVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(m_IO.getPosition(), Meters)) + .linearVelocity( + m_velocity.mut_replace(m_IO.getVelocity(), MetersPerSecond)); + }, + this + ) + ); + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIDRoutineElevatorRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIDRoutineElevatorRoutine.dynamic(direction); + } }