From 684dd24387e9b57a008d05ee3e8e3cca5aee1f77 Mon Sep 17 00:00:00 2001 From: Vignesh1234587 Date: Sat, 7 Dec 2024 10:27:36 -0500 Subject: [PATCH] changed some unit stuff and added conversions --- .../frc/robot/subsystems/arms/PivotIOSim.java | 7 +- .../robot/subsystems/arms/PivotIOTalonFX.java | 22 +- .../frc/robot/subsystems/drive/Drive.java | 208 +++++++++++------- .../robot/subsystems/drive/ModuleIOSim.java | 33 --- .../robot/subsystems/elevator/Elevator.java | 43 ++-- .../robot/subsystems/elevator/ElevatorIO.java | 6 +- .../subsystems/elevator/ElevatorIOSim.java | 50 ++++- .../flywheel/FlywheelIOTalonFX.java | 24 +- .../subsystems/indexers/IndexerIOSim.java | 3 +- .../subsystems/indexers/IndexerIOTalonFX.java | 14 +- src/main/java/frc/robot/util/Conversions.java | 179 +++++++++++++++ vendordeps/REVLib.json | 74 +++++++ 12 files changed, 494 insertions(+), 169 deletions(-) create mode 100644 src/main/java/frc/robot/util/Conversions.java create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java b/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java index 4aa356d..df96121 100644 --- a/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java @@ -14,13 +14,10 @@ /** Add your docs here. */ public class PivotIOSim implements PivotIO { -<<<<<<< Updated upstream -======= - private final DCMotor pivotGearbox = DCMotor.getKrakenX60Foc(2); + // private final DCMotor pivotGearbox = DCMotor.getKrakenX60Foc(2); - public RobotContainer robotContainer = new RobotContainer(); + // public RobotContainer robotContainer = new RobotContainer(); ->>>>>>> Stashed changes // SIM VARIABLES private int gearBoxMotorCount = 2; diff --git a/src/main/java/frc/robot/subsystems/arms/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/arms/PivotIOTalonFX.java index 9013b7f..7857bc1 100644 --- a/src/main/java/frc/robot/subsystems/arms/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/arms/PivotIOTalonFX.java @@ -13,7 +13,15 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +<<<<<<< Updated upstream import frc.robot.constants.PhysicalConstants; +======= +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.physicalConstants; +>>>>>>> Stashed changes import frc.robot.util.Conversions; import org.littletonrobotics.junction.Logger; @@ -27,11 +35,11 @@ public class PivotIOTalonFX implements PivotIO { private double startAngleDegs; - private final StatusSignal leaderPositionDegs; - private final StatusSignal velocityDegsPerSec; - private final StatusSignal appliedVolts; - private final StatusSignal currentAmps; - private final StatusSignal pitch; + private final StatusSignal leaderPositionDegs; + private final StatusSignal velocityDegsPerSec; + private final StatusSignal appliedVolts; + private final StatusSignal currentAmps; + private final StatusSignal pitch; public PivotIOTalonFX(int leadID, int followID, int gyroID) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -96,9 +104,13 @@ public void updateInputs(PivotIOInputs inputs) { // Conversions.falconToDegrees( // (followPositionDegs.getValueAsDouble()), Constants.PivotConstants.REDUCTION); inputs.velocityDegsPerSec = +<<<<<<< Updated upstream Conversions.falconToDegrees( velocityDegsPerSec.getValueAsDouble() * 2048, PhysicalConstants.PivotConstants.REDUCTION); +======= + velocityDegsPerSec.getValueAsDouble(); +>>>>>>> Stashed changes inputs.appliedVolts = appliedVolts.getValueAsDouble(); inputs.currentAmps = currentAmps.getValueAsDouble(); inputs.positionSetpointDegs = positionSetpointDegs; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 38284b6..8a7e931 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,14 +15,18 @@ import static edu.wpi.first.units.Units.*; -import edu.wpi.first.math.Matrix; +import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -34,14 +38,22 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +<<<<<<< Updated upstream import frc.robot.constants.PhysicalConstants; import frc.robot.subsystems.vision.VisionIO; +======= +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.generated.TunerConstants; +>>>>>>> Stashed changes import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -49,6 +61,7 @@ import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { +<<<<<<< Updated upstream private static final double MAX_LINEAR_SPEED = PhysicalConstants.SwerveConstants.MAX_LINEAR_SPEED * 0.85; private static final double TRACK_WIDTH_X = @@ -61,15 +74,45 @@ public class Drive extends SubsystemBase { PhysicalConstants.SwerveConstants.MAX_ANGULAR_SPEED; private static double multiplier = 1.0; private static boolean toggle = false; +======= + // TunerConstants doesn't include these constants, so they are declared locally + static final double ODOMETRY_FREQUENCY = + new CANBus(TunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; + public static final double DRIVE_BASE_RADIUS = + Math.max( + Math.max( + Math.hypot(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontRight.LocationY), + Math.hypot(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); +>>>>>>> Stashed changes + + // PathPlanner config constants + private static final double ROBOT_MASS_KG = 74.088; + private static final double ROBOT_MOI = 6.883; + private static final double WHEEL_COF = 1.2; + private static final RobotConfig PP_CONFIG = + new RobotConfig( + ROBOT_MASS_KG, + ROBOT_MOI, + new ModuleConfig( + TunerConstants.FrontLeft.WheelRadius, + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1) + .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), + TunerConstants.FrontLeft.SlipCurrent, + 1), + getModuleTranslations()); + static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; - public double yawVelocityRadPerSec = gyroInputs.yawVelocityRadPerSec; - - private final PIDController rotationController; - static final Lock odometryLock = new ReentrantLock(); + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -80,26 +123,8 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - public SwerveDrivePoseEstimator poseEstimator = + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - TimestampedT2d lastNoteLocT2d = new TimestampedT2d(new Translation2d(0, 0), -1.); - - public class TimestampedPose2d { - Pose2d pose; - double time; - } - - public class TimestampedT2d { - Translation2d translation; - double time; - - public TimestampedT2d(Translation2d translation, double time) { - this.translation = translation; - this.time = time; - } - } - - CircularBuffer robotPoseBuffer; public Drive( GyroIO gyroIO, @@ -108,19 +133,24 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO) { this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); + modules[0] = new Module(flModuleIO, 0, TunerConstants.FrontLeft); + modules[1] = new Module(frModuleIO, 1, TunerConstants.FrontRight); + modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft); + modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight); + + // Usage reporting for swerve template + HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); - // Start threads (no-op for each if no signals have been created) + // Start odometry thread PhoenixOdometryThread.getInstance().start(); - AutoBuilder.configureHolonomic( + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( this::getPose, this::setPose, - () -> kinematics.toChassisSpeeds(getModuleStates()), + this::getChassisSpeeds, this::runVelocity, +<<<<<<< Updated upstream new HolonomicPathFollowerConfig( new PIDConstants(5), new PIDConstants(1.5), @@ -130,6 +160,12 @@ public Drive( () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red, +======= + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + PP_CONFIG, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, +>>>>>>> Stashed changes this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( @@ -151,32 +187,18 @@ public Drive( null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(voltage.in(Volts)); - } - }, - null, - this)); - - rotationController = new PIDController(0.1, 0, 0); - - rotationController.setTolerance(5); - rotationController.enableContinuousInput(-180, 180); - robotPoseBuffer = new CircularBuffer<>(11); + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); } + @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); - } - odometryLock.unlock(); Logger.processInputs("Drive/Gyro", gyroInputs); for (var module : modules) { module.periodic(); } + odometryLock.unlock(); // Stop moving when disabled if (DriverStation.isDisabled()) { @@ -184,6 +206,7 @@ public void periodic() { module.stop(); } } + // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); @@ -221,6 +244,9 @@ public void periodic() { // Apply update poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); } /** @@ -230,20 +256,28 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + speeds.discretize(0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + modules[i].runSetpoint(setpointStates[i]); } - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } } /** Stops the drive. */ @@ -266,12 +300,14 @@ public void stopWithX() { /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); } /** Returns a command to run a dynamic test in the specified direction. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @@ -293,6 +329,30 @@ private SwerveModulePosition[] getModulePositions() { return states; } + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { @@ -309,12 +369,7 @@ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } - /** - * Adds a vision measurement to the pose estimator. - * - * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. - */ + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -325,21 +380,22 @@ public void addVisionMeasurement( /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return MAX_LINEAR_SPEED; + return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); } /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return MAX_ANGULAR_SPEED; + return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; } /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + new Translation2d(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), + new Translation2d(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), + new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } + } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 7702e48..0a9cb83 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -28,21 +28,6 @@ * constants from Phoenix. Simulation is always based on voltage control. */ public class ModuleIOSim implements ModuleIO { -<<<<<<< Updated upstream - - private static final double LOOP_PERIOD_SECS = 0.02; - private int gearBoxMotorCountDrive = 1; - private int gearBoxMotorCountTurn = 1; - private double gearingDrive = 6.75; - private double gearingTurn = 150.0 / 7.0; - private double momentOfInertiaDrive = 0.025; - private double momentOfInertiaTurn = 0.004; - - private DCMotorSim driveSim = - new DCMotorSim(DCMotor.getNEO(gearBoxMotorCountDrive), gearingDrive, momentOfInertiaDrive); - private DCMotorSim turnSim = - new DCMotorSim(DCMotor.getNEO(gearBoxMotorCountTurn), gearingTurn, momentOfInertiaTurn); -======= // TunerConstants doesn't support separate sim constants, so they are declared locally private static final double DRIVE_KP = 0.05; private static final double DRIVE_KD = 0.0; @@ -57,7 +42,6 @@ public class ModuleIOSim implements ModuleIO { private final DCMotorSim driveSim; private final DCMotorSim turnSim; ->>>>>>> Stashed changes private boolean driveClosedLoop = false; private boolean turnClosedLoop = false; @@ -67,10 +51,6 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; -<<<<<<< Updated upstream - private double clampedValueLowVolts = -12.0; - private double clampedValueHighVolts = 12.0; -======= public ModuleIOSim(SwerveModuleConstants constants) { // Create drive and turn sim models driveSim = @@ -87,7 +67,6 @@ public ModuleIOSim(SwerveModuleConstants constants) { // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); } ->>>>>>> Stashed changes @Override public void updateInputs(ModuleIOInputs inputs) { @@ -133,17 +112,6 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override -<<<<<<< Updated upstream - public void setDriveVoltage(double volts) { - driveAppliedVolts = MathUtil.clamp(volts, clampedValueLowVolts, clampedValueHighVolts); - driveSim.setInputVoltage(driveAppliedVolts); - } - - @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, clampedValueLowVolts, clampedValueHighVolts); - turnSim.setInputVoltage(turnAppliedVolts); -======= public void setDriveOpenLoop(double output) { driveClosedLoop = false; driveAppliedVolts = output; @@ -166,6 +134,5 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { turnClosedLoop = true; turnController.setSetpoint(rotation.getRadians()); ->>>>>>> Stashed changes } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index ad1bf5f..247c7a0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.physicalConstants; import static edu.wpi.first.units.Units.MetersPerSecond; @@ -24,10 +25,7 @@ public class Elevator extends SubsystemBase { private static final LoggedNetworkNumber kV = new LoggedNetworkNumber("Elevator/kV"); private static final LoggedNetworkNumber kA = new LoggedNetworkNumber("Elevator/kA"); - // amp bar gains - private static final LoggedNetworkNumber barkP = new LoggedNetworkNumber("Bar/kP"); - private static final LoggedNetworkNumber barkV = new LoggedNetworkNumber("Bar/kV"); private static final LoggedNetworkNumber barkG = new LoggedNetworkNumber("Bar/kG"); private TrapezoidProfile extenderProfile; @@ -39,12 +37,7 @@ public class Elevator extends SubsystemBase { private static double maxVelocityDegPerSec; private static double maxAccelerationDegPerSecSquared; - private TrapezoidProfile barProfile; - private TrapezoidProfile.Constraints barConstraints; - - private TrapezoidProfile.State barGoal = new TrapezoidProfile.State(); - private TrapezoidProfile.State barCurrent = new TrapezoidProfile.State(); - + private double goal; // private double barGoalPos; private final ElevatorFeedforward elevatorFFModel; @@ -63,8 +56,7 @@ public Elevator(ElevatorIO elevator) { kP.setDefault(0.44); kI.setDefault(0); - barkP.setDefault(0.25); - barkV.setDefault(0.2); + barkG.setDefault(0); break; case REPLAY: @@ -76,8 +68,7 @@ public Elevator(ElevatorIO elevator) { kP.setDefault(15); kI.setDefault(0); - barkP.setDefault(0); - barkV.setDefault(0); + barkG.setDefault(0); break; case SIM: @@ -89,8 +80,7 @@ public Elevator(ElevatorIO elevator) { kP.setDefault(1); kI.setDefault(0); - barkP.setDefault(0); - barkV.setDefault(0); + barkG.setDefault(0); break; default: @@ -102,8 +92,7 @@ public Elevator(ElevatorIO elevator) { kP.setDefault(15); kI.setDefault(0); - barkP.setDefault(0); - barkV.setDefault(0); + barkG.setDefault(0); break; } @@ -115,26 +104,28 @@ public Elevator(ElevatorIO elevator) { maxVelocityDegPerSec = 1200; maxAccelerationDegPerSecSquared = 1550; - barConstraints = - new TrapezoidProfile.Constraints(maxVelocityDegPerSec, maxAccelerationDegPerSecSquared); - barProfile = new TrapezoidProfile(barConstraints); - barCurrent = barProfile.calculate(0, barCurrent, barGoal); + this.elevator.configurePID(kP.get(), 0, 0); elevatorFFModel = new ElevatorFeedforward(kS.get(), kG.get(), kV.get(), kA.get()); } public boolean atGoal() { +<<<<<<< Updated upstream return (Math.abs(eInputs.elevatorPosition - goal) <= PhysicalConstants.ElevatorConstants.THRESHOLD); +======= + return (Math.abs(eInputs.elevatorPositionInch - goal) + <= physicalConstants.ElevatorConstants.THRESHOLD); +>>>>>>> Stashed changes } public double getElevatorPosition() { - return eInputs.elevatorPosition; + return eInputs.elevatorPositionInch; } private double getElevatorError() { - return eInputs.positionSetpoint - eInputs.elevatorPosition; + return eInputs.positionSetpointInch - eInputs.elevatorPositionInch; } public boolean elevatorAtSetpoint() { @@ -186,13 +177,17 @@ public void periodic() { extenderProfile.calculate( PhysicalConstants.LOOP_PERIOD_SECS, extenderCurrent, extenderGoal); +<<<<<<< Updated upstream barCurrent = barProfile.calculate(PhysicalConstants.LOOP_PERIOD_SECS, barCurrent, barGoal); +======= + +>>>>>>> Stashed changes setPositionExtend(extenderCurrent.position, extenderCurrent.velocity); Logger.processInputs("Elevator", eInputs); - Logger.recordOutput("amp bar currentPos", barCurrent.position); + // if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode())) { // elevator.configurePID(kP.get(), kI.get(), 0); // } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index ec88363..8124a8c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -7,11 +7,11 @@ public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { - double elevatorPosition = 0; - double elevatorVelocity = 0; + double elevatorPositionInch = 0; + double elevatorVelocityInchesPerSecond = 0; double currentAmps = 0; double appliedVolts = 0; - double positionSetpoint = 0; + double positionSetpointInch = 0; } public default void updateInputs(ElevatorIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 2d8907a..272be71 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -18,7 +18,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.CurrentUnit; + import edu.wpi.first.wpilibj.simulation.ElevatorSim; import frc.robot.constants.physicalConstants; @@ -26,31 +26,63 @@ /** Add your docs here. */ public class ElevatorIOSim implements ElevatorIO { +<<<<<<< Updated upstream private final DCMotor simGearbox = DCMotor.getKrakenX60Foc(2); private ElevatorSim sim = new ElevatorSim(simGearbox, 1, 1, 0.01, 0.0, 3, true, 0.0); private PIDController pid = new PIDController(0, 0, 0); private Distance position = Inches.of(0); private LinearVelocity velocityInchPerSec = InchesPerSecond.of(0); +======= + // SIM VARIABLES (CHANGE) + private int gearBoxMotorCount = 2; + private int gearing = 1; + private double carriageMassKg = 1; + private double drumRadiusMeters = 0.01; + private double minHeightMeters = 0; + private double maxHeightMeters = 3; + private boolean simulateGravity = true; + private double initialPositionMeters = 0.0; + + private final DCMotor simGearbox = DCMotor.getFalcon500(gearBoxMotorCount); + private ElevatorSim sim = + new ElevatorSim( + simGearbox, + gearing, + carriageMassKg, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + initialPositionMeters); + private PIDController pid = new PIDController(0, 0, 0); + + private Distance position = Inches.of(0); + private LinearVelocity velocity = InchesPerSecond.of(0); +>>>>>>> Stashed changes private Voltage appliedVolts = Volts.of(0.0); private Current currentAmps = Amps.of(0.0); private Distance positionSetpointInches = Inches.of(0); - private double clampedValueLowVolts = -12.0; - private double clampedValueHighVolts = 12.0; - private double metersToInches = 39.37; + + @Override public void updateInputs(ElevatorIOInputs inputs) { positionSetpointInches = Inches.of(pid.getSetpoint()); +<<<<<<< Updated upstream +======= + +>>>>>>> Stashed changes appliedVolts.plus( Volts.of(MathUtil.clamp( pid.calculate(Meters.of(sim.getPositionMeters()).in(Inches), positionSetpointInches.in(Inches)), -12.0, 12))); sim.setInputVoltage(appliedVolts.in(Volts)); +<<<<<<< Updated upstream position = Inches.of(Meters.of(sim.getPositionMeters()).in(Inches)); position = Meters.of(sim.getPositionMeters()); @@ -61,6 +93,16 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.appliedVolts = appliedVolts.in(Volts); inputs.elevatorPosition = position; inputs.elevatorVelocity = velocityInchPerSec; +======= + position = Meters.of(sim.getPositionMeters()); + velocity = MetersPerSecond.of(sim.getVelocityMetersPerSecond()); + currentAmps = Amps.of(sim.getCurrentDrawAmps()); + + inputs.positionSetpointInch = positionSetpointInches.in(Inches); + inputs.appliedVolts = appliedVolts.in(Volts); + inputs.elevatorPositionInch = position.in(Inches); + inputs.elevatorVelocityInchesPerSecond = velocity.in(InchesPerSecond); +>>>>>>> Stashed changes inputs.currentAmps = currentAmps.in(Amps); sim.update(physicalConstants.LOOP_PERIOD_SECS); diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java index 3f53512..b235db4 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -23,6 +23,11 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; 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.Distance; +import edu.wpi.first.units.measure.Voltage; public class FlywheelIOTalonFX implements FlywheelIO { private static final double GEAR_RATIO = 1.5; @@ -30,11 +35,11 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFX leader = new TalonFX(0); private final TalonFX follower = new TalonFX(1); - private final StatusSignal leaderPosition = leader.getPosition(); - private final StatusSignal leaderVelocity = leader.getVelocity(); - private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); - private final StatusSignal leaderCurrent = leader.getSupplyCurrent(); - private final StatusSignal followerCurrent = follower.getSupplyCurrent(); + private final StatusSignal leaderPosition = leader.getPosition(); + private final StatusSignal leaderVelocity = leader.getVelocity(); + private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); + private final StatusSignal leaderCurrent = leader.getSupplyCurrent(); + private final StatusSignal followerCurrent = follower.getSupplyCurrent(); public FlywheelIOTalonFX() { var config = new TalonFXConfiguration(); @@ -72,14 +77,7 @@ public void setVoltage(double volts) { public void setVelocity(double velocityRadPerSec, double ffVolts) { leader.setControl( new VelocityVoltage( - Units.radiansToRotations(velocityRadPerSec), - 0.0, - true, - ffVolts, - 0, - false, - false, - false)); + Units.radiansToRotations(velocityRadPerSec))); } @Override diff --git a/src/main/java/frc/robot/subsystems/indexers/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexers/IndexerIOSim.java index 4a9e345..fae7b69 100644 --- a/src/main/java/frc/robot/subsystems/indexers/IndexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/indexers/IndexerIOSim.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.constants.PhysicalConstants; @@ -13,7 +14,7 @@ public class IndexerIOSim implements IndexerIO { private int numberOFIndexerMotors = 0; private final DCMotor indexer = DCMotor.getKrakenX60(numberOFIndexerMotors); - private final DCMotorSim sim = new DCMotorSim(indexer, indexerGearing, indexerMOI); + private final DCMotorSim sim = new DCMotorSim(LinearSystemId.createDCMotorSystem(indexer, indexerMOI, indexerGearing), indexer); private PIDController pid = new PIDController(0.0, 0.0, 0.0); private double clampedValueLowVolts = -12.0; diff --git a/src/main/java/frc/robot/subsystems/indexers/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexers/IndexerIOTalonFX.java index 7b26fcb..cd1bb15 100644 --- a/src/main/java/frc/robot/subsystems/indexers/IndexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/indexers/IndexerIOTalonFX.java @@ -9,14 +9,18 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; 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; public class IndexerIOTalonFX implements IndexerIO { private final TalonFX talon; - private final StatusSignal positionRads; - private final StatusSignal velocityRadsPerSec; - private final StatusSignal appliedVolts; - private final StatusSignal currentAmps; + private final StatusSignal positionRads; + private final StatusSignal velocityRadsPerSec; + private final StatusSignal appliedVolts; + private final StatusSignal currentAmps; public IndexerIOTalonFX(int motorID) { talon = new TalonFX(motorID); @@ -57,7 +61,7 @@ public void setVoltage(double volts) { public void setPosition(double positionDegs, double ffVolts) { talon.setControl( new PositionVoltage( - Units.degreesToRotations(positionDegs), 0, false, ffVolts, 0, false, false, false)); + Units.degreesToRotations(positionDegs))); } @Override diff --git a/src/main/java/frc/robot/util/Conversions.java b/src/main/java/frc/robot/util/Conversions.java new file mode 100644 index 0000000..2a123b3 --- /dev/null +++ b/src/main/java/frc/robot/util/Conversions.java @@ -0,0 +1,179 @@ +package frc.robot.util; + +public class Conversions { + + /** + * @param positionCounts CANCoder Position Counts + * @param gearRatio Gear Ratio between CANCoder and Mechanism + * @return Degrees of Rotation of Mechanism + */ + public static double CANcoderToDegrees(double positionCounts, double gearRatio) { + return positionCounts * (360.0 / (gearRatio * 4096.0)); + } + /** + * @param radians + * @return degrees + */ + public static double radiansToDegrees(double radians) { + return radians * 180.0 / Math.PI; + } + /** + * @param degrees + * @return radians + */ + public static double degreesToRadians(double degrees) { + return degrees * Math.PI / 180.0; + } + /** + * @param degrees Degrees of rotation of Mechanism + * @param gearRatio Gear Ratio between CANCoder and Mechanism + * @return CANCoder Position Counts + */ + public static double degreesToCANcoder(double degrees, double gearRatio) { + return degrees / (360.0 / (gearRatio * 4096.0)); + } + + /** + * @param counts Falcon Position Counts + * @param gearRatio Gear Ratio between Falcon and Mechanism + * @return Degrees of Rotation of Mechanism + */ + public static double falconToDegrees(double positionCounts, double gearRatio) { + // return positionCounts * (360.0 / (gearRatio * 2048.0)); + return positionCounts * (360.0 / (gearRatio)); + } + + /** + * @param degrees Degrees of rotation of Mechanism + * @param gearRatio Gear Ratio between Falcon and Mechanism + * @return Falcon Position Counts + */ + public static double degreesToFalcon(double degrees, double gearRatio) { + // return degrees / (360.0 / (gearRatio * 2048.0)); + return degrees / (360.0 / (gearRatio)); + } + + /** + * @param velocityCounts Falcon Velocity Counts + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) + * @return RPM of Mechanism + */ + public static double falconToRPM(double velocityCounts, double gearRatio) { + double motorRPM = velocityCounts * (600.0 / 2048.0); + double mechRPM = motorRPM / gearRatio; + return mechRPM; + } + + /** + * @param RPM RPM of mechanism + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) + * @return RPM of Mechanism + */ + public static double RPMToFalcon(double RPM, double gearRatio) { + double motorRPM = RPM * gearRatio; + double sensorCounts = motorRPM * (2048.0 / 600.0); + return sensorCounts; + } + + /** + * @param velocitycounts Falcon Velocity Counts + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS) + * @return Falcon Velocity Counts + */ + public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) { + double wheelRPM = falconToRPM(velocitycounts, gearRatio); + double wheelMPS = (wheelRPM * circumference) / 60; + return wheelMPS; + } + + /** + * @param velocity Velocity MPS + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS) + * @return Falcon Velocity Counts + */ + public static double MPSToFalcon(double velocity, double circumference, double gearRatio) { + double wheelRPM = ((velocity * 60) / circumference); + double wheelVelocity = RPMToFalcon(wheelRPM, gearRatio); + return wheelVelocity; + } + + /** + * @param positionCounts Falcon Position Counts + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Wheel + * @return Meters + */ + public static double falconToMeters( + double positionCounts, double circumference, double gearRatio) { + return positionCounts * (circumference / (gearRatio * 2048.0)); + } + + /** + * @param meters Meters + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Wheel + * @return Falcon Position Counts + */ + public static double metersToFalcon(double meters, double circumference, double gearRatio) { + return meters / (circumference / (gearRatio * 2048.0)); + } + /** + * @param meters Meters + * @param circumference Circumference of Wheel + * @param gearRatio Gear Ratio between Falcon and Wheel + * @return Motor Rotations + */ + public static double MetersToMotorRot(double meters, double circumference, double gearRatio) { + return meters / (circumference / (gearRatio)); + } + /** + * @param inches Inches + * @param circumference Circumference of Wheel (Inches) + * @param gearRatio Gear Ratio between Falcon and Wheel + * @return Motor Rotations + */ + public static double inchesToMotorRot(double inches, double circumference, double gearRatio) { + return inches / (circumference / (gearRatio)); + } + + /** + * @param motorPositionRot Motor Position Inches + * @param circumference Circumference of Wheel (Inches) + * @param gearRatio Gear Ratio between Motor and Mechanism + * @return Position of Motor in Inches + */ + public static double motorRotToInches( + double motorPositionRot, double circumference, double gearRatio) { + return motorPositionRot * (circumference / (gearRatio)); + } + /** + * @param velocityInches Mechanism Velocity Inches + * @param circumference Circumference of Wheel (Inches) + * @param gearRatio Gear Ratio between Motor and Mechanism + * @return RPM of Motor + */ + public static double IPSToMotorRPM(double velocityIPS, double circumference, double gearRatio) { + return velocityIPS / (circumference / (gearRatio)) * 60.0; + } + /** + * @param velocityRPM Motor Velocity RPM + * @param circumference Circumference of Wheel (Inches) + * @param gearRatio Gear Ratio between Falcon and Mechanism + * @return Mechanism Velocity in Inches Per Second + */ + public static double motorRPMToIPS(double velocityRPM, double circumference, double gearRatio) { + return velocityRPM * (circumference / (gearRatio)) / 60.0; + } + /** + * @param accelerationInchPerSecSec Mechanism Acceleration Inch Per Sec^2 + * @param circumference Circumference of Wheel (Inches) + * @param gearRatio Gear Ratio between Motor and Mechanism + * @return Motor Accelearation RPM Per Sec + */ + public static double IPSSToMotorRPMS( + double accelerationInchPerSecSec, double circumference, double gearRatio) { + return accelerationInchPerSecSec / (circumference / (gearRatio)) * 60.0; + } +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..70cdac9 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.0-beta-3", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.0-beta-3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.0-beta-3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.0-beta-3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.0-beta-3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file