From ebaf5e635086bb7c5be21a110c4a8fe41978782d Mon Sep 17 00:00:00 2001 From: YUANZL <69568611+hs150521@users.noreply.github.com> Date: Sat, 8 Feb 2025 20:36:15 +0800 Subject: [PATCH] Revert "[Refactor] change superstructure into robot container" --- src/main/java/frc/robot/RobotConstants.java | 692 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 22 +- .../commands/ElevatorZeroingCommand.java | 39 + .../robot/subsystems/elevator/ElevatorIO.java | 57 +- .../subsystems/elevator/ElevatorIOReal.java | 186 +++++ .../subsystems/elevator/ElevatorIOSim.java | 27 +- .../elevator/ElevatorIOTalonFX.java | 148 ---- .../elevator/ElevatorSubsystem.java | 37 +- .../endeffector/EndEffectorSubsystem.java | 14 - 9 files changed, 664 insertions(+), 558 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ElevatorZeroingCommand.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index df90fdb..fc90fe6 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -4,16 +4,20 @@ package frc.robot; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveModule.ClosedLoopOutputType; import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveModuleConstantsFactory; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.*; import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.utils.TunableNumber; import org.frcteam6941.swerve.SwerveSetpointGenerator.KinematicLimits; @@ -30,373 +34,387 @@ * wherever the constants are needed, to reduce verbosity. */ public final class RobotConstants { - // basic constants - public static final boolean disableHAL = false; - public static final double LOOPER_DT = 1 / 50.0; - public static final boolean TUNING = true; - // canbus name - public static String CAN_BUS_NAME = "rio"; - public static String CANIVORE_CAN_BUS_NAME = "10541Canivore0"; - // serial baud rate - public static int baudRate = 115200; - - /** - * Constants related to the robot's vision subsystem. - */ - public static class VisionConstants { - public static final SerialPort.Port serialPort = SerialPort.Port.kMXP; - } - - /** - * Constants related to the robot's indicators, such as LEDs. - */ - public static class IndicatorConstants { - // TODO:adapt when needed - public static final int LED_PORT = 0; - public static final int LED_BUFFER_LENGTH = 17; - } - - /** - * Constants specific to the swerve drivetrain configuration. - */ - public static class SwerveConstants { - // pigeon id - public static final int PIGEON_ID = 14; - - // swerve driving - /** - * Gearing between the drive motor output shaft and the wheel. - */ - public static final double DRIVE_GEAR_RATIO = 6.7460317460317460317460317460317; - /** - * Gearing between the steer motor output shaft and the azimuth gear. - */ - public static final double STEER_GEAR_RATIO = 21.428571428571428571428571428571; - - /** - * Max Voltage Output in voltage. - */ - public static final Measure MAX_VOLTAGE = Volts.of(12.0); - /** - * Radius of the wheel in meters. - */ - public static final Measure wheelRadius = Meters.of(0.0479); - /** - * The max speed of the swerve (should not larger than speedAt12Volts) - */ - public static final Measure maxSpeed = MetersPerSecond.of(4.5); - /** - * The max angular speed of the swerve. - */ - public static final Measure maxAngularRate = RotationsPerSecond.of(1.5 * Math.PI); - - // Kinematic limits for different driving modes - public static final KinematicLimits DRIVETRAIN_UNCAPPED = new KinematicLimits(maxSpeed.magnitude(), - 11.0, 1000.0); - public static final KinematicLimits DRIVETRAIN_SMOOTHED = new KinematicLimits(4.5, 30.0, 200.0); - public static final KinematicLimits DRIVETRAIN_LIMITED = new KinematicLimits(2.0, 10.0, 1200.0); - public static final KinematicLimits DRIVETRAIN_ROBOT_ORIENTED = new KinematicLimits(2.0, 5.0, 1500.0); - - public static final Measure speedAt12Volts = maxSpeed; - /** - * The stator current at which the wheels start to slip - */ - public static final Measure slipCurrent = Amps.of(150.0); - /** - * Theoretical free speed (m/s) at 12v applied output; - */ - - // ffw & wheel c - public static final Measure wheelCircumferenceMeters = Meters - .of(wheelRadius.magnitude() * 2 * Math.PI); - public static final SimpleMotorFeedforward DRIVETRAIN_FEEDFORWARD = new SimpleMotorFeedforward(0.69522, - 2.3623, 0.19367); - - public static final double statorCurrent = 110; - public static final double supplyCurrent = 50; - public static final double VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5; - public static final double deadband = maxSpeed.magnitude() * 0.05; - public static final double rotationalDeadband = maxAngularRate.magnitude() * 0.05; + // basic constants + public static final boolean disableHAL = false; + public static final double LOOPER_DT = 1 / 50.0; + public static final boolean TUNING = true; + // canbus name + public static String CAN_BUS_NAME = "rio"; + public static String CANIVORE_CAN_BUS_NAME = "10541Canivore0"; + // serial baud rate + public static int baudRate = 115200; /** - * Swerve steering gains + * Constants related to the robot's vision subsystem. */ - private static final Slot0Configs steerGains = new Slot0Configs().withKP(120)// 120 - .withKI(0.2)// 0.2 - .withKD(0.005)// 0.005 - .withKS(0).withKV(0).withKA(0); + public static class VisionConstants { + public static final SerialPort.Port serialPort = SerialPort.Port.kMXP; + } /** - * Swerve driving gains - */ - private static final Slot0Configs driveGains = new Slot0Configs().withKP(1).withKI(0).withKD(0) - .withKS(0).withKV(0.12).withKA(0); - private static final ClosedLoopOutputType steerClosedLoopOutput = 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 driveClosedLoopOutput = ClosedLoopOutputType.Voltage; - /** - * The closed-loop output type to use for the steer motors; This affects the - * PID/FF gains for the steer motors + * Constants related to the robot's indicators, such as LEDs. */ + public static class IndicatorConstants { + // TODO:adapt when needed + public static final int LED_PORT = 0; + public static final int LED_BUFFER_LENGTH = 17; + } - private static final double STEER_INERTIA = 0.00001; - /** - * Simulation only - */ - private static final double DRIVE_INERTIA = 0.001; - /** - * Simulation only - */ - private static final Measure steerFrictionVoltage = Volts.of(0.25); - /** - * Simulation only - */ - private static final Measure driveFrictionVoltage = Volts.of(0.25); /** - * Every 1 rotation of the azimuth results in COUPLE_RATIO drive motor turns; + * Constants specific to the swerve drivetrain configuration. */ - private static final double COUPLE_RATIO = 3.5; - private static final boolean STEER_MOTOR_REVERSED = true; - public static final LegacySwerveModuleConstantsFactory ConstantCreator = new LegacySwerveModuleConstantsFactory() - .withDriveMotorGearRatio(DRIVE_GEAR_RATIO).withSteerMotorGearRatio(STEER_GEAR_RATIO) - .withWheelRadius(wheelRadius.in(Inches)).withSlipCurrent(slipCurrent.magnitude()) - .withSteerMotorGains(steerGains).withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) - .withSpeedAt12VoltsMps(speedAt12Volts.magnitude()).withSteerInertia(STEER_INERTIA) - .withDriveInertia(DRIVE_INERTIA) - .withSteerFrictionVoltage(steerFrictionVoltage.magnitude()) - .withDriveFrictionVoltage(driveFrictionVoltage.magnitude()) - .withFeedbackSource(LegacySwerveModuleConstants.SteerFeedbackType.SyncCANcoder) - .withCouplingGearRatio(COUPLE_RATIO).withSteerMotorInverted(STEER_MOTOR_REVERSED); - private static final int FRONT_LEFT_DRIVE_MOTOR_ID = 8; - private static final int FRONT_LEFT_STEER_MOTOR_ID = 7; - private static final int FRONT_LEFT_ENCODER_ID = 20; - private static final double FRONT_LEFT_ENCODER_OFFSET = 0.4609375; // -0.37451171875;// - // 0.052955;//0.127686//0.5329550781 - private static final Measure frontLeftXPos = Meters.of(0.127); - private static final Measure frontLeftYPos = Meters.of(0.247); - public static final LegacySwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - FRONT_LEFT_STEER_MOTOR_ID, FRONT_LEFT_DRIVE_MOTOR_ID, FRONT_LEFT_ENCODER_ID, - FRONT_LEFT_ENCODER_OFFSET, frontLeftXPos.magnitude(), frontLeftYPos.magnitude(), false); - // Front Right - private static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 2; - private static final int FRONT_RIGHT_STEER_MOTOR_ID = 1; - private static final int FRONT_RIGHT_ENCODER_ID = 0; - private static final double FRONT_RIGHT_ENCODER_OFFSET = -0.3500976; // -0.119385;// - // 0.125685;//0.13623046875//0.117686//0.046875 - private static final Measure frontRightXPos = Meters.of(0.127); - private static final Measure frontRightYPos = Meters.of(-0.247); - public static final LegacySwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - FRONT_RIGHT_STEER_MOTOR_ID, FRONT_RIGHT_DRIVE_MOTOR_ID, FRONT_RIGHT_ENCODER_ID, - FRONT_RIGHT_ENCODER_OFFSET, frontRightXPos.magnitude(), frontRightYPos.magnitude(), - true); - // Back Left - private static final int BACK_LEFT_DRIVE_MOTOR_ID = 6; - private static final int BACK_LEFT_STEER_MOTOR_ID = 5; - private static final int BACK_LEFT_ENCODER_ID = 11; - private static final double BACK_LEFT_ENCODER_OFFSET = -0.420166; // -0.310302734375;// - // 0.773925;//-0.223//0.401611//0.77392578125 - private static final Measure backLeftXPos = Meters.of(-0.180); - private static final Measure backLeftYPos = Meters.of(0.247); - public static final LegacySwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( - BACK_LEFT_STEER_MOTOR_ID, BACK_LEFT_DRIVE_MOTOR_ID, BACK_LEFT_ENCODER_ID, - BACK_LEFT_ENCODER_OFFSET, backLeftXPos.magnitude(), backLeftYPos.magnitude(), false); - // Back Right - private static final int BACK_RIGHT_DRIVE_MOTOR_ID = 4; - private static final int BACK_RIGHT_STEER_MOTOR_ID = 3; - private static final int BACK_RIGHT_ENCODER_ID = 10; - private static final double BACK_RIGHT_ENCODER_OFFSET = -0.458007;// -0.151611;// - // 0.422119;//-0.5684550781//-0.064453//0.432279296875 - private static final Measure backRightXPos = Meters.of(-0.180); - private static final Measure backRightYPos = Meters.of(-0.247); - public static final LegacySwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - BACK_RIGHT_STEER_MOTOR_ID, BACK_RIGHT_DRIVE_MOTOR_ID, BACK_RIGHT_ENCODER_ID, - BACK_RIGHT_ENCODER_OFFSET, backRightXPos.magnitude(), backRightYPos.magnitude(), true); - // swervemodule - public static LegacySwerveModuleConstants[] modules = {FrontLeft, FrontRight, BackLeft, BackRight}; - public static final Translation2d[] modulePlacements = new Translation2d[]{ - new Translation2d(SwerveConstants.FrontLeft.LocationX, - SwerveConstants.FrontLeft.LocationY), - new Translation2d(SwerveConstants.FrontRight.LocationX, - SwerveConstants.FrontRight.LocationY), - new Translation2d(SwerveConstants.BackLeft.LocationX, - SwerveConstants.BackLeft.LocationY), - new Translation2d(SwerveConstants.BackRight.LocationX, - SwerveConstants.BackRight.LocationY)}; + public static class SwerveConstants { + // pigeon id + public static final int PIGEON_ID = 14; + + // swerve driving + /** + * Gearing between the drive motor output shaft and the wheel. + */ + public static final double DRIVE_GEAR_RATIO = 6.7460317460317460317460317460317; + /** + * Gearing between the steer motor output shaft and the azimuth gear. + */ + public static final double STEER_GEAR_RATIO = 21.428571428571428571428571428571; + + /** + * Max Voltage Output in voltage. + */ + public static final Measure MAX_VOLTAGE = Volts.of(12.0); + /** + * Radius of the wheel in meters. + */ + public static final Measure wheelRadius = Meters.of(0.0479); + /** + * The max speed of the swerve (should not larger than speedAt12Volts) + */ + public static final Measure maxSpeed = MetersPerSecond.of(4.5); + /** + * The max angular speed of the swerve. + */ + public static final Measure maxAngularRate = RotationsPerSecond.of(1.5 * Math.PI); + + // Kinematic limits for different driving modes + public static final KinematicLimits DRIVETRAIN_UNCAPPED = new KinematicLimits(maxSpeed.magnitude(), + 11.0, 1000.0); + public static final KinematicLimits DRIVETRAIN_SMOOTHED = new KinematicLimits(4.5, 30.0, 200.0); + public static final KinematicLimits DRIVETRAIN_LIMITED = new KinematicLimits(2.0, 10.0, 1200.0); + public static final KinematicLimits DRIVETRAIN_ROBOT_ORIENTED = new KinematicLimits(2.0, 5.0, 1500.0); + + public static final Measure speedAt12Volts = maxSpeed; + /** + * The stator current at which the wheels start to slip + */ + public static final Measure slipCurrent = Amps.of(150.0); + /** + * Theoretical free speed (m/s) at 12v applied output; + */ + + // ffw & wheel c + public static final Measure wheelCircumferenceMeters = Meters + .of(wheelRadius.magnitude() * 2 * Math.PI); + public static final SimpleMotorFeedforward DRIVETRAIN_FEEDFORWARD = new SimpleMotorFeedforward(0.69522, + 2.3623, 0.19367); + + public static final double statorCurrent = 110; + public static final double supplyCurrent = 50; + public static final double VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5; + public static final double deadband = maxSpeed.magnitude() * 0.05; + public static final double rotationalDeadband = maxAngularRate.magnitude() * 0.05; + + /** + * Swerve steering gains + */ + private static final Slot0Configs steerGains = new Slot0Configs().withKP(120)// 120 + .withKI(0.2)// 0.2 + .withKD(0.005)// 0.005 + .withKS(0).withKV(0).withKA(0); + + /** + * Swerve driving gains + */ + private static final Slot0Configs driveGains = new Slot0Configs().withKP(1).withKI(0).withKD(0) + .withKS(0).withKV(0.12).withKA(0); + private static final ClosedLoopOutputType steerClosedLoopOutput = 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 driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + /** + * The closed-loop output type to use for the steer motors; This affects the + * PID/FF gains for the steer motors + */ + + private static final double STEER_INERTIA = 0.00001; + /** + * Simulation only + */ + private static final double DRIVE_INERTIA = 0.001; + /** + * Simulation only + */ + private static final Measure steerFrictionVoltage = Volts.of(0.25); + /** + * Simulation only + */ + private static final Measure driveFrictionVoltage = Volts.of(0.25); + /** + * Every 1 rotation of the azimuth results in COUPLE_RATIO drive motor turns; + */ + private static final double COUPLE_RATIO = 3.5; + private static final boolean STEER_MOTOR_REVERSED = true; + public static final LegacySwerveModuleConstantsFactory ConstantCreator = new LegacySwerveModuleConstantsFactory() + .withDriveMotorGearRatio(DRIVE_GEAR_RATIO).withSteerMotorGearRatio(STEER_GEAR_RATIO) + .withWheelRadius(wheelRadius.in(Inches)).withSlipCurrent(slipCurrent.magnitude()) + .withSteerMotorGains(steerGains).withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(speedAt12Volts.magnitude()).withSteerInertia(STEER_INERTIA) + .withDriveInertia(DRIVE_INERTIA) + .withSteerFrictionVoltage(steerFrictionVoltage.magnitude()) + .withDriveFrictionVoltage(driveFrictionVoltage.magnitude()) + .withFeedbackSource(LegacySwerveModuleConstants.SteerFeedbackType.SyncCANcoder) + .withCouplingGearRatio(COUPLE_RATIO).withSteerMotorInverted(STEER_MOTOR_REVERSED); + private static final int FRONT_LEFT_DRIVE_MOTOR_ID = 8; + private static final int FRONT_LEFT_STEER_MOTOR_ID = 7; + private static final int FRONT_LEFT_ENCODER_ID = 20; + private static final double FRONT_LEFT_ENCODER_OFFSET = 0.4609375; // -0.37451171875;// + // 0.052955;//0.127686//0.5329550781 + private static final Measure frontLeftXPos = Meters.of(0.127); + private static final Measure frontLeftYPos = Meters.of(0.247); + public static final LegacySwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( + FRONT_LEFT_STEER_MOTOR_ID, FRONT_LEFT_DRIVE_MOTOR_ID, FRONT_LEFT_ENCODER_ID, + FRONT_LEFT_ENCODER_OFFSET, frontLeftXPos.magnitude(), frontLeftYPos.magnitude(), false); + // Front Right + private static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 2; + private static final int FRONT_RIGHT_STEER_MOTOR_ID = 1; + private static final int FRONT_RIGHT_ENCODER_ID = 0; + private static final double FRONT_RIGHT_ENCODER_OFFSET = -0.3500976; // -0.119385;// + // 0.125685;//0.13623046875//0.117686//0.046875 + private static final Measure frontRightXPos = Meters.of(0.127); + private static final Measure frontRightYPos = Meters.of(-0.247); + public static final LegacySwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( + FRONT_RIGHT_STEER_MOTOR_ID, FRONT_RIGHT_DRIVE_MOTOR_ID, FRONT_RIGHT_ENCODER_ID, + FRONT_RIGHT_ENCODER_OFFSET, frontRightXPos.magnitude(), frontRightYPos.magnitude(), + true); + // Back Left + private static final int BACK_LEFT_DRIVE_MOTOR_ID = 6; + private static final int BACK_LEFT_STEER_MOTOR_ID = 5; + private static final int BACK_LEFT_ENCODER_ID = 11; + private static final double BACK_LEFT_ENCODER_OFFSET = -0.420166; // -0.310302734375;// + // 0.773925;//-0.223//0.401611//0.77392578125 + private static final Measure backLeftXPos = Meters.of(-0.180); + private static final Measure backLeftYPos = Meters.of(0.247); + public static final LegacySwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( + BACK_LEFT_STEER_MOTOR_ID, BACK_LEFT_DRIVE_MOTOR_ID, BACK_LEFT_ENCODER_ID, + BACK_LEFT_ENCODER_OFFSET, backLeftXPos.magnitude(), backLeftYPos.magnitude(), false); + // Back Right + private static final int BACK_RIGHT_DRIVE_MOTOR_ID = 4; + private static final int BACK_RIGHT_STEER_MOTOR_ID = 3; + private static final int BACK_RIGHT_ENCODER_ID = 10; + private static final double BACK_RIGHT_ENCODER_OFFSET = -0.458007;// -0.151611;// + // 0.422119;//-0.5684550781//-0.064453//0.432279296875 + private static final Measure backRightXPos = Meters.of(-0.180); + private static final Measure backRightYPos = Meters.of(-0.247); + public static final LegacySwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( + BACK_RIGHT_STEER_MOTOR_ID, BACK_RIGHT_DRIVE_MOTOR_ID, BACK_RIGHT_ENCODER_ID, + BACK_RIGHT_ENCODER_OFFSET, backRightXPos.magnitude(), backRightYPos.magnitude(), true); + // swervemodule + public static LegacySwerveModuleConstants[] modules = { FrontLeft, FrontRight, BackLeft, BackRight }; + public static final Translation2d[] modulePlacements = new Translation2d[] { + new Translation2d(SwerveConstants.FrontLeft.LocationX, + SwerveConstants.FrontLeft.LocationY), + new Translation2d(SwerveConstants.FrontRight.LocationX, + SwerveConstants.FrontRight.LocationY), + new Translation2d(SwerveConstants.BackLeft.LocationX, + SwerveConstants.BackLeft.LocationY), + new Translation2d(SwerveConstants.BackRight.LocationX, + SwerveConstants.BackRight.LocationY) }; + + /** + * Constants for the heading controller used in the swerve drivetrain. + */ + public static class headingController { + public static final frc.robot.utils.TunableNumber HEADING_KP = new frc.robot.utils.TunableNumber( + "HEADING PID/kp", 0.09); + public static final frc.robot.utils.TunableNumber HEADING_KI = new frc.robot.utils.TunableNumber( + "HEADING PID/ki", 0.000); + public static final frc.robot.utils.TunableNumber HEADING_KD = new frc.robot.utils.TunableNumber( + "HEADING PID/kd", 0.004); + public static final frc.robot.utils.TunableNumber MAX_ERROR_CORRECTION_ANGLE = new frc.robot.utils.TunableNumber( + "HEADING/Max Error Correction Angle", 120.0); + } + + /** + * Constants for the steer motor gains in the swerve drivetrain. + */ + public static class steerGainsClass { + public static final TunableNumber STEER_KP = new TunableNumber("STEER PID/kp", 120); + public static final TunableNumber STEER_KI = new TunableNumber("STEER PID/ki", 0.2); + public static final TunableNumber STEER_KD = new TunableNumber("STEER PID/kd", 0.005); + public static final TunableNumber STEER_KA = new TunableNumber("STEER PID/ka", 0); + public static final TunableNumber STEER_KV = new TunableNumber("STEER PID/kv", 0); + public static final TunableNumber STEER_KS = new TunableNumber("STEER PID/ks", 0); + } + + /** + * Constants for the drive motor gains in the swerve drivetrain. + */ + public static class driveGainsClass { + public static final TunableNumber DRIVE_KP = new TunableNumber("DRIVE PID/kp", 0.03); + public static final TunableNumber DRIVE_KI = new TunableNumber("DRIVE PID/ki", 0); + public static final TunableNumber DRIVE_KD = new TunableNumber("DRIVE PID/kd", 0.0001); + public static final TunableNumber DRIVE_KA = new TunableNumber("DRIVE PID/ka", 0); + public static final TunableNumber DRIVE_KV = new TunableNumber("DRIVE PID/kv", 0.12); + public static final TunableNumber DRIVE_KS = new TunableNumber("DRIVE PID/ks", 0.045); + } - /** - * Constants for the heading controller used in the swerve drivetrain. - */ - public static class headingController { - public static final frc.robot.utils.TunableNumber HEADING_KP = new frc.robot.utils.TunableNumber( - "HEADING PID/kp", 0.09); - public static final frc.robot.utils.TunableNumber HEADING_KI = new frc.robot.utils.TunableNumber( - "HEADING PID/ki", 0.000); - public static final frc.robot.utils.TunableNumber HEADING_KD = new frc.robot.utils.TunableNumber( - "HEADING PID/kd", 0.004); - public static final frc.robot.utils.TunableNumber MAX_ERROR_CORRECTION_ANGLE = new frc.robot.utils.TunableNumber( - "HEADING/Max Error Correction Angle", 120.0); } /** - * Constants for the steer motor gains in the swerve drivetrain. + * Constants specific to the reef aim mechanism. */ - public static class steerGainsClass { - public static final TunableNumber STEER_KP = new TunableNumber("STEER PID/kp", 120); - public static final TunableNumber STEER_KI = new TunableNumber("STEER PID/ki", 0.2); - public static final TunableNumber STEER_KD = new TunableNumber("STEER PID/kd", 0.005); - public static final TunableNumber STEER_KA = new TunableNumber("STEER PID/ka", 0); - public static final TunableNumber STEER_KV = new TunableNumber("STEER PID/kv", 0); - public static final TunableNumber STEER_KS = new TunableNumber("STEER PID/ks", 0); + public static final class ReefAimConstants { + public static final Transform2d tagLeftToRobot = new Transform2d(); // vec(robot) - vec(tag) when + // shooting left coral + public static final Transform2d tagRightToRobot = new Transform2d(); } /** - * Constants for the drive motor gains in the swerve drivetrain. + * Constants related to the beambreak subsystem. */ - public static class driveGainsClass { - public static final TunableNumber DRIVE_KP = new TunableNumber("DRIVE PID/kp", 0.03); - public static final TunableNumber DRIVE_KI = new TunableNumber("DRIVE PID/ki", 0); - public static final TunableNumber DRIVE_KD = new TunableNumber("DRIVE PID/kd", 0.0001); - public static final TunableNumber DRIVE_KA = new TunableNumber("DRIVE PID/ka", 0); - public static final TunableNumber DRIVE_KV = new TunableNumber("DRIVE PID/kv", 0.12); - public static final TunableNumber DRIVE_KS = new TunableNumber("DRIVE PID/ks", 0.045); + public static class BeamBreakConstants { + public static final int ENDEFFECTOR_MIDDLE_BEAMBREAK_ID = 1; + public static final int ENDEFFECTOR_EDGE_BEAMBREAK_ID = 3; + public static final int INTAKER_BEAMBREAK_ID = 0; } - } - - /** - * Constants specific to the reef aim mechanism. - */ - public static final class ReefAimConstants { - public static final Transform2d tagLeftToRobot = new Transform2d(); // vec(robot) - vec(tag) when - // shooting left coral - public static final Transform2d tagRightToRobot = new Transform2d(); - } - - /** - * Constants related to the beambreak subsystem. - */ - public static class BeamBreakConstants { - public static final int ENDEFFECTOR_MIDDLE_BEAMBREAK_ID = 1; - public static final int ENDEFFECTOR_EDGE_BEAMBREAK_ID = 3; - public static final int INTAKER_BEAMBREAK_ID = 0; - } - - /** - * Constants related to the endeffector subsystem. - */ - public static class EndEffectorConstants { - public static final int ENDEFFECTOR_MOTOR_ID = 31; - - public static final int STATOR_CURRENT_LIMIT_AMPS = 60; - public static final int SUPPLY_CURRENT_LIMIT_AMPS = 20; - public static final boolean IS_BRAKE = true; - public static final boolean IS_INVERT = false; - public static final double REDUCTION = 1; - - public static final TunableNumber INTAKE_RPS = new TunableNumber("EndEffector/indexRPS", -40); - public static final TunableNumber TRANSFER_RPS = new TunableNumber("EndEffector/transferRPS", -25); - public static final TunableNumber HOLD_RPS = new TunableNumber("EndEffector/holdRPS", 0.0); - public static final TunableNumber SHOOT_RPS = new TunableNumber("EndEffector/shootRPS", -80); - public static final TunableNumber IDLE_RPS = new TunableNumber("EndEffector/idleRPS", -0); - /** - * Constants for the endeffector motor gains. + * Constants related to the endeffector subsystem. */ - public static class EndEffectorGainsClass { - public static final TunableNumber ENDEFFECTOR_KP = new TunableNumber("ENDEFFECTOR PID/kp", 0.2); - public static final TunableNumber ENDEFFECTOR_KI = new TunableNumber("ENDEFFECTOR PID/ki", 0); - public static final TunableNumber ENDEFFECTOR_KD = new TunableNumber("ENDEFFECTOR PID/kd", - 0.001); - public static final TunableNumber ENDEFFECTOR_KA = new TunableNumber("ENDEFFECTOR PID/ka", - 0.0037512677); - public static final TunableNumber ENDEFFECTOR_KV = new TunableNumber("ENDEFFECTOR PID/kv", - 0.113);// 0.107853495 - public static final TunableNumber ENDEFFECTOR_KS = new TunableNumber("ENDEFFECTOR PID/ks", - 0.28475008); + public static class EndEffectorConstants { + public static final int ENDEFFECTOR_MOTOR_ID = 31; + + public static final int STATOR_CURRENT_LIMIT_AMPS = 60; + public static final int SUPPLY_CURRENT_LIMIT_AMPS = 20; + public static final boolean IS_BRAKE = true; + public static final boolean IS_INVERT = false; + public static final double REDUCTION = 1; + + public static final TunableNumber INTAKE_RPS = new TunableNumber("EndEffector/indexRPS", -40); + public static final TunableNumber TRANSFER_RPS = new TunableNumber("EndEffector/transferRPS", -25); + public static final TunableNumber HOLD_RPS = new TunableNumber("EndEffector/holdRPS", 0.0); + public static final TunableNumber SHOOT_RPS = new TunableNumber("EndEffector/shootRPS", -80); + public static final TunableNumber IDLE_RPS = new TunableNumber("EndEffector/idleRPS", -0); + + /** + * Constants for the endeffector motor gains. + */ + public static class EndEffectorGainsClass { + public static final TunableNumber ENDEFFECTOR_KP = new TunableNumber("ENDEFFECTOR PID/kp", 0.2); + public static final TunableNumber ENDEFFECTOR_KI = new TunableNumber("ENDEFFECTOR PID/ki", 0); + public static final TunableNumber ENDEFFECTOR_KD = new TunableNumber("ENDEFFECTOR PID/kd", + 0.001); + public static final TunableNumber ENDEFFECTOR_KA = new TunableNumber("ENDEFFECTOR PID/ka", + 0.0037512677); + public static final TunableNumber ENDEFFECTOR_KV = new TunableNumber("ENDEFFECTOR PID/kv", + 0.113);// 0.107853495 + public static final TunableNumber ENDEFFECTOR_KS = new TunableNumber("ENDEFFECTOR PID/ks", + 0.28475008); + } } - } - - - /** - * Constants related to the robot's intake subsystem. - */ - public static class intakeConstants { - public static final int INTAKER_MOTOR_ID = 15; - public static final int INTAKER_PIVOT_MOTOR_ID = 16; - public static final TunableNumber INTAKE_VELOCITY = new TunableNumber("Intake/intakeVelocity", 600); - public static final TunableNumber INTAKE_VOLTAGE = new TunableNumber("Intake/intakeVoltage", 3.0); - public static double PIVOT_RATIO = 1; - public static Rotation2d RETRACTED_ANGLE = Rotation2d.fromDegrees(90); - public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(120); - public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); /** - * Constants for the intake pivot motor gains in the intake subsystem. + * Constants related to the robot's intake subsystem. */ - public static class intakePivotGainsClass { - public static final TunableNumber INTAKE_PIVOT_KP = new TunableNumber("INTAKE_PIVOT PID/kp", - 0.03); - public static final TunableNumber INTAKE_PIVOT_KI = new TunableNumber("INTAKE_PIVOT PID/ki", 0); - public static final TunableNumber INTAKE_PIVOT_KD = new TunableNumber("INTAKE_PIVOT PID/kd", - 0.0001); - public static final TunableNumber INTAKE_PIVOT_KA = new TunableNumber("INTAKE_PIVOT PID/ka", 0); - public static final TunableNumber INTAKE_PIVOT_KV = new TunableNumber("INTAKE_PIVOT PID/kv", - 0.12); - public static final TunableNumber INTAKE_PIVOT_KS = new TunableNumber("INTAKE_PIVOT PID/ks", - 0.045); + public static class intakeConstants { + public static final int INTAKER_MOTOR_ID = 15; + public static final int INTAKER_PIVOT_MOTOR_ID = 16; + public static double PIVOT_RATIO = 1; + public static Rotation2d RETRACTED_ANGLE = Rotation2d.fromDegrees(90); + public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(120); + public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); + + public static final TunableNumber INTAKE_VELOCITY = new TunableNumber("Intake/intakeVelocity", 600); + public static final TunableNumber INTAKE_VOLTAGE = new TunableNumber("Intake/intakeVoltage", 3.0); + + /** + * Constants for the intake pivot motor gains in the intake subsystem. + */ + public static class intakePivotGainsClass { + public static final TunableNumber INTAKE_PIVOT_KP = new TunableNumber("INTAKE_PIVOT PID/kp", + 0.03); + public static final TunableNumber INTAKE_PIVOT_KI = new TunableNumber("INTAKE_PIVOT PID/ki", 0); + public static final TunableNumber INTAKE_PIVOT_KD = new TunableNumber("INTAKE_PIVOT PID/kd", + 0.0001); + public static final TunableNumber INTAKE_PIVOT_KA = new TunableNumber("INTAKE_PIVOT PID/ka", 0); + public static final TunableNumber INTAKE_PIVOT_KV = new TunableNumber("INTAKE_PIVOT PID/kv", + 0.12); + public static final TunableNumber INTAKE_PIVOT_KS = new TunableNumber("INTAKE_PIVOT PID/ks", + 0.045); + } + + /** + * Constants for the intake motor gains in the intake subsystem. + */ + public static class intakeGainsClass { + public static final TunableNumber INTAKE_KP = new TunableNumber("INTAKE PID/kp", 0.03); + public static final TunableNumber INTAKE_KI = new TunableNumber("INTAKE PID/ki", 0); + public static final TunableNumber INTAKE_KD = new TunableNumber("INTAKE PID/kd", 0.0001); + public static final TunableNumber INTAKE_KA = new TunableNumber("INTAKE PID/ka", 0); + public static final TunableNumber INTAKE_KV = new TunableNumber("INTAKE PID/kv", 0.12); + public static final TunableNumber INTAKE_KS = new TunableNumber("INTAKE PID/ks", 0.045); + } } /** - * Constants for the intake motor gains in the intake subsystem. + * Constants related to the elevator subsystem. */ - public static class intakeGainsClass { - public static final TunableNumber INTAKE_KP = new TunableNumber("INTAKE PID/kp", 0.03); - public static final TunableNumber INTAKE_KI = new TunableNumber("INTAKE PID/ki", 0); - public static final TunableNumber INTAKE_KD = new TunableNumber("INTAKE PID/kd", 0.0001); - public static final TunableNumber INTAKE_KA = new TunableNumber("INTAKE PID/ka", 0); - public static final TunableNumber INTAKE_KV = new TunableNumber("INTAKE PID/kv", 0.12); - public static final TunableNumber INTAKE_KS = new TunableNumber("INTAKE PID/ks", 0.045); + public static class ElevatorConstants { + public static final int LEFT_ELEVATOR_MOTOR_ID = 50; + public static final int RIGHT_ELEVATOR_MOTOR_ID = 51; + + public static final double ELEVATOR_SPOOL_DIAMETER = 0.04 + 0.003; //0.04m for spool diameter, 0.003 for rope diameter + public static final double ELEVATOR_GEAR_RATIO = 3.0; + public static final double DEADZONE_DISTANCE = 0.0; + + public static final TunableNumber motionAcceleration = new TunableNumber("Elevator/MotionAcceleration", + 200); + public static final TunableNumber motionCruiseVelocity = new TunableNumber( + "Elevator/MotionCruiseVelocity", 100); + public static final TunableNumber motionJerk = new TunableNumber("Elevator/MotionJerk", 0.0); + + public static final TunableNumber MAX_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/max", + 1.57); + public static final TunableNumber IDLE_EXTENSION_METERS = new TunableNumber( + "ELEVATOR GOALS/idle", 0); + public static final TunableNumber L1_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L1", + 0.45); + public static final TunableNumber L2_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L2", + 0.53); + public static final TunableNumber L3_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L3", + 0.95);// 0.107853495 + public static final TunableNumber L4_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L4", + 1.55); + public static final TunableNumber ELEVATOR_ZEROING_CURRENT = new TunableNumber("Elevator zeroing current", + 20); + } - } - - /** - * Constants related to the elevator subsystem. - */ - public static final class ElevatorConstants { - public static final int LEFT_ELEVATOR_MOTOR_ID = 50; - public static final int RIGHT_ELEVATOR_MOTOR_ID = 51; - - public static final double ELEVATOR_SPOOL_DIAMETER = 0.04 + 0.003; //0.04m for spool diameter, 0.003 for rope diameter - public static final double ELEVATOR_GEAR_RATIO = 3.0; - public static final boolean leftMotorClockwise = true; - public static final double elevatorMotorRPS = 70; // In rotation * s^{-1} - public static final double elevatorMotorAccel = 160;// In rotation * s^{-2} - public static final double ELEVATOR_ZEROING_CURRENT = 15; //in amps - // Constants for elevator level positions relative to starting position. Todo: tune all constants below - public static final double[] Position = new double[]{ - 0.0, //lowest - 0.0, //L1 - 0.8 - 0.105, //L2 - 1.22 - 0.105, //L3 - 1.80 - 0.105, //L4 - 1.80 //highest - }; - /** - * Constants for the elevator motor gains. - */ - public static class ElevatorGainsClass { - public static final TunableNumber ELEVATOR_KP = new TunableNumber("ELEVATOR PID/kp", 0.2); - public static final TunableNumber ELEVATOR_KI = new TunableNumber("ELEVATOR PID/ki", 0); - public static final TunableNumber ELEVATOR_KD = new TunableNumber("ELEVATOR PID/kd", 0.001); - public static final TunableNumber ELEVATOR_KA = new TunableNumber("ELEVATOR PID/ka", 0.0037512677); - public static final TunableNumber ELEVATOR_KV = new TunableNumber("ELEVATOR PID/kv", 0.113);// 0.107853495 - public static final TunableNumber ELEVATOR_KS = new TunableNumber("ELEVATOR PID/ks", 0.28475008); + + /** + * Constants for the elevator motor gains. + */ + public static class ElevatorGainsClass { + public static final TunableNumber ELEVATOR_KP = new TunableNumber("ELEVATOR PID/kp", 15); + public static final TunableNumber ELEVATOR_KI = new TunableNumber("ELEVATOR PID/ki", 0); + public static final TunableNumber ELEVATOR_KD = new TunableNumber("ELEVATOR PID/kd", 0); + public static final TunableNumber ELEVATOR_KA = new TunableNumber("ELEVATOR PID/ka", + 0); + public static final TunableNumber ELEVATOR_KV = new TunableNumber("ELEVATOR PID/kv", 0);// 0.107853495 + public static final TunableNumber ELEVATOR_KS = new TunableNumber("ELEVATOR PID/ks", + 0); + public static final TunableNumber ELEVATOR_KG = new TunableNumber("ELEVATOR PID/kg", 0.3); + } } - } -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7d28ae..dfd5f9b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,11 +17,11 @@ import frc.robot.auto.basics.AutoActions; import frc.robot.commands.*; import frc.robot.display.Display; -//import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.apriltagvision.AprilTagVision; import frc.robot.subsystems.apriltagvision.AprilTagVisionIONorthstar; import frc.robot.subsystems.beambreak.BeambreakIOReal; -import frc.robot.subsystems.elevator.ElevatorIOTalonFX; +import frc.robot.subsystems.elevator.ElevatorIOReal; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.endeffector.EndEffectorIOReal; import frc.robot.subsystems.endeffector.EndEffectorSubsystem; @@ -61,10 +61,10 @@ public class RobotContainer { new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 1)); Swerve swerve = Swerve.getInstance(); Display display = Display.getInstance(); - ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX()); + ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOReal()); EndEffectorSubsystem endEffectorSubsystem = new EndEffectorSubsystem(new EndEffectorIOReal(), new BeambreakIOReal(ENDEFFECTOR_MIDDLE_BEAMBREAK_ID), new BeambreakIOReal(ENDEFFECTOR_EDGE_BEAMBREAK_ID)); - //Superstructure superstructure = new Superstructure(endEffectorSubsystem); + Superstructure superstructure = new Superstructure(endEffectorSubsystem); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -132,11 +132,17 @@ private void configureOperatorBindings(CommandXboxController operatorController) private void configureTesterBindings(CommandXboxController controller) { //test of endeffector state machine new Trigger(controller.leftBumper()) - .onTrue(endEffectorSubsystem.setWantedSuperStateCommand(EndEffectorSubsystem.WantedState.FUNNEL_INTAKE)); - new Trigger(controller.rightTrigger()) - .onTrue(endEffectorSubsystem.setWantedSuperStateCommand(EndEffectorSubsystem.WantedState.SHOOT)); + .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.INTAKE_CORAL_FUNNEL)); + new Trigger(controller.rightBumper()) + .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.SHOOT_CORAL)); new Trigger(controller.start()) - .onTrue(endEffectorSubsystem.setWantedSuperStateCommand(EndEffectorSubsystem.WantedState.IDLE)); + .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.STOPPED)); + //test of elevator heights + controller.a().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L1_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L1_EXTENSION_METERS.get()))); + controller.b().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L2_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L2_EXTENSION_METERS.get()))); + controller.x().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L3_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L3_EXTENSION_METERS.get()))); + controller.y().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L4_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L4_EXTENSION_METERS.get()))); + controller.povDown().onTrue(new ElevatorZeroingCommand(elevatorSubsystem)); } /** diff --git a/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java b/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java new file mode 100644 index 0000000..2342c5e --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.subsystems.elevator.*; +import edu.wpi.first.math.MathUtil; +import frc.robot.RobotConstants; + + +public class ElevatorZeroingCommand extends Command { + private ElevatorSubsystem elevator; + + public ElevatorZeroingCommand(ElevatorSubsystem elevator) { + this.elevator = elevator; + } + + @Override + public void initialize() { + + } + + @Override + public void execute(){ + elevator.setVoltage(-1); + } + + @Override + public boolean isFinished() { + return Math.abs(elevator.getLeaderCurrent()) > RobotConstants.ElevatorConstants.ELEVATOR_ZEROING_CURRENT.get(); + } + + @Override + public void end(boolean interrupted) { + elevator.resetPosition(); + elevator.setVoltage(0); + } + + +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index f525ccd..f9d1e3d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -1,46 +1,37 @@ package frc.robot.subsystems.elevator; -import edu.wpi.first.units.*; import org.littletonrobotics.junction.AutoLog; -import static edu.wpi.first.units.Units.*; - public interface ElevatorIO { - void updateInputs(ElevatorIOInputs inputs); - - void setElevatorDirectVoltage(double volts); - - void setElevatorTarget(double meters); + @AutoLog + class ElevatorIOInputs { + public double positionMeters = 0.0; + public double velocityMetersPerSec = 0.0; + public double motionMagicVelocityTarget = 0.0; + public double motionMagicPositionTarget = 0.0; + public double setpointMeters = 0.0; + public double[] appliedVolts = new double[] {}; // {leader, follower} + public double[] statorCurrentAmps = new double[] {}; // {leader, follower} + public double[] supplyCurrentAmps = new double[] {}; // {leader, follower} + public double[] tempCelsius = new double[] {}; // {leader, follower} + // public double acceleration = 0.0; may add later + } - void resetElevatorPosition(); + public default void updateInputs(ElevatorIOInputs inputs) {} - double getElevatorVelocity(); + public default void setVoltage(double voltage) {} - double getElevatorPosition(); + public default void setPosition(double meters) {} - boolean isNearExtension(double expected); + public default void stop() { + setVoltage(0.0); + } - boolean isCurrentMax(double expected); + public default void resetEncoder(double position) {} - @AutoLog - class ElevatorIOInputs { - public Measure leftElevatorVelocity = RadiansPerSecond.zero(); - public Measure leftElevatorPosition = Radians.zero(); - public Measure leftElevatorAppliedVoltage = Volts.zero(); - public Measure leftElevatorSupplyCurrent = Amps.zero(); - - public Measure rightElevatorVelocity = RadiansPerSecond.zero(); - public Measure rightElevatorPosition = Radians.zero(); - public Measure rightElevatorAppliedVoltage = Volts.zero(); - public Measure rightElevatorSupplyCurrent = Amps.zero(); - - public Measure targetElevatorVelocity = RadiansPerSecond.zero(); - - public double ElevatorKP = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KP.get(); - public double ElevatorKI = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KI.get(); - public double ElevatorKD = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KD.get(); - public double ElevatorKA = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KA.get(); - public double ElevatorKV = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KV.get(); - public double ElevatorKS = frc.robot.RobotConstants.ElevatorConstants.ElevatorGainsClass.ELEVATOR_KS.get(); + public default void resetEncoder() { + resetEncoder(0.0); } + + public default void resetPosition() {} } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java new file mode 100644 index 0000000..c238d3c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -0,0 +1,186 @@ +package frc.robot.subsystems.elevator; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.*; +import frc.robot.RobotConstants; + +import static frc.robot.RobotConstants.*; +import static frc.robot.RobotConstants.ElevatorConstants.*; +import static frc.robot.RobotConstants.ElevatorGainsClass.*; + +public class ElevatorIOReal implements ElevatorIO { + /* Hardware */ + private final TalonFX leader; + private final TalonFX follower; + + /* Configurators */ + private TalonFXConfigurator leaderConfigurator; + private TalonFXConfigurator followerConfigurator; + + /* Configs */ + private final CurrentLimitsConfigs currentLimitsConfigs; + private final MotorOutputConfigs leaderMotorConfigs; + private final MotorOutputConfigs followerMotorConfigs; + private final Slot0Configs slot0Configs; + private final MotionMagicConfigs motionMagicConfigs; + + private final MotionMagicVoltage motionRequest = new MotionMagicVoltage(0.0).withEnableFOC(true); + private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + + private final StatusSignal voltageLeft; + private final StatusSignal voltageRight; + private final StatusSignal statorLeft; + private final StatusSignal statorRight; + private final StatusSignal supplyLeft; + private final StatusSignal supplyRight; + private final StatusSignal tempLeft; + private final StatusSignal tempRight; + + public ElevatorIOReal() { + this.leader = new TalonFX(LEFT_ELEVATOR_MOTOR_ID, CANIVORE_CAN_BUS_NAME); + this.follower = new TalonFX(RIGHT_ELEVATOR_MOTOR_ID, CANIVORE_CAN_BUS_NAME); + + this.leaderConfigurator = leader.getConfigurator(); + this.followerConfigurator = follower.getConfigurator(); + + currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs.StatorCurrentLimitEnable = true; + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + currentLimitsConfigs.StatorCurrentLimit = 80.0; + currentLimitsConfigs.SupplyCurrentLimit = 30.0; + + leaderMotorConfigs = new MotorOutputConfigs(); + leaderMotorConfigs.NeutralMode = NeutralModeValue.Brake; + leaderMotorConfigs.Inverted = InvertedValue.Clockwise_Positive; + followerMotorConfigs = new MotorOutputConfigs(); + followerMotorConfigs.NeutralMode = NeutralModeValue.Brake; + + motionMagicConfigs = new MotionMagicConfigs(); + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); + + slot0Configs = new Slot0Configs(); + slot0Configs.kA = ELEVATOR_KA.get(); + slot0Configs.kS = ELEVATOR_KS.get(); + slot0Configs.kV = ELEVATOR_KV.get(); + slot0Configs.kP = ELEVATOR_KP.get(); + slot0Configs.kI = ELEVATOR_KI.get(); + slot0Configs.kD = ELEVATOR_KD.get(); + + leader.setPosition(0.0); + follower.setPosition(0.0); + + leaderConfigurator.apply(currentLimitsConfigs); + leaderConfigurator.apply(leaderMotorConfigs); + leaderConfigurator.apply(slot0Configs); + leaderConfigurator.apply(motionMagicConfigs); + followerConfigurator.apply(currentLimitsConfigs); + followerConfigurator.apply(followerMotorConfigs); + followerConfigurator.apply(slot0Configs); + followerConfigurator.apply(motionMagicConfigs); + + voltageLeft = leader.getSupplyVoltage(); + voltageRight = follower.getSupplyVoltage(); + statorLeft = leader.getStatorCurrent(); + statorRight = follower.getStatorCurrent(); + supplyLeft = leader.getSupplyCurrent(); + supplyRight = follower.getSupplyCurrent(); + tempLeft = leader.getDeviceTemp(); + tempRight = follower.getDeviceTemp(); + + follower.setControl(new Follower(leader.getDeviceID(), true)); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + BaseStatusSignal.refreshAll( + voltageLeft, voltageRight, + statorLeft, statorRight, + supplyLeft, supplyRight, + tempLeft, tempRight + ); + + inputs.positionMeters = getHeight(); + inputs.velocityMetersPerSec = getVelocity(); + inputs.motionMagicVelocityTarget = rotationsToMeters(leader.getClosedLoopReferenceSlope().getValue()); + inputs.motionMagicPositionTarget = rotationsToMeters(leader.getClosedLoopReference().getValue()); + inputs.appliedVolts = new double[] { voltageLeft.getValueAsDouble(), voltageRight.getValueAsDouble() }; + inputs.statorCurrentAmps = new double[] { statorLeft.getValueAsDouble(), statorRight.getValueAsDouble() }; + inputs.supplyCurrentAmps = new double[] { supplyLeft.getValueAsDouble(), supplyRight.getValueAsDouble() }; + inputs.tempCelsius = new double[] { tempLeft.getValueAsDouble(), tempRight.getValueAsDouble() }; + + if (RobotConstants.TUNING) { + slot0Configs.kA = ELEVATOR_KA.get(); + slot0Configs.kS = ELEVATOR_KS.get(); + slot0Configs.kV = ELEVATOR_KV.get(); + slot0Configs.kP = ELEVATOR_KP.get(); + slot0Configs.kI = ELEVATOR_KI.get(); + slot0Configs.kD = ELEVATOR_KD.get(); + slot0Configs.kG = ELEVATOR_KG.get(); + + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); + + leaderConfigurator.apply(slot0Configs); + followerConfigurator.apply(slot0Configs); + leaderConfigurator.apply(motionMagicConfigs); + followerConfigurator.apply(motionMagicConfigs); + } + } + + @Override + public void setPosition(double heightMeters) { + leader.setControl(motionRequest.withPosition(metersToRotations(heightMeters))); + } + + @Override + public void setVoltage(double voltage) { + leader.setControl(voltageOut.withOutput(voltage)); + } + + @Override + public void resetEncoder(final double position) { + leader.setPosition(position); + follower.setPosition(position); + } + + + + @Override + public void resetPosition(){ + leader.setPosition(0.0); + follower.setPosition(0.0); + } + + + private double getHeight() { + return rotationsToMeters(leader.getPosition().getValueAsDouble()); + } + + private double getVelocity() { + return rotationsToMeters(leader.getVelocity().getValueAsDouble()); + } + + private double metersToRotations(double heightMeters) { + return (heightMeters / (Math.PI * ELEVATOR_SPOOL_DIAMETER)) * ELEVATOR_GEAR_RATIO; + } + + private double rotationsToMeters(double rotations) { + return rotations * (Math.PI * ELEVATOR_SPOOL_DIAMETER) / ELEVATOR_GEAR_RATIO; + } + +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 8005d2b..9b9eee0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,23 +1,27 @@ package frc.robot.subsystems.elevator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.Measure; import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.RobotConstants; import static edu.wpi.first.units.Units.*; +/* public class ElevatorIOSim implements ElevatorIO { private static final double LOOP_PERIOD_SECS = 0.02; private final DCMotorSim leftElevatorTalonSim = new DCMotorSim(edu.wpi.first.math.system.plant.LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), - 0.025, 6.75), DCMotor.getFalcon500(1), null); + 0.025, 6.75), DCMotor.getFalcon500(1), null); private final DCMotorSim rightElevatorTalonSim = new DCMotorSim(edu.wpi.first.math.system.plant.LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), - 0.025, 6.75), DCMotor.getFalcon500(1), null); + 0.025, 6.75), DCMotor.getFalcon500(1) , null); - private final Measure leftElevatorAppliedVoltage = Volts.zero(); - private final Measure rightElevatorAppliedVoltage = Volts.zero(); - private final Measure targetElevatorVelocity = RadiansPerSecond.zero(); + private Measure leftElevatorAppliedVoltage = Volts.zero(); + private Measure rightElevatorAppliedVoltage = Volts.zero(); + private Measure targetElevatorVelocity = RadiansPerSecond.zero(); @Override public void updateInputs(ElevatorIOInputs inputs) { @@ -49,10 +53,6 @@ public void updateInputs(ElevatorIOInputs inputs) { public void setElevatorDirectVoltage(double volts) { } - @Override - public void resetElevatorPosition() { - } - @Override public boolean isNearExtension(double expected) { return true; @@ -68,13 +68,8 @@ public double getElevatorPosition() { } @Override - public double getElevatorVelocity() { + public double getVelocity() { return rightElevatorTalonSim.getAngularVelocityRadPerSec() / 6.28 * 60; } - - @Override - public boolean isCurrentMax(double max) { - //TODO:fix me - return false; - } } +*/ \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java deleted file mode 100644 index 4a93283..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ /dev/null @@ -1,148 +0,0 @@ -package frc.robot.subsystems.elevator; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.RobotConstants; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.RobotConstants.ElevatorConstants.*; - - -public class ElevatorIOTalonFX implements ElevatorIO { - private final MotionMagicVoltage positionVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true); - - private final TalonFX leftElevatorTalon = new TalonFX(LEFT_ELEVATOR_MOTOR_ID, RobotConstants.CANIVORE_CAN_BUS_NAME); - private final TalonFX rightElevatorTalon = new TalonFX(RIGHT_ELEVATOR_MOTOR_ID, RobotConstants.CANIVORE_CAN_BUS_NAME); - - private final StatusSignal leftElevatorVelocity = leftElevatorTalon.getVelocity(); - private final StatusSignal leftElevatorPosition = leftElevatorTalon.getPosition(); - private final StatusSignal leftElevatorAppliedVoltage = leftElevatorTalon.getMotorVoltage(); - private final StatusSignal leftElevatorSupplyCurrent = leftElevatorTalon.getSupplyCurrent(); - private final StatusSignal leftElevatorCurrent = leftElevatorTalon.getStatorCurrent(); - private final StatusSignal rightElevatorVelocity = rightElevatorTalon.getVelocity(); - private final StatusSignal rightElevatorPosition = rightElevatorTalon.getPosition(); - private final StatusSignal rightElevatorAppliedVoltage = rightElevatorTalon.getMotorVoltage(); - private final StatusSignal rightElevatorSupplyCurrent = rightElevatorTalon.getSupplyCurrent(); - private double targetElevatorVelocity = 0; - - public ElevatorIOTalonFX() { - TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); - elevatorMotorConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - elevatorMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - elevatorMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - elevatorMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - elevatorMotorConfig.Feedback.SensorToMechanismRatio = 1; - elevatorMotorConfig.MotorOutput.Inverted = RobotConstants.ElevatorConstants.leftMotorClockwise ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - - MotionMagicConfigs mmConfigs = elevatorMotorConfig.MotionMagic; - mmConfigs.MotionMagicCruiseVelocity = RobotConstants.ElevatorConstants.elevatorMotorRPS; - mmConfigs.MotionMagicAcceleration = RobotConstants.ElevatorConstants.elevatorMotorAccel; - leftElevatorTalon.setPosition(0.0); - rightElevatorTalon.setPosition(0.0); - leftElevatorTalon.getConfigurator().apply(elevatorMotorConfig); - rightElevatorTalon.getConfigurator().apply(elevatorMotorConfig); - - StatusCode response = leftElevatorTalon.getConfigurator().apply(elevatorMotorConfig); - if (response.isError()) - System.out.println("Left Elevator TalonFX failed config with error" + response); - response = leftElevatorTalon.clearStickyFaults(); - if (response.isError()) - System.out.println("Left Elevator TalonFX failed sticky fault clearing with error" + response); - - rightElevatorTalon.setControl(new Follower(leftElevatorTalon.getDeviceID(), true)); - } - - @Override - public void updateInputs(ElevatorIOInputs inputs) { - BaseStatusSignal.refreshAll( - leftElevatorVelocity, - leftElevatorPosition, - leftElevatorAppliedVoltage, - leftElevatorSupplyCurrent, - rightElevatorVelocity, - rightElevatorPosition, - rightElevatorAppliedVoltage, - rightElevatorSupplyCurrent); - - inputs.leftElevatorVelocity = RadiansPerSecond.of(Units.rotationsToRadians(leftElevatorVelocity.getValueAsDouble())); - inputs.leftElevatorPosition = Radians.of(Units.rotationsToRadians(leftElevatorPosition.getValueAsDouble())); - inputs.leftElevatorAppliedVoltage = Volts.of(leftElevatorAppliedVoltage.getValueAsDouble()); - inputs.leftElevatorSupplyCurrent = Amps.of(leftElevatorSupplyCurrent.getValueAsDouble()); - - inputs.rightElevatorVelocity = RadiansPerSecond.of(Units.rotationsToRadians(rightElevatorVelocity.getValueAsDouble())); - inputs.rightElevatorPosition = Radians.of(Units.rotationsToRadians(rightElevatorPosition.getValueAsDouble())); - inputs.rightElevatorAppliedVoltage = Volts.of(rightElevatorAppliedVoltage.getValueAsDouble()); - inputs.rightElevatorSupplyCurrent = Amps.of(rightElevatorSupplyCurrent.getValueAsDouble()); - - inputs.targetElevatorVelocity = RadiansPerSecond.of(targetElevatorVelocity); - - leftElevatorTalon.getConfigurator().apply(new Slot0Configs() - .withKP(inputs.ElevatorKP) - .withKI(inputs.ElevatorKI) - .withKD(inputs.ElevatorKD) - .withKA(inputs.ElevatorKA) - .withKV(inputs.ElevatorKV) - .withKS(inputs.ElevatorKS)); - } - - @Override - public void setElevatorDirectVoltage(double volts) { - leftElevatorTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setElevatorTarget(double meters) { - leftElevatorTalon.setControl(positionVoltage.withPosition(metersToRotations(meters))); - } - - @Override - public void resetElevatorPosition(){ - leftElevatorTalon.setPosition(0.0); - rightElevatorTalon.setPosition(0.0); - } - - @Override - public boolean isNearExtension(double expected) { - return MathUtil.isNear(metersToRotations(expected), leftElevatorTalon.getPosition().getValueAsDouble(), 0.02); - } - - @Override - public boolean isCurrentMax(double max){ - return leftElevatorTalon.getStatorCurrent().getValueAsDouble() > max; - } - - @Override - public double getElevatorPosition() { - return rotationsToMeters(leftElevatorTalon.getPosition().getValueAsDouble()); - } - - @Override - public double getElevatorVelocity() { - return leftElevatorVelocity.getValueAsDouble() * 60; - } - - private double metersToRotations(double heightMeters) { - return (heightMeters / (Math.PI * ELEVATOR_SPOOL_DIAMETER)) * ELEVATOR_GEAR_RATIO; - } - - private double rotationsToMeters(double rotations) { - return rotations * (Math.PI * ELEVATOR_SPOOL_DIAMETER) / ELEVATOR_GEAR_RATIO; - } -} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 4ae8e5d..847f93e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -1,14 +1,19 @@ package frc.robot.subsystems.elevator; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lombok.Getter; import org.littletonrobotics.junction.Logger; -@Getter +import static frc.robot.RobotConstants.ElevatorConstants.*; + public class ElevatorSubsystem extends SubsystemBase { private final ElevatorIO io; private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private final LinearFilter currentFilter = LinearFilter.movingAverage(5); + public double currentFilterValue = 0.0; + public ElevatorSubsystem(ElevatorIO io) { this.io = io; } @@ -17,5 +22,33 @@ public ElevatorSubsystem(ElevatorIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); + + currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps[0]); + Logger.recordOutput("Elevator/CurrentFilter", currentFilterValue); + } + + public void setPosition(double heightMeters) { + io.setPosition(heightMeters); + Logger.recordOutput("Elevator/Setpoint", heightMeters); + } + + public void setVoltage(double voltage) { + io.setVoltage(voltage); + } + + public double getPositionMeters() { + return inputs.positionMeters; + } + + public boolean isAtSetpoint(double setpoint) { + return MathUtil.isNear(setpoint, inputs.positionMeters, DEADZONE_DISTANCE); + } + + public double getLeaderCurrent(){ + return currentFilterValue; + } + + public void resetPosition(){ + io.resetPosition(); } } diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index 6596140..2c3266e 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -1,8 +1,6 @@ package frc.robot.subsystems.endeffector; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.subsystems.Superstructure; import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -183,17 +181,5 @@ public boolean isIntakeFinished () { return hasTransitionedToTransfer; } - public boolean isEndEffectorIntaking () { - return systemState == SystemState.FUNNEL_INTAKING || systemState==SystemState.GROUND_INTAKING || systemState==SystemState.TRANSFERRING; - } - - public boolean isEndEffectorHolding () { - return systemState == SystemState.HOLDING; - } - public void setWantedState(WantedState wantedState) {this.wantedState = wantedState;} - - public Command setWantedSuperStateCommand(WantedState wantedState) { - return new InstantCommand(() -> setWantedState(wantedState)); - } }