From de2f56a2c16cd88f209c3a561809815ed2bce79e Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sun, 26 Jan 2025 22:03:56 +0800 Subject: [PATCH 01/19] Got devbot working Signed-off-by: Jade Turner --- .../java/org/curtinfrc/frc2025/Constants.java | 28 +- .../java/org/curtinfrc/frc2025/Robot.java | 44 ++-- .../frc2025/generated/TunerConstants.java | 76 +++--- .../subsystems/ejector/EjectorConstants.java | 7 +- .../subsystems/ejector/EjectorIONEO.java | 8 +- .../frc2025/subsystems/elevator/Elevator.java | 19 +- .../elevator/ElevatorConstants.java | 6 +- .../subsystems/elevator/ElevatorIO.java | 22 +- .../subsystems/elevator/ElevatorIONEO.java | 40 +++ .../subsystems/elevator/ElevatorIONeo.java | 24 -- .../elevator/ElevatorIONeoMaxMotion.java | 59 ----- .../ElevatorIONeoMaxMotionLaserCAN.java | 38 --- .../subsystems/elevator/ElevatorIOSim.java | 36 --- .../subsystems/intake/IntakeConstants.java | 8 +- .../subsystems/intake/IntakeIONEO.java | 2 +- tuner-swerve-project.json | 244 ++++++++++++++++++ 16 files changed, 396 insertions(+), 265 deletions(-) create mode 100644 src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java delete mode 100644 src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeo.java delete mode 100644 src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotion.java delete mode 100644 src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotionLaserCAN.java delete mode 100644 src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java create mode 100644 tuner-swerve-project.json diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index b591aef0..c3c28631 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -16,8 +16,6 @@ import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.aprilTagLayout; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import java.util.ArrayList; @@ -30,7 +28,7 @@ * (log replay from a file). */ public final class Constants { - public static final RobotType robotType = RobotType.SIMBOT; + public static final RobotType robotType = RobotType.DEVBOT; public static final double ROBOT_X = 550; // mm public static final double ROBOT_Y = 570; @@ -41,15 +39,6 @@ public static final Mode getMode() { }; } - @SuppressWarnings("resource") - public static RobotType getRobot() { - if (RobotBase.isReal() && RobotType.SIMBOT == robotType) { - new Alert("Robot type is invalid, defaulting to competition robot.", AlertType.kWarning); - return RobotType.COMPBOT; - } - return robotType; - } - public static enum RobotType { /** Running in simulation */ SIMBOT, @@ -82,22 +71,23 @@ public static void main(String... args) { // TODO: MAKE SETPOINTS public enum Setpoints { /* in mm */ - COLLECT(950, List.of(13, 12), List.of(1, 2)), - L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), - L2(810, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), - L3(1210, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); + COLLECT(0, List.of(13, 12), List.of(1, 2)), + // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO + // TODO actually subtract + L2(13.2 - 2.5, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), + L3(35.2 - 2.5, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); - private final int _elevatorSetpoint; + private final double _elevatorSetpoint; private final List _tagIdsBlue; private final List _tagIdsRed; - Setpoints(int elevator, List tagIdsBlue, List tagIdsRed) { + Setpoints(double elevator, List tagIdsBlue, List tagIdsRed) { this._elevatorSetpoint = elevator; this._tagIdsBlue = tagIdsBlue; this._tagIdsRed = tagIdsRed; } - public int elevatorSetpoint() { + public double elevatorSetpoint() { return this._elevatorSetpoint; } diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index c8829901..312177fe 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -30,8 +30,7 @@ import org.curtinfrc.frc2025.subsystems.ejector.EjectorIOSim; import org.curtinfrc.frc2025.subsystems.elevator.Elevator; import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIO; -import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONeoMaxMotionLaserCAN; -import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOSim; +import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONEO; import org.curtinfrc.frc2025.subsystems.intake.Intake; import org.curtinfrc.frc2025.subsystems.intake.IntakeIO; import org.curtinfrc.frc2025.subsystems.intake.IntakeIONEO; @@ -140,7 +139,6 @@ public Robot() { new VisionIOLimelightGamepiece(camera0Name), new VisionIOLimelight(camera1Name, drive::getRotation), new VisionIOQuestNav()); - // elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN()); elevator = new Elevator(new ElevatorIO() {}); intake = new Intake(new IntakeIONEO()); ejector = new Ejector(new EjectorIONEO()); @@ -158,10 +156,10 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOLimelightGamepiece(camera0Name), - new VisionIOLimelight(camera1Name, drive::getRotation), - new VisionIOQuestNav()); - elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN()); + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}); + elevator = new Elevator(new ElevatorIONEO()); intake = new Intake(new IntakeIONEO()); ejector = new Ejector(new EjectorIONEO()); } @@ -184,7 +182,7 @@ public Robot() { new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose), new VisionIO() {}); - elevator = new Elevator(new ElevatorIOSim()); + elevator = new Elevator(new ElevatorIO() {}); intake = new Intake(new IntakeIOSim()); ejector = new Ejector(new EjectorIOSim()); } @@ -269,13 +267,20 @@ public Robot() { () -> controller.getLeftX(), () -> -controller.getRightX())); - intake.setDefaultCommand(intake.intake(intakeVolts / 4)); + intake.setDefaultCommand(intake.intake(intakeVolts)); ejector.setDefaultCommand(ejector.stop()); - intake.frontSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor)); + elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); intake.backSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor.negate())); - intake.backSensor.whileTrue(ejector.eject(15).until(ejector.sensor)); + intake.backSensor.whileTrue(intake.intake(intakeVolts / 2)); + intake.backSensor.onTrue( + ejector + .eject(300) + .until(intake.backSensor.negate()) + .andThen(ejector.eject(-60).until(intake.backSensor))); + ejector.sensor.whileTrue(intake.stop()); - controller.x().whileTrue(ejector.eject(1500).until(ejector.sensor.negate())); + // controller.x().whileTrue(ejector.eject(150).until(ejector.sensor.negate())); + controller.x().whileTrue(ejector.eject(5000)); // Lock to 0° when A button is held controller @@ -290,15 +295,12 @@ public Robot() { .onTrue( Commands.runOnce( () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), + drive.setPose(new Pose2d(drive.getPose().getTranslation(), Rotation2d.kPi)), drive) .ignoringDisable(true)); - controller.pov(0).whileTrue(superstructure.align(Setpoints.L1)); - controller.pov(90).whileTrue(superstructure.align(Setpoints.L2)); - controller.pov(180).whileTrue(superstructure.align(Setpoints.L3)); - controller.pov(270).whileTrue(superstructure.align(Setpoints.COLLECT)); + controller.rightBumper().whileTrue(elevator.goToSetpoint(Setpoints.L3)); + controller.leftBumper().whileTrue(elevator.goToSetpoint(Setpoints.L2)); } /** This function is called periodically during all modes. */ @@ -317,6 +319,12 @@ public void robotPeriodic() { // Runs virtual subsystems VirtualSubsystem.periodicAll(); + if (ejector.getCurrentCommand() != null) { + Logger.recordOutput("EjectorCommand", ejector.getCurrentCommand().getName()); + } else { + Logger.recordOutput("EjectorCommand", "null"); + } + autoChooser.periodic(); // Return to normal thread priority diff --git a/src/main/java/org/curtinfrc/frc2025/generated/TunerConstants.java b/src/main/java/org/curtinfrc/frc2025/generated/TunerConstants.java index d4a1538f..a4594541 100644 --- a/src/main/java/org/curtinfrc/frc2025/generated/TunerConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/generated/TunerConstants.java @@ -12,9 +12,9 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; - // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html + public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -22,13 +22,13 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(33.427) - .withKD(1.5312) - .withKS(0.082888) - .withKV(2.4037) - .withKA(0.070482) + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final double kDriveGearRatio = 5.902777777777778; @@ -56,7 +56,7 @@ public class TunerConstants { SteerMotorArrangement.TalonFX_Integrated; // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; // The stator current at which the wheels start to slip; @@ -73,7 +73,7 @@ public class TunerConstants { // Swerve azimuth does not require much torque output, so we can set a relatively // low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(30)) .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs @@ -85,19 +85,19 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.41); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.14); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.125; private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); + private static final Distance kWheelRadius = Inches.of(1.9); - private static final boolean kInvertLeftSide = true; - private static final boolean kInvertRightSide = false; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 0; + private static final int kPigeonId = 20; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -139,48 +139,48 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left - private static final int kFrontLeftDriveMotorId = 41; - private static final int kFrontLeftSteerMotorId = 42; - private static final int kFrontLeftEncoderId = 43; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.099365234375); + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 6; + private static final int kFrontLeftEncoderId = 18; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.2392578125); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(8.035); - private static final Distance kFrontLeftYPos = Inches.of(8.035); + private static final Distance kFrontLeftXPos = Inches.of(8.03937); + private static final Distance kFrontLeftYPos = Inches.of(8.43307); // Front Right - private static final int kFrontRightDriveMotorId = 11; - private static final int kFrontRightSteerMotorId = 12; - private static final int kFrontRightEncoderId = 13; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.447998046875); + private static final int kFrontRightDriveMotorId = 5; + private static final int kFrontRightSteerMotorId = 4; + private static final int kFrontRightEncoderId = 21; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.081298828125); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(8.035); - private static final Distance kFrontRightYPos = Inches.of(-8.035); + private static final Distance kFrontRightXPos = Inches.of(8.03937); + private static final Distance kFrontRightYPos = Inches.of(-8.43307); // Back Left - private static final int kBackLeftDriveMotorId = 31; - private static final int kBackLeftSteerMotorId = 32; - private static final int kBackLeftEncoderId = 33; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.384765625); + private static final int kBackLeftDriveMotorId = 3; + private static final int kBackLeftSteerMotorId = 2; + private static final int kBackLeftEncoderId = 17; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.099365234375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-8.035); - private static final Distance kBackLeftYPos = Inches.of(8.035); + private static final Distance kBackLeftXPos = Inches.of(-8.03937); + private static final Distance kBackLeftYPos = Inches.of(8.43307); // Back Right - private static final int kBackRightDriveMotorId = 21; - private static final int kBackRightSteerMotorId = 22; - private static final int kBackRightEncoderId = 23; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.064453125); + private static final int kBackRightDriveMotorId = 7; + private static final int kBackRightSteerMotorId = 8; + private static final int kBackRightEncoderId = 19; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.151611328125); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-8.035); - private static final Distance kBackRightYPos = Inches.of(-8.035); + private static final Distance kBackRightXPos = Inches.of(-8.03937); + private static final Distance kBackRightYPos = Inches.of(-8.43307); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java index f3057fe1..15379082 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java @@ -1,14 +1,15 @@ package org.curtinfrc.frc2025.subsystems.ejector; public class EjectorConstants { - public static final int motorId = 99; + public static final int motorId = 30; public static final int sensorPort = 99; public static final int currentLimit = 60; public static final double ejectorMoi = 0.025; public static final double ejectorReduction = 1; - public static final double kP = 1.0; + public static final double kP = 0.01; public static final double kD = 0; public static final double kS = 0; - public static final double kV = 473; + // public static final double kV = 473; + public static final double kV = 0; public static final double kA = 0; } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIONEO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIONEO.java index 724290c0..b62c801c 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIONEO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIONEO.java @@ -8,16 +8,15 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DigitalInput; import org.curtinfrc.frc2025.util.SparkUtil; public class EjectorIONEO implements EjectorIO { private final SparkMax neo = new SparkMax(motorId, MotorType.kBrushless); - private final DigitalInput sensor = new DigitalInput(sensorPort); + // private final DigitalInput sensor = new DigitalInput(sensorPort); public EjectorIONEO() { SparkMaxConfig config = new SparkMaxConfig(); - config.smartCurrentLimit(0, currentLimit).idleMode(IdleMode.kCoast); + config.smartCurrentLimit(0, currentLimit).idleMode(IdleMode.kCoast).inverted(true); SparkUtil.tryUntilOk( 5, @@ -31,7 +30,8 @@ public void updateInputs(EjectorIOInputs inputs) { inputs.currentAmps = neo.getOutputCurrent(); inputs.positionRotations = neo.getEncoder().getPosition(); inputs.angularVelocityRotationsPerMinute = neo.getEncoder().getVelocity(); - inputs.sensor = sensor.get(); + // inputs.sensor = sensor.get(); + inputs.sensor = false; } @Override diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java index 2fddfabb..7b915541 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java @@ -1,13 +1,18 @@ package org.curtinfrc.frc2025.subsystems.elevator; +import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import org.curtinfrc.frc2025.Constants.Setpoints; import org.littletonrobotics.junction.Logger; public class Elevator extends SubsystemBase { private final ElevatorIO io; private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private final PIDController pid = new PIDController(kP, 0, kD); public Elevator(ElevatorIO io) { this.io = io; @@ -19,7 +24,19 @@ public void periodic() { Logger.processInputs("Elevator", inputs); } + public Trigger atSetpoint = new Trigger(pid::atSetpoint); + public Command goToSetpoint(Setpoints point) { - return run(() -> io.goToSetpoint(point)).until(() -> io.isStable()); + return run( + () -> { + var out = pid.calculate(inputs.positionRotations, point.elevatorSetpoint()); + Logger.recordOutput("Elevator/Output", out); + Logger.recordOutput("Elevator/Error", pid.getError()); + io.setVoltage(out); + }); + } + + public Command stop() { + return runOnce(() -> io.setVoltage(0)); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java index 004dd89e..a7df6408 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java @@ -1,9 +1,9 @@ package org.curtinfrc.frc2025.subsystems.elevator; public class ElevatorConstants { - public static int motorCount = 2; + public static int motorCount = 1; public static int distanceSensorPort = 99; - public static int motorPort = 99; + public static int motorPort = 31; public static double tolerance = 0.01; public static double maxVel = 5676; @@ -11,7 +11,7 @@ public class ElevatorConstants { public static double allowedErr = 0; // TODO: TUNE PID - public static double kP = 1; + public static double kP = 0.5; public static double kI = 0; public static double kD = 0; public static double kMinOutput = 0; diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java index 41614cff..c1de5de9 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java @@ -1,30 +1,18 @@ package org.curtinfrc.frc2025.subsystems.elevator; -import org.curtinfrc.frc2025.Constants.Setpoints; import org.littletonrobotics.junction.AutoLog; public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { - public double distanceSensorReading; - public double encoderReading; - public Setpoints point; - public double motorVoltage; - public double motorCurrent; - public double motorTemp; - public double motorVelocity; + public double appliedVolts; + public double currentAmps; + public double positionRotations; + public double angularVelocityRotationsPerMinute; } - public default void goToSetpoint(Setpoints point) {} - - public default void applyVoltage(double volts) {} - public default void updateInputs(ElevatorIOInputs inputs) {} - public default void reset() {} - - public default boolean isStable() { - return false; - } + public default void setVoltage(double volts) {} } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java new file mode 100644 index 00000000..abc99f9a --- /dev/null +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java @@ -0,0 +1,40 @@ +package org.curtinfrc.frc2025.subsystems.elevator; + +import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.currentLimit; +import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.*; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import org.curtinfrc.frc2025.util.SparkUtil; + +public class ElevatorIONEO implements ElevatorIO { + private final SparkMax neo = new SparkMax(motorPort, MotorType.kBrushless); + // private final DigitalInput sensor = new DigitalInput(sensorPort); + + public ElevatorIONEO() { + SparkMaxConfig config = new SparkMaxConfig(); + config.smartCurrentLimit(0, currentLimit).idleMode(IdleMode.kCoast).inverted(true); + + SparkUtil.tryUntilOk( + 5, + () -> + neo.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.appliedVolts = neo.getBusVoltage() * neo.getAppliedOutput(); + inputs.currentAmps = neo.getOutputCurrent(); + inputs.positionRotations = neo.getEncoder().getPosition(); + inputs.angularVelocityRotationsPerMinute = neo.getEncoder().getVelocity(); + } + + @Override + public void setVoltage(double volts) { + neo.setVoltage(volts); + } +} diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeo.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeo.java deleted file mode 100644 index 7e41fc52..00000000 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeo.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.curtinfrc.frc2025.subsystems.elevator; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import org.curtinfrc.frc2025.Constants.Setpoints; - -public class ElevatorIONeo implements ElevatorIO { - protected final SparkMax elevatorMotor = - new SparkMax(ElevatorConstants.motorPort, MotorType.kBrushless); - protected final RelativeEncoder elevatorEncoder = elevatorMotor.getEncoder(); - protected Setpoints setpoint = Setpoints.COLLECT; - - public ElevatorIONeo() {} - - @Override - public void updateInputs(ElevatorIOInputs inputs) { - inputs.point = this.setpoint; - inputs.encoderReading = elevatorEncoder.getPosition(); - inputs.motorCurrent = elevatorMotor.getOutputCurrent(); - inputs.motorTemp = elevatorMotor.getMotorTemperature(); - inputs.motorVelocity = elevatorEncoder.getVelocity(); - } -} diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotion.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotion.java deleted file mode 100644 index 6b1ab05b..00000000 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotion.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.curtinfrc.frc2025.subsystems.elevator; - -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.config.SparkMaxConfig; -import org.curtinfrc.frc2025.Constants.Setpoints; -import org.curtinfrc.frc2025.util.SparkUtil; - -public class ElevatorIONeoMaxMotion extends ElevatorIONeo { - private SparkClosedLoopController controller = elevatorMotor.getClosedLoopController(); - - public static double convertSetpoint(double set /* in mm */) { - return set / 21; /* mm to revolutions */ - } - - public ElevatorIONeoMaxMotion() { - var config = new SparkMaxConfig(); - config.voltageCompensation(12.0).smartCurrentLimit(80); - - config - .closedLoop - .p(ElevatorConstants.kP) - .i(ElevatorConstants.kI) - .d(ElevatorConstants.kD) - .outputRange(ElevatorConstants.kMinOutput, ElevatorConstants.kMaxOutput); - - config.closedLoop.velocityFF(1 / ElevatorConstants.kV); - - config - .closedLoop - .maxMotion - .maxVelocity(ElevatorConstants.maxVel) - .maxAcceleration(ElevatorConstants.maxAccel) - .allowedClosedLoopError(ElevatorConstants.allowedErr); - - SparkUtil.tryUntilOk( - 5, - () -> - elevatorMotor.configure( - config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - } - - @Override - public void goToSetpoint(Setpoints point) { - setpoint = point; - controller.setReference(convertSetpoint(point.elevatorSetpoint()), ControlType.kPosition); - } - - @Override - public boolean isStable() { - double vel = elevatorEncoder.getVelocity(); - return ElevatorConstants.tolerance > vel && vel > -ElevatorConstants.tolerance; - // double pos = elevatorEncoder.getPosition(); - // return setpoint.elevatorSetpoint() - ElevatorConstants.tolerance < pos - // && pos < setpoint.elevatorSetpoint() + ElevatorConstants.tolerance; - } -} diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotionLaserCAN.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotionLaserCAN.java deleted file mode 100644 index 4f0661b6..00000000 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMaxMotionLaserCAN.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.curtinfrc.frc2025.subsystems.elevator; - -import au.grapplerobotics.ConfigurationFailedException; -import au.grapplerobotics.LaserCan; - -public class ElevatorIONeoMaxMotionLaserCAN extends ElevatorIONeoMaxMotion { - private LaserCan lc; - private double distance = -1; - - public ElevatorIONeoMaxMotionLaserCAN() { - lc = new LaserCan(ElevatorConstants.distanceSensorPort); - try { - lc.setRangingMode(LaserCan.RangingMode.SHORT); - lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); - lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - } catch (ConfigurationFailedException e) { - System.out.println("Configuration failed! " + e); - } - } - - @Override - public void reset() { - LaserCan.Measurement measurement = lc.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - distance = measurement.distance_mm; - } - - if (distance >= 0) { - elevatorEncoder.setPosition(ElevatorIONeoMaxMotion.convertSetpoint(distance)); - } - } - - @Override - public void updateInputs(ElevatorIOInputs inputs) { - super.updateInputs(inputs); - inputs.distanceSensorReading = distance; - } -} diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java deleted file mode 100644 index ebfbd18e..00000000 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.curtinfrc.frc2025.subsystems.elevator; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.curtinfrc.frc2025.Constants.Setpoints; - -public class ElevatorIOSim implements ElevatorIO { - private DCMotor motor = DCMotor.getNEO(ElevatorConstants.motorCount); - private DCMotorSim elevatorSim; - private Setpoints set = Setpoints.COLLECT; - - public ElevatorIOSim() { - elevatorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(motor, 0.025, 4.0), motor); - } - - @Override - public void updateInputs(ElevatorIOInputs inputs) { - elevatorSim.update(0.02); - - inputs.point = set; - inputs.distanceSensorReading = 0; - inputs.encoderReading = elevatorSim.getAngularPositionRotations(); - } - - public void goToSetpoint(Setpoints point) { - set = point; - - elevatorSim.setAngle(ElevatorIONeoMaxMotion.convertSetpoint(point.elevatorSetpoint())); - } - - @Override - public boolean isStable() { - return true; - } -} diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java index ddb35d1f..2b25ef2a 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java @@ -1,11 +1,11 @@ package org.curtinfrc.frc2025.subsystems.intake; public class IntakeConstants { - public static double intakeVolts = 8; + public static double intakeVolts = 4; public static int intakeCurrentLimit = 40; public static double motorReduction = 1.0; public static double intakeMoi = 1.0; - public static int intakeMotorId = 99; - public static int intakeFrontSensorPort = 99; - public static int intakeBackSensorPort = 99; + public static int intakeMotorId = 32; + public static int intakeFrontSensorPort = 9; + public static int intakeBackSensorPort = 8; } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeIONEO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeIONEO.java index 81fdba6c..e51691a1 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeIONEO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeIONEO.java @@ -18,7 +18,7 @@ public class IntakeIONEO implements IntakeIO { public IntakeIONEO() { SparkMaxConfig config = new SparkMaxConfig(); - config.smartCurrentLimit(0, intakeCurrentLimit).idleMode(IdleMode.kCoast); + config.smartCurrentLimit(0, intakeCurrentLimit).idleMode(IdleMode.kCoast).inverted(true); SparkUtil.tryUntilOk( 5, () -> diff --git a/tuner-swerve-project.json b/tuner-swerve-project.json new file mode 100644 index 00000000..79288a9b --- /dev/null +++ b/tuner-swerve-project.json @@ -0,0 +1,244 @@ +{ + "Version": "1.0.0.0", + "LastState": 11, + "Modules": [ + { + "ModuleName": "Front Left", + "ModuleId": 0, + "Encoder": { + "Id": 18, + "Name": "Front Left CANcoder", + "Model": "CANCoder vers. H", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": null, + "IsStandaloneFx": false + }, + "SteerMotor": { + "Id": 6, + "Name": "Front Left Turn", + "Model": "Talon FX", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "DriveMotor": { + "Id": 1, + "Name": "Front Left Drive", + "Model": "Talon FX vers. C", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "IsEncoderInverted": false, + "IsSteerInverted": true, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": -0.2392578125, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 6, + "ValidatedDriveId": 1, + "ValidatedEncoderId": 18 + }, + { + "ModuleName": "Front Right", + "ModuleId": 1, + "Encoder": { + "Id": 21, + "Name": "Front Left CANcoder", + "Model": "CANCoder vers. H", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": null, + "IsStandaloneFx": false + }, + "SteerMotor": { + "Id": 4, + "Name": "Front Right Turn", + "Model": "Talon FX", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "DriveMotor": { + "Id": 5, + "Name": "Front Right Drive", + "Model": "Talon FX vers. C", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "IsEncoderInverted": false, + "IsSteerInverted": true, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": 0.081298828125, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 4, + "ValidatedDriveId": 5, + "ValidatedEncoderId": 21 + }, + { + "ModuleName": "Back Left", + "ModuleId": 2, + "Encoder": { + "Id": 17, + "Name": "Back Left CANcoder", + "Model": "CANCoder vers. H", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": null, + "IsStandaloneFx": false + }, + "SteerMotor": { + "Id": 2, + "Name": "Back Left Turn", + "Model": "Talon FX", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "DriveMotor": { + "Id": 3, + "Name": "Back Left Drive", + "Model": "Talon FX vers. C", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "IsEncoderInverted": false, + "IsSteerInverted": true, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": 0.099365234375, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 2, + "ValidatedDriveId": 3, + "ValidatedEncoderId": 17 + }, + { + "ModuleName": "Back Right", + "ModuleId": 3, + "Encoder": { + "Id": 19, + "Name": "Back Right CANcoder", + "Model": "CANCoder vers. H", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": null, + "IsStandaloneFx": false + }, + "SteerMotor": { + "Id": 8, + "Name": "Back Right Turn", + "Model": "Talon FX", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "DriveMotor": { + "Id": 7, + "Name": "Back Right Drive", + "Model": "Talon FX vers. C", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": { + "Name": "WCP Kraken x60", + "FreeSpeedRps": 96.7, + "SlipCurrentLimit": 120, + "StatorCurrentLimit": 60 + }, + "IsStandaloneFx": false + }, + "IsEncoderInverted": false, + "IsSteerInverted": true, + "SelectedEncoderType": "CANcoder", + "EncoderOffset": 0.151611328125, + "DriveMotorSelectionState": 1, + "SteerMotorSelectionState": 1, + "SteerEncoderSelectionState": 1, + "IsModuleValidationComplete": true, + "ValidatedSteerId": 8, + "ValidatedDriveId": 7, + "ValidatedEncoderId": 19 + } + ], + "SwerveOptions": { + "kSpeedAt12Volts": 5.1370140354266, + "Gyro": { + "Id": 20, + "Name": "Pigeon2", + "Model": "Pigeon 2 vers. S", + "CANbus": "A56F57BE46324B532020204A0B2C12FF", + "CANbusFriendly": "Drivebase", + "SelectedMotorType": null, + "IsStandaloneFx": false + }, + "IsValidGyroCANbus": true, + "VerticalTrackSizeInches": 16.07874, + "HorizontalTrackSizeInches": 16.86614, + "WheelRadiusInches": 1.9, + "IsLeftSideInverted": false, + "IsRightSideInverted": true, + "SwerveModuleType": 4, + "SwerveModuleConfiguration": { + "ModuleBrand": 4, + "DriveRatio": 5.902777777777778, + "SteerRatio": 21.428571428571427, + "CouplingRatio": 3.125, + "CustomName": "L2 16T" + }, + "HasVerifiedSteer": true, + "SelectedModuleManufacturer": "Swerve Drive Specialties (SDS)", + "HasVerifiedDrive": true, + "IsValidConfiguration": true + } +} From 53b0cfef29fc2a8c3b0da23a02ac5202202a56da Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Thu, 30 Jan 2025 15:36:02 +0800 Subject: [PATCH 02/19] update choreo lenghts Signed-off-by: Jade Turner --- src/main/deploy/choreo/Close Nodes.traj | 59 +++++++++++-------------- src/main/deploy/choreo/autos.chor | 12 ++--- 2 files changed, 33 insertions(+), 38 deletions(-) diff --git a/src/main/deploy/choreo/Close Nodes.traj b/src/main/deploy/choreo/Close Nodes.traj index e8ab9efd..34345641 100644 --- a/src/main/deploy/choreo/Close Nodes.traj +++ b/src/main/deploy/choreo/Close Nodes.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.615602731704712, "y":7.34986686706543, "heading":-0.942477796076938, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.6582536697387695, "y":5.08836030960083, "heading":-1.0232340455030795, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":1.6227154731750488, "y":7.467684745788574, "heading":-0.942477796076938, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.6217777729034424, "y":5.0564446449279785, "heading":-1.0232340455030795, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"1.615602731704712 m", "val":1.615602731704712}, "y":{"exp":"7.34986686706543 m", "val":7.34986686706543}, "heading":{"exp":"-CoralStation", "val":-0.942477796076938}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.6582536697387695 m", "val":3.6582536697387695}, "y":{"exp":"5.08836030960083 m", "val":5.08836030960083}, "heading":{"exp":"-1.0232340455030795 rad", "val":-1.0232340455030795}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"1.6227154731750488 m", "val":1.6227154731750488}, "y":{"exp":"7.467684745788574 m", "val":7.467684745788574}, "heading":{"exp":"-CoralStation", "val":-0.942477796076938}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.6217777729034424 m", "val":3.6217777729034424}, "y":{"exp":"5.0564446449279785 m", "val":5.0564446449279785}, "heading":{"exp":"-1.0232340455030795 rad", "val":-1.0232340455030795}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,35 +26,30 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.16607], + "waypoints":[0.0,0.93288], "samples":[ - {"t":0.0, "x":1.6156, "y":7.34987, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.04257, "ay":-6.69403, "alpha":-0.31548, "fx":[101.41397,103.88751,104.1752,101.65279], "fy":[-115.09822,-112.87105,-112.60136,-114.88318]}, - {"t":0.04485, "x":1.62168, "y":7.34313, "heading":-0.94248, "vx":0.271, "vy":-0.30022, "omega":-0.01415, "ax":6.04258, "ay":-6.69301, "alpha":-0.30912, "fx":[101.44165,103.86548,104.14686,101.67612], "fy":[-115.05656,-112.87376,-112.60968,-114.84494]}, - {"t":0.0897, "x":1.63991, "y":7.32294, "heading":-0.94311, "vx":0.542, "vy":-0.60039, "omega":-0.02801, "ax":6.04241, "ay":-6.69195, "alpha":-0.3016, "fx":[101.47074,103.83568,104.11105,101.70152], "fy":[-115.01015,-112.88007,-112.62129,-114.80133]}, - {"t":0.13455, "x":1.6703, "y":7.28928, "heading":-0.94437, "vx":0.813, "vy":-0.90052, "omega":-0.04154, "ax":6.04205, "ay":-6.69081, "alpha":-0.2925, "fx":[101.50264,103.79618,104.06529,101.7299], "fy":[-114.95657,-112.89055,-112.6373,-114.75034]}, - {"t":0.1794, "x":1.71283, "y":7.24217, "heading":-0.94623, "vx":1.08398, "vy":-1.20059, "omega":-0.05466, "ax":6.04143, "ay":-6.68951, "alpha":-0.28116, "fx":[101.53947,103.74414,104.00589,101.76267], "fy":[-114.89217,-112.906,-112.65927,-114.68892]}, - {"t":0.22424, "x":1.76753, "y":7.18159, "heading":-0.94868, "vx":1.35493, "vy":-1.50061, "omega":-0.06727, "ax":6.0405, "ay":-6.68796, "alpha":-0.26661, "fx":[101.58447,103.67506,103.92707,101.80197], "fy":[-114.81125,-112.92769,-112.68966,-114.61236]}, - {"t":0.26909, "x":1.83437, "y":7.10757, "heading":-0.9517, "vx":1.62584, "vy":-1.80056, "omega":-0.07922, "ax":6.03912, "ay":-6.686, "alpha":-0.2472, "fx":[101.64285,103.58128,103.81915,101.8512], "fy":[-114.70446,-112.95783,-112.73258,-114.5129]}, - {"t":0.31394, "x":1.91336, "y":7.02009, "heading":-0.95525, "vx":1.89669, "vy":-2.10042, "omega":-0.09031, "ax":6.03704, "ay":-6.68335, "alpha":-0.21996, "fx":[101.72385,103.44885,103.66457,101.91633], "fy":[-114.55497,-113.00047,-112.79549,-114.3767]}, - {"t":0.35879, "x":2.00449, "y":6.91917, "heading":-0.9593, "vx":2.16744, "vy":-2.40016, "omega":-0.10018, "ax":6.03379, "ay":-6.67945, "alpha":-0.17894, "fx":[101.84571,103.24942,103.42805,102.00907], "fy":[-114.32906,-113.06387,-112.89323,-114.17612]}, - {"t":0.40364, "x":2.10777, "y":6.8048, "heading":-0.9638, "vx":2.43805, "vy":-2.69972, "omega":-0.1082, "ax":6.02817, "ay":-6.67297, "alpha":-0.11017, "fx":[102.05083,102.91568,103.02691,102.15632], "fy":[-113.94711,-113.16735,-113.06012,-113.84658]}, - {"t":0.44849, "x":2.22318, "y":6.67701, "heading":-0.96865, "vx":2.70841, "vy":-2.999, "omega":-0.11314, "ax":6.0165, "ay":-6.65979, "alpha":0.02865, "fx":[102.46595,102.24061,102.21197,102.43693], "fy":[-113.1653,-113.36861,-113.39678,-113.19392]}, - {"t":0.49334, "x":2.3507, "y":6.53581, "heading":-0.97372, "vx":2.97824, "vy":-3.29768, "omega":-0.11186, "ax":5.979, "ay":-6.61791, "alpha":0.45208, "fx":[103.71419,100.13723,99.73475,103.21811], "fy":[-110.70957,-113.94423,-114.37222,-111.2489]}, - {"t":0.53819, "x":2.49028, "y":6.38126, "heading":-0.97874, "vx":3.24639, "vy":-3.59449, "omega":-0.09158, "ax":0.94434, "ay":-1.02689, "alpha":2.03591, "fx":[17.7863,8.44343,14.44875,23.57349], "fy":[-9.83514,-16.19343,-25.04409,-18.79575]}, - {"t":0.58303, "x":2.63683, "y":6.21902, "heading":-0.98285, "vx":3.28874, "vy":-3.64054, "omega":-0.00027, "ax":-0.93377, "ay":1.03674, "alpha":-2.03607, "fx":[-17.63814,-8.26615,-14.23733,-23.39111], "fy":[10.01122,16.33018,25.20295,18.99429]}, - {"t":0.62788, "x":2.78338, "y":6.05679, "heading":-0.98286, "vx":3.24686, "vy":-3.59405, "omega":-0.09159, "ax":-5.97878, "ay":6.61807, "alpha":-0.45262, "fx":[-103.7265,-100.15004,-99.71428,-103.19832], "fy":[110.69773,113.93193,114.38908,111.26693]}, - {"t":0.67273, "x":2.92299, "y":5.90226, "heading":-0.98697, "vx":2.97872, "vy":-3.29723, "omega":-0.11189, "ax":-6.01652, "ay":6.65974, "alpha":-0.02876, "fx":[-102.46862,-102.24298,-102.21009,-102.43535], "fy":[113.16242,113.36596,113.39797,113.19488]}, - {"t":0.71758, "x":3.05053, "y":5.76108, "heading":-0.99199, "vx":2.70889, "vy":-2.99855, "omega":-0.11318, "ax":-6.02833, "ay":6.6728, "alpha":0.11021, "fx":[-102.04285,-102.90477,-103.04035,-102.17296], "fy":[113.95374,113.17692,113.04754,113.83115]}, - {"t":0.76243, "x":3.16596, "y":5.63331, "heading":-0.99706, "vx":2.43853, "vy":-2.69928, "omega":-0.10824, "ax":-6.03408, "ay":6.67916, "alpha":0.17907, "fx":[-101.82703,-103.22472,-103.45624,-102.04423], "fy":[114.34515,113.08614,112.86715,114.14417]}, - {"t":0.80728, "x":3.26925, "y":5.51896, "heading":-1.00192, "vx":2.1679, "vy":-2.39973, "omega":-0.1002, "ax":-6.03748, "ay":6.68294, "alpha":0.22014, "fx":[-101.69527,-103.41106,-103.70701,-101.96974], "fy":[114.57978,113.03482,112.75628,114.32856]}, - {"t":0.85213, "x":3.36041, "y":5.41806, "heading":-1.00641, "vx":1.89713, "vy":-2.10001, "omega":-0.09033, "ax":-6.03971, "ay":6.68544, "alpha":0.24742, "fx":[-101.60569,-103.53179,-103.87524,-101.92223], "fy":[114.73681,113.00297,112.68073,114.44915]}, - {"t":0.89698, "x":3.43942, "y":5.3306, "heading":-1.01046, "vx":1.62626, "vy":-1.80018, "omega":-0.07924, "ax":-6.04128, "ay":6.68722, "alpha":0.26684, "fx":[-101.54042,-103.61569,-103.99616,-101.88983], "fy":[114.84963,112.98195,112.62574,114.53372]}, - {"t":0.94183, "x":3.50628, "y":5.25659, "heading":-1.01401, "vx":1.35531, "vy":-1.50026, "omega":-0.06727, "ax":-6.04245, "ay":6.68856, "alpha":0.28137, "fx":[-101.49057,-103.67707,-104.08732,-101.8665], "fy":[114.93479,112.96738,112.58388,114.59616]}, - {"t":0.98667, "x":3.56099, "y":5.19603, "heading":-1.01703, "vx":1.08431, "vy":-1.20029, "omega":-0.05465, "ax":-6.04335, "ay":6.68961, "alpha":0.29265, "fx":[-101.45122,-103.72384,-104.15841,-101.84888], "fy":[115.00137,112.95681,112.55102,114.6442]}, - {"t":1.03152, "x":3.60354, "y":5.14893, "heading":-1.01948, "vx":0.81328, "vy":-0.90027, "omega":-0.04152, "ax":-6.04405, "ay":6.69045, "alpha":0.30167, "fx":[-101.41943,-103.76073,-104.21526,-101.83492], "fy":[115.05482,112.94874,112.52469,114.68246]}, - {"t":1.07637, "x":3.63394, "y":5.11528, "heading":-1.02134, "vx":0.54221, "vy":-0.60021, "omega":-0.02799, "ax":-6.04462, "ay":6.69115, "alpha":0.30903, "fx":[-101.39335,-103.79077,-104.26156,-101.82331], "fy":[115.09855,112.94222,112.5033,114.71392]}, - {"t":1.12122, "x":3.65217, "y":5.09509, "heading":-1.0226, "vx":0.27111, "vy":-0.30012, "omega":-0.01413, "ax":-6.04507, "ay":6.69174, "alpha":0.31516, "fx":[-101.37161,-103.81572,-104.29953,-101.8129], "fy":[115.13494,112.93683,112.486,114.74075]}, - {"t":1.16607, "x":3.65825, "y":5.08836, "heading":-1.02323, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.62272, "y":7.46768, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.25789, "ay":-13.58908, "alpha":-1.50169, "fx":[91.17642,104.88507,106.76588,91.19886], "fy":[-124.99165,-113.73423,-111.94248,-124.94955]}, + {"t":0.04442, "x":1.63382, "y":7.45428, "heading":-0.94248, "vx":0.50011, "vy":-0.60367, "omega":-0.06671, "ax":11.25986, "ay":-13.58854, "alpha":-1.39187, "fx":[91.69322,104.46525,106.1537,91.78303], "fy":[-124.57221,-114.07559,-112.47436,-124.47675]}, + {"t":0.08885, "x":1.66715, "y":7.41405, "heading":-0.94544, "vx":1.00031, "vy":-1.20731, "omega":-0.12854, "ax":11.26122, "ay":-13.58818, "alpha":-1.21894, "fx":[92.48566,103.76102,105.19786,92.69827], "fy":[-123.91953,-114.64606,-113.29345,-123.72729]}, + {"t":0.13327, "x":1.7227, "y":7.34701, "heading":-0.95115, "vx":1.50057, "vy":-1.81094, "omega":-0.18269, "ax":11.26096, "ay":-13.58683, "alpha":-0.90659, "fx":[93.94329,102.44792,103.46063,94.2816], "fy":[-122.69605,-115.69176,-114.75012,-122.40113]}, + {"t":0.17769, "x":1.80047, "y":7.25316, "heading":-0.95927, "vx":2.00081, "vy":-2.41451, "omega":-0.22296, "ax":11.2516, "ay":-13.57581, "alpha":-0.1714, "fx":[97.55101,99.20919,99.36402,97.68194], "fy":[-119.53916,-118.16743,-118.0258,-119.42082]}, + {"t":0.22212, "x":1.90045, "y":7.1325, "heading":-0.96917, "vx":2.50064, "vy":-3.01759, "omega":-0.23058, "ax":11.02403, "ay":-13.30146, "alpha":3.85764, "fx":[118.37978,76.78415,79.92677,110.75049], "fy":[-96.60905,-132.01101,-130.84939,-106.08164]}, + {"t":0.26654, "x":2.02242, "y":6.98533, "heading":-0.97941, "vx":2.99037, "vy":-3.60848, "omega":-0.05921, "ax":0.24438, "ay":-0.25545, "alpha":1.24392, "fx":[2.91583,-3.04315,1.36758,7.31321], "fy":[3.04547,-0.95598,-7.51286,-3.51737]}, + {"t":0.31096, "x":2.1555, "y":6.82477, "heading":-0.98204, "vx":3.00122, "vy":-3.61983, "omega":-0.00395, "ax":0.00338, "ay":0.00275, "alpha":0.00031, "fx":[0.02976,0.02829,0.02937,0.03084], "fy":[0.02539,0.0244,0.02278,0.02377]}, + {"t":0.35538, "x":2.28883, "y":6.66397, "heading":-0.98222, "vx":3.00137, "vy":-3.61971, "omega":-0.00394, "ax":0.0016, "ay":0.00135, "alpha":-0.00003, "fx":[0.01399,0.01413,0.01403,0.01389], "fy":[0.01169,0.01178,0.01194,0.01184]}, + {"t":0.39981, "x":2.42216, "y":6.50318, "heading":-0.9824, "vx":3.00144, "vy":-3.61965, "omega":-0.00394, "ax":0.0009, "ay":0.00077, "alpha":-0.00003, "fx":[0.00787,0.008,0.0079,0.00778], "fy":[0.00661,0.00669,0.00683,0.00674]}, + {"t":0.44423, "x":2.55549, "y":6.34238, "heading":-0.98257, "vx":3.00148, "vy":-3.61961, "omega":-0.00394, "ax":0.00041, "ay":0.00036, "alpha":-0.00002, "fx":[0.00355,0.00366,0.00358,0.00347], "fy":[0.00301,0.00309,0.00321,0.00313]}, + {"t":0.48865, "x":2.68883, "y":6.18159, "heading":-0.98275, "vx":3.0015, "vy":-3.6196, "omega":-0.00394, "ax":0.00002, "ay":0.00003, "alpha":-0.00002, "fx":[0.00017,0.00027,0.0002,0.0001], "fy":[0.00021,0.00027,0.00038,0.00032]}, + {"t":0.53308, "x":2.82216, "y":6.02079, "heading":-0.98292, "vx":3.0015, "vy":-3.6196, "omega":-0.00394, "ax":-0.00031, "ay":-0.00024, "alpha":-0.00002, "fx":[-0.00272,-0.00263,-0.0027,-0.00279], "fy":[-0.0022,-0.00214,-0.00204,-0.0021]}, + {"t":0.5775, "x":2.9555, "y":5.86, "heading":-0.9831, "vx":3.00149, "vy":-3.61961, "omega":-0.00394, "ax":-0.00098, "ay":-0.00072, "alpha":-0.00036, "fx":[-0.00878,-0.00706,-0.00833,-0.01005], "fy":[-0.00784,-0.00669,-0.00479,-0.00594]}, + {"t":0.62192, "x":3.08883, "y":5.6992, "heading":-0.98327, "vx":3.00144, "vy":-3.61964, "omega":-0.00396, "ax":-0.23469, "ay":0.26739, "alpha":-1.25284, "fx":[-2.85714,3.16027,-1.25671,-7.26072], "fy":[-2.97587,1.03133,7.65183,3.65119]}, + {"t":0.66635, "x":3.22194, "y":5.53867, "heading":-0.98345, "vx":2.99102, "vy":-3.60776, "omega":-0.05961, "ax":-11.02195, "ay":13.30056, "alpha":-3.87457, "fx":[-118.65079,-77.01474,-79.56619,-110.53659], "fy":[96.28164,131.86556,131.06537,106.30699]}, + {"t":0.71077, "x":3.34393, "y":5.39153, "heading":-0.98609, "vx":2.50139, "vy":-3.01691, "omega":-0.23173, "ax":-11.25257, "ay":13.5749, "alpha":0.17045, "fx":[-97.54271,-99.18623,-99.38868,-97.72239], "fy":[119.54478,118.18581,118.00423,119.38665]}, + {"t":0.75519, "x":3.44395, "y":5.2709, "heading":-0.99639, "vx":2.00152, "vy":-2.41387, "omega":-0.22416, "ax":-11.26298, "ay":13.58497, "alpha":0.91018, "fx":[-93.74065,-102.2437,-103.68061,-94.53947], "fy":[122.84963,115.87224,114.55157,122.20059]}, + {"t":0.79962, "x":3.52175, "y":5.17708, "heading":-1.00635, "vx":1.50118, "vy":-1.81038, "omega":-0.18373, "ax":-11.26438, "ay":13.58527, "alpha":1.22517, "fx":[-92.1122,-103.39143,-105.58197,-93.16774], "fy":[124.19605,114.97972,112.93618,123.37255]}, + {"t":0.84404, "x":3.57732, "y":5.11006, "heading":-1.01451, "vx":1.00078, "vy":-1.20688, "omega":-0.1293, "ax":-11.26437, "ay":13.58442, "alpha":1.39987, "fx":[-91.1914,-103.97008,-106.66463,-92.42676], "fy":[124.93874,114.5274,111.99073,123.9979]}, + {"t":0.88846, "x":3.61066, "y":5.06985, "heading":-1.02025, "vx":0.50038, "vy":-0.60342, "omega":-0.06712, "ax":-11.26403, "ay":13.58355, "alpha":1.51083, "fx":[-90.60016,-104.31659,-107.36089,-91.96339], "fy":[125.40878,114.25617,111.37273,124.3864]}, + {"t":0.93288, "x":3.62178, "y":5.05644, "heading":-1.02323, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/autos.chor b/src/main/deploy/choreo/autos.chor index c5c4141b..ba331956 100644 --- a/src/main/deploy/choreo/autos.chor +++ b/src/main/deploy/choreo/autos.chor @@ -65,16 +65,16 @@ }, "bumper":{ "front":{ - "exp":"41 cm", - "val":0.41 + "exp":"370 mm", + "val":0.37 }, "side":{ - "exp":"37.5 cm", - "val":0.375 + "exp":"355 mm", + "val":0.355 }, "back":{ - "exp":"41 cm", - "val":0.41 + "exp":"370 mm", + "val":0.37 } }, "differentialTrackWidth":{ From 1518951db67f4facba0ed6113c771fe99360502d Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Thu, 30 Jan 2025 15:37:29 +0800 Subject: [PATCH 03/19] drive limiting --- .../frc2025/subsystems/drive/Drive.java | 38 +++++++++++++------ 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 0d3dd5f0..bb1f2d9a 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -80,6 +80,11 @@ public class Drive extends SubsystemBase { private final PIDController yController = new PIDController(10.0, 0.0, 0.0); private final PIDController headingController = new PIDController(7.5, 0.0, 0.0); + private final SlewRateLimiter xLimiter = + new SlewRateLimiter(5 * 0.02); // Limits acceleration to 3 mps + private final SlewRateLimiter yLimiter = new SlewRateLimiter(5 * 0.02); + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(0 * 0.02); + RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner(); public Drive( @@ -167,11 +172,11 @@ public void periodic() { } // Update gyro angle - if (gyroInputs.connected) { + if (gyroInputs.connected && i >= 0 && i < gyroInputs.odometryYawPositions.length) { // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; } else { - // Use the angle delta from the kinematics and module deltas + // Fallback to calculating the angle delta Twist2d twist = kinematics.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } @@ -270,25 +275,34 @@ public Command joystickDrive( DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { return run( () -> { - // Get linear velocity - Translation2d linearVelocity = - getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double xSpeed = xSupplier.getAsDouble(); - // Apply rotation deadband - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + double ySpeed = ySupplier.getAsDouble(); + + double omegaSpeed = omegaSupplier.getAsDouble(); + + Translation2d linearVelocity = getLinearVelocityFromJoysticks(xSpeed, ySpeed); + + double omega = MathUtil.applyDeadband(omegaSpeed, DEADBAND); - // Square rotation value for more precise control omega = Math.copySign(omega * omega, omega); - // Convert to field relative speeds & send command + Logger.recordOutput("Drive/OmegaUnlimited", omega * getMaxAngularSpeedRadPerSec()); + + var limited = omegaLimiter.calculate(omega * getMaxAngularSpeedRadPerSec()); + ChassisSpeeds speeds = new ChassisSpeeds( - linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(), - omega * getMaxAngularSpeedRadPerSec()); + xLimiter.calculate(linearVelocity.getX() * getMaxLinearSpeedMetersPerSec()), + yLimiter.calculate(linearVelocity.getY() * getMaxLinearSpeedMetersPerSec()), + limited); + + Logger.recordOutput("Drive/OmegaLimited", limited); + boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, isFlipped ? getRotation().plus(Rotation2d.kPi) : getRotation())); From 336c726e657cb2f2951719780d67b821c3b665d0 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Thu, 30 Jan 2025 18:24:35 +0800 Subject: [PATCH 04/19] intake stuff --- .../java/org/curtinfrc/frc2025/Robot.java | 67 +++++++++++++++---- .../frc2025/subsystems/drive/Drive.java | 13 ++-- .../frc2025/subsystems/elevator/Elevator.java | 6 ++ .../subsystems/intake/IntakeConstants.java | 4 +- 4 files changed, 67 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 312177fe..a1f665ee 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.Set; import org.curtinfrc.frc2025.Constants.Mode; import org.curtinfrc.frc2025.Constants.Setpoints; import org.curtinfrc.frc2025.generated.TunerConstants; @@ -269,17 +270,39 @@ public Robot() { intake.setDefaultCommand(intake.intake(intakeVolts)); ejector.setDefaultCommand(ejector.stop()); - elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); - intake.backSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor.negate())); - intake.backSensor.whileTrue(intake.intake(intakeVolts / 2)); - intake.backSensor.onTrue( - ejector - .eject(300) - .until(intake.backSensor.negate()) - .andThen(ejector.eject(-60).until(intake.backSensor))); - ejector.sensor.whileTrue(intake.stop()); - - // controller.x().whileTrue(ejector.eject(150).until(ejector.sensor.negate())); + // elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); + controller + .rightBumper() + .negate() + .and(controller.leftBumper().negate()) + .onTrue(elevator.goToSetpoint(Setpoints.COLLECT)); + intake + .backSensor + .negate() + .or(elevator.isNotAtCollect) + .and(intake.frontSensor) + .whileTrue(intake.intake(intakeVolts)); + intake + .backSensor + .or(elevator.isNotAtCollect) + .onTrue( + ejector + .eject(200) + .until( + intake.frontSensor.negate().and(intake.backSensor.or(elevator.isNotAtCollect))) + .andThen( + ejector + .eject(-3) + .until(intake.frontSensor) + .andThen(ejector.eject(3).until(intake.frontSensor.negate())))); + intake + .backSensor + .or(elevator.isNotAtCollect) + .and(intake.frontSensor.negate()) + .whileTrue(intake.stop()); + + // intake.backSensor.or(elevator.isNotAtCollect).and(intake.frontSensor).whileTrue(ejector.eject(50)); + controller.x().whileTrue(ejector.eject(5000)); // Lock to 0° when A button is held @@ -299,8 +322,12 @@ public Robot() { drive) .ignoringDisable(true)); - controller.rightBumper().whileTrue(elevator.goToSetpoint(Setpoints.L3)); - controller.leftBumper().whileTrue(elevator.goToSetpoint(Setpoints.L2)); + controller + .rightBumper() + .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L3), Set.of(elevator))); + controller + .leftBumper() + .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L2), Set.of(elevator))); } /** This function is called periodically during all modes. */ @@ -353,7 +380,19 @@ public void teleopInit() {} /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + if (ejector.getCurrentCommand() != null) { + Logger.recordOutput("EjectorCommand", ejector.getCurrentCommand().getName()); + } else { + Logger.recordOutput("EjectorCommand", "null"); + } + + if (intake.getCurrentCommand() != null) { + Logger.recordOutput("IntakeCommand", intake.getCurrentCommand().getName()); + } else { + Logger.recordOutput("IntakeCommand", "null"); + } + } /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index bb1f2d9a..ad161b84 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -80,10 +80,9 @@ public class Drive extends SubsystemBase { private final PIDController yController = new PIDController(10.0, 0.0, 0.0); private final PIDController headingController = new PIDController(7.5, 0.0, 0.0); - private final SlewRateLimiter xLimiter = - new SlewRateLimiter(5 * 0.02); // Limits acceleration to 3 mps - private final SlewRateLimiter yLimiter = new SlewRateLimiter(5 * 0.02); - private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(0 * 0.02); + private final SlewRateLimiter xLimiter = new SlewRateLimiter(7); // Limits acceleration to 3 mps + private final SlewRateLimiter yLimiter = new SlewRateLimiter(7); + // private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1); RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner(); @@ -289,15 +288,15 @@ public Command joystickDrive( Logger.recordOutput("Drive/OmegaUnlimited", omega * getMaxAngularSpeedRadPerSec()); - var limited = omegaLimiter.calculate(omega * getMaxAngularSpeedRadPerSec()); + // var limited = omegaLimiter.calculate(omega * getMaxAngularSpeedRadPerSec()); ChassisSpeeds speeds = new ChassisSpeeds( xLimiter.calculate(linearVelocity.getX() * getMaxLinearSpeedMetersPerSec()), yLimiter.calculate(linearVelocity.getY() * getMaxLinearSpeedMetersPerSec()), - limited); + omega * getMaxAngularSpeedRadPerSec()); - Logger.recordOutput("Drive/OmegaLimited", limited); + // Logger.recordOutput("Drive/OmegaLimited", limited); boolean isFlipped = DriverStation.getAlliance().isPresent() diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java index 7b915541..1d2eed5c 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java @@ -13,6 +13,9 @@ public class Elevator extends SubsystemBase { private final ElevatorIO io; private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); private final PIDController pid = new PIDController(kP, 0, kD); + private Setpoints setpoint = Setpoints.COLLECT; + + public final Trigger isNotAtCollect = new Trigger(() -> setpoint != Setpoints.COLLECT); public Elevator(ElevatorIO io) { this.io = io; @@ -22,11 +25,14 @@ public Elevator(ElevatorIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); + Logger.recordOutput("Elevator/setpoint", setpoint); + Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect); } public Trigger atSetpoint = new Trigger(pid::atSetpoint); public Command goToSetpoint(Setpoints point) { + setpoint = point; return run( () -> { var out = pid.calculate(inputs.positionRotations, point.elevatorSetpoint()); diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java index 2b25ef2a..4087b2c8 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/IntakeConstants.java @@ -6,6 +6,6 @@ public class IntakeConstants { public static double motorReduction = 1.0; public static double intakeMoi = 1.0; public static int intakeMotorId = 32; - public static int intakeFrontSensorPort = 9; - public static int intakeBackSensorPort = 8; + public static int intakeFrontSensorPort = 8; + public static int intakeBackSensorPort = 9; } From f301794fc7edc901b943c00ef12e15accff37930 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Thu, 30 Jan 2025 19:43:55 +0800 Subject: [PATCH 05/19] actually made somethings work --- .../java/org/curtinfrc/frc2025/Constants.java | 2 +- .../java/org/curtinfrc/frc2025/Robot.java | 35 +++++++------------ .../subsystems/ejector/EjectorConstants.java | 6 ++-- 3 files changed, 17 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index c3c28631..ef8bb5c4 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -75,7 +75,7 @@ public enum Setpoints { // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO // TODO actually subtract L2(13.2 - 2.5, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), - L3(35.2 - 2.5, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); + L3(35.327, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); private final double _elevatorSetpoint; private final List _tagIdsBlue; diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index a1f665ee..6de550f2 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -268,42 +268,33 @@ public Robot() { () -> controller.getLeftX(), () -> -controller.getRightX())); - intake.setDefaultCommand(intake.intake(intakeVolts)); - ejector.setDefaultCommand(ejector.stop()); // elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); controller .rightBumper() .negate() .and(controller.leftBumper().negate()) .onTrue(elevator.goToSetpoint(Setpoints.COLLECT)); + + intake.setDefaultCommand(intake.intake(intakeVolts)); + ejector.setDefaultCommand(ejector.stop()); + intake .backSensor - .negate() - .or(elevator.isNotAtCollect) - .and(intake.frontSensor) - .whileTrue(intake.intake(intakeVolts)); + .and(intake.frontSensor.negate()) + .whileTrue( + Commands.parallel(intake.intake(intakeVolts), ejector.eject(5)).withName("front")); intake .backSensor - .or(elevator.isNotAtCollect) - .onTrue( - ejector - .eject(200) - .until( - intake.frontSensor.negate().and(intake.backSensor.or(elevator.isNotAtCollect))) - .andThen( - ejector - .eject(-3) - .until(intake.frontSensor) - .andThen(ejector.eject(3).until(intake.frontSensor.negate())))); + .and(intake.frontSensor) + .whileTrue( + Commands.parallel(intake.intake(intakeVolts), ejector.eject(5)) + .withName("front and back")); intake .backSensor - .or(elevator.isNotAtCollect) .and(intake.frontSensor.negate()) - .whileTrue(intake.stop()); - - // intake.backSensor.or(elevator.isNotAtCollect).and(intake.frontSensor).whileTrue(ejector.eject(50)); + .whileTrue(Commands.parallel(intake.stop(), ejector.stop()).withName("not front and back")); - controller.x().whileTrue(ejector.eject(5000)); + controller.x().whileTrue(ejector.eject(1000)); // Lock to 0° when A button is held controller diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java index 15379082..c93d7e96 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java @@ -6,10 +6,10 @@ public class EjectorConstants { public static final int currentLimit = 60; public static final double ejectorMoi = 0.025; public static final double ejectorReduction = 1; - public static final double kP = 0.01; + public static final double kP = 0; public static final double kD = 0; - public static final double kS = 0; + public static final double kS = 0.8; // public static final double kV = 473; - public static final double kV = 0; + public static final double kV = 0.11; public static final double kA = 0; } From 8b1f43f5b33a2df5b0b6fd484b3385b1daa2b0db Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Fri, 31 Jan 2025 17:16:01 +0800 Subject: [PATCH 06/19] basic cycles --- .../java/org/curtinfrc/frc2025/Constants.java | 4 +- .../java/org/curtinfrc/frc2025/Robot.java | 24 ++- .../frc2025/subsystems/elevator/Elevator.java | 16 +- .../elevator/ElevatorConstants.java | 57 ++++-- .../subsystems/elevator/ElevatorIO.java | 24 +++ .../subsystems/elevator/ElevatorIONEO.java | 37 +++- .../subsystems/elevator/ElevatorIONEOMPC.java | 187 ++++++++++++++++++ .../java/org/curtinfrc/frc2025/util/Util.java | 20 ++ 8 files changed, 339 insertions(+), 30 deletions(-) create mode 100644 src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEOMPC.java create mode 100644 src/main/java/org/curtinfrc/frc2025/util/Util.java diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index ef8bb5c4..6519fc52 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -74,8 +74,8 @@ public enum Setpoints { COLLECT(0, List.of(13, 12), List.of(1, 2)), // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO // TODO actually subtract - L2(13.2 - 2.5, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), - L3(35.327, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); + L2(11, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), + L3(32.8, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); private final double _elevatorSetpoint; private final List _tagIdsBlue; diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 6de550f2..3178c7ee 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -40,6 +40,7 @@ import org.curtinfrc.frc2025.subsystems.vision.VisionIO; import org.curtinfrc.frc2025.subsystems.vision.VisionIOLimelight; import org.curtinfrc.frc2025.subsystems.vision.VisionIOLimelightGamepiece; +import org.curtinfrc.frc2025.subsystems.vision.VisionIOPhotonVision; import org.curtinfrc.frc2025.subsystems.vision.VisionIOPhotonVisionSim; import org.curtinfrc.frc2025.subsystems.vision.VisionIOQuestNav; import org.curtinfrc.frc2025.util.AutoChooser; @@ -158,7 +159,7 @@ public Robot() { new Vision( drive::addVisionMeasurement, new VisionIO() {}, - new VisionIO() {}, + new VisionIOPhotonVision(camera1Name, robotToCamera1) {}, new VisionIO() {}); elevator = new Elevator(new ElevatorIONEO()); intake = new Intake(new IntakeIONEO()); @@ -268,7 +269,7 @@ public Robot() { () -> controller.getLeftX(), () -> -controller.getRightX())); - // elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); + elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); controller .rightBumper() .negate() @@ -294,7 +295,16 @@ public Robot() { .and(intake.frontSensor.negate()) .whileTrue(Commands.parallel(intake.stop(), ejector.stop()).withName("not front and back")); - controller.x().whileTrue(ejector.eject(1000)); + elevator.toZero.whileTrue( + elevator + .zero() + .until(elevator.toZero.negate()) + .ignoringDisable(true) + .withName("ElevatorZero")); + // elevator.isNotAtCollect.negate().whileTrue(new PrintCommand("pls pls work")); + + controller.b().onTrue(elevator.zero()); + controller.rightTrigger().whileTrue(ejector.eject(1000)); // Lock to 0° when A button is held controller @@ -319,6 +329,8 @@ public Robot() { controller .leftBumper() .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L2), Set.of(elevator))); + + controller.pov(0).whileTrue(drive.autoAlign(Setpoints.COLLECT)); } /** This function is called periodically during all modes. */ @@ -345,6 +357,12 @@ public void robotPeriodic() { autoChooser.periodic(); + if (elevator.getCurrentCommand() != null) { + Logger.recordOutput("ElevatorCommand", elevator.getCurrentCommand().getName()); + } else { + Logger.recordOutput("ElevatorCommand", "null"); + } + // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java index 1d2eed5c..742ba4fd 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java @@ -15,7 +15,8 @@ public class Elevator extends SubsystemBase { private final PIDController pid = new PIDController(kP, 0, kD); private Setpoints setpoint = Setpoints.COLLECT; - public final Trigger isNotAtCollect = new Trigger(() -> setpoint != Setpoints.COLLECT); + public final Trigger isNotAtCollect = new Trigger(() -> inputs.point != Setpoints.COLLECT); + public final Trigger toZero = new Trigger(() -> inputs.touchSensor); public Elevator(ElevatorIO io) { this.io = io; @@ -25,11 +26,12 @@ public Elevator(ElevatorIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); + Logger.recordOutput("Elevator/toZero", toZero.getAsBoolean()); Logger.recordOutput("Elevator/setpoint", setpoint); - Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect); + // Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect); } - public Trigger atSetpoint = new Trigger(pid::atSetpoint); + // public Trigger atSetpoint = new Trigger(pid::atSetpoint); public Command goToSetpoint(Setpoints point) { setpoint = point; @@ -42,6 +44,14 @@ public Command goToSetpoint(Setpoints point) { }); } + public Command zero() { + return run(() -> io.zero()); + } + + // public Command goToSetpoint(Setpoints point) { + // return run(() -> io.goToSetpoint(point)); + // } + public Command stop() { return runOnce(() -> io.setVoltage(0)); } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java index a7df6408..f9bae532 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java @@ -1,20 +1,49 @@ package org.curtinfrc.frc2025.subsystems.elevator; +// public class ElevatorConstants { +// public static int motorCount = 1; +// public static int distanceSensorPort = 99; +// public static int motorPort = 31; +// public static double tolerance = 0.01; + +// public static double maxVel = 5676; +// public static double maxAccel = 0; +// public static double allowedErr = 0; + +// // TODO: TUNE PID +// public static double kP = 0.5; +// public static double kI = 0; +// public static double kD = 0; +// public static double kMinOutput = 0; +// public static double kMaxOutput = 0; +// public static double kV = 473; +// } + +// package org.curtinfrc.frc2025.subsystems.elevator; + public class ElevatorConstants { - public static int motorCount = 1; - public static int distanceSensorPort = 99; - public static int motorPort = 31; - public static double tolerance = 0.01; + // reduction for elevator neo is 10.7 : 1 + // TODO: GET VALUES FOR SOME OF THESE + public static int motorCount = 1; // Number of motors in the elevator system + // public static int distanceSensorPort = 0; // Port for distance sensor (if used) + public static int motorPort = 31; // Motor controller port + public static double tolerance = 0.01; // Velocity tolerance (rotations per second) + public static double positionTolerance = 0.002; // Position tolerance (rotations) + public static double friction = 0.05; // Estimated system friction (Nm) TODO + public static double stableVelocityThreshold = + 0.005; // Threshold for velocity to be considered stable - public static double maxVel = 5676; - public static double maxAccel = 0; - public static double allowedErr = 0; + // Motion profile constraints + public static double maxVel = 5676 / 10.7; // Max velocity in RPM + public static double maxAccel = 38197; // Max acceleration in RPM per second + public static double allowedErr = 0.002; // Allowable position error for stability - // TODO: TUNE PID - public static double kP = 0.5; - public static double kI = 0; - public static double kD = 0; - public static double kMinOutput = 0; - public static double kMaxOutput = 0; - public static double kV = 473; + // Model Predictive Control Gains + public static double kP = 0.5; // Proportional gain TODO + public static double kI = 0; // Integral gain TODO + public static double kD = 0; // Derivative gain TODO + public static double kA = 0; + public static double kMinOutput = -12.0; // Minimum voltage output + public static double kMaxOutput = 12.0; // Maximum voltage output + public static double kV = 0; // Voltage constant (i for it from the datasheet) } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java index c1de5de9..11b68717 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java @@ -1,5 +1,6 @@ package org.curtinfrc.frc2025.subsystems.elevator; +import org.curtinfrc.frc2025.Constants.Setpoints; import org.littletonrobotics.junction.AutoLog; public interface ElevatorIO { @@ -10,9 +11,32 @@ public static class ElevatorIOInputs { public double currentAmps; public double positionRotations; public double angularVelocityRotationsPerMinute; + public double distanceSensorReading; + public double encoderReading; + public Setpoints point; + public double pointRot; + public double motorVoltage; + public double motorCurrent; + public double motorTemp; + public double motorVelocity; + public double positionError; + public double velocityError; + public boolean stable; + public int predictionHorizon; + public double predictedPosition; + public double predictedVelocity; + public boolean touchSensor; } public default void updateInputs(ElevatorIOInputs inputs) {} public default void setVoltage(double volts) {} + + public default void goToSetpoint(Setpoints point) {} + + public default Setpoints getSetpoint() { + return Setpoints.COLLECT; + } + + public default void zero() {} } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java index abc99f9a..c86bdb6e 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java @@ -3,17 +3,23 @@ import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.currentLimit; import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.*; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; +import org.curtinfrc.frc2025.Constants.Setpoints; import org.curtinfrc.frc2025.util.SparkUtil; public class ElevatorIONEO implements ElevatorIO { - private final SparkMax neo = new SparkMax(motorPort, MotorType.kBrushless); - // private final DigitalInput sensor = new DigitalInput(sensorPort); + protected final SparkMax elevatorMotor = + new SparkMax(ElevatorConstants.motorPort, MotorType.kBrushless); + protected final RelativeEncoder elevatorEncoder = elevatorMotor.getEncoder(); + protected Setpoints setpoint = Setpoints.COLLECT; + protected DigitalInput touch = new DigitalInput(0); public ElevatorIONEO() { SparkMaxConfig config = new SparkMaxConfig(); @@ -22,19 +28,34 @@ public ElevatorIONEO() { SparkUtil.tryUntilOk( 5, () -> - neo.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + elevatorMotor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); } @Override public void updateInputs(ElevatorIOInputs inputs) { - inputs.appliedVolts = neo.getBusVoltage() * neo.getAppliedOutput(); - inputs.currentAmps = neo.getOutputCurrent(); - inputs.positionRotations = neo.getEncoder().getPosition(); - inputs.angularVelocityRotationsPerMinute = neo.getEncoder().getVelocity(); + inputs.appliedVolts = elevatorMotor.getBusVoltage() * elevatorMotor.getAppliedOutput(); + inputs.currentAmps = elevatorMotor.getOutputCurrent(); + inputs.positionRotations = elevatorMotor.getEncoder().getPosition(); + inputs.angularVelocityRotationsPerMinute = elevatorMotor.getEncoder().getVelocity(); + + inputs.encoderReading = elevatorEncoder.getPosition(); + inputs.motorCurrent = elevatorMotor.getOutputCurrent(); + inputs.motorTemp = elevatorMotor.getMotorTemperature(); + inputs.motorVelocity = elevatorEncoder.getVelocity(); + + inputs.point = this.setpoint; + + inputs.touchSensor = !touch.get(); } @Override public void setVoltage(double volts) { - neo.setVoltage(volts); + elevatorMotor.setVoltage(volts); + } + + @Override + public void zero() { + elevatorEncoder.setPosition(0); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEOMPC.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEOMPC.java new file mode 100644 index 00000000..82856437 --- /dev/null +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEOMPC.java @@ -0,0 +1,187 @@ +package org.curtinfrc.frc2025.subsystems.elevator; + +import org.curtinfrc.frc2025.Constants.Setpoints; +// import org.curtinfrc.frc2025.util.util; +import org.curtinfrc.frc2025.util.Util; + +public class ElevatorIONEOMPC extends ElevatorIONEO { + private final double controlLoopPeriod = 0.02; + + private Setpoints set = Setpoints.COLLECT; + private double computedVoltage = 0.0; + private int predictionHorizon = 20; + private double[] referenceTrajectory = new double[20]; + private double velocityError = 0; + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.distanceSensorReading = 0.0; + inputs.encoderReading = elevatorEncoder.getPosition(); + inputs.point = set; + inputs.pointRot = set.elevatorSetpoint(); + + inputs.motorVoltage = computedVoltage; + + inputs.motorCurrent = elevatorMotor.getOutputCurrent(); + inputs.motorTemp = elevatorMotor.getMotorTemperature(); + inputs.motorVelocity = elevatorEncoder.getVelocity() / 60.0; + + inputs.positionError = Math.abs(elevatorEncoder.getPosition() - set.elevatorSetpoint()); + + // inputs.stable = isStable(); + + inputs.predictionHorizon = predictionHorizon; + + inputs.predictedPosition = elevatorEncoder.getPosition(); + inputs.predictedVelocity = elevatorEncoder.getVelocity() / 60.0; + + inputs.velocityError = velocityError; + } + + @Override + public Setpoints getSetpoint() { + return set; + } + + @Override + public void goToSetpoint(Setpoints point) { + set = point; + + double targetPosition = point.elevatorSetpoint(); + double currentPosition = elevatorEncoder.getPosition(); + double velocity = elevatorEncoder.getVelocity() / 60.0; + + adjustPredictionHorizon(currentPosition, targetPosition); + generateReferenceTrajectory(targetPosition); + + computedVoltage = computeMPCControl(currentPosition, velocity); + + computedVoltage = + clampVoltageForDeadband( + computedVoltage, Math.abs(targetPosition - currentPosition), velocity); + + // computedVoltage = + // reduceVoltageNearSetpoint(computedVoltage, Math.abs(targetPosition - currentPosition)); + + if (Math.abs(targetPosition - currentPosition) < ElevatorConstants.positionTolerance) { + computedVoltage = 0; + } + + elevatorMotor.setVoltage(computedVoltage); + } + + // @Override + // public boolean isStable() { + // double positionError = + // Math.abs( + // elevatorEncoder.getPosition() + // - set.elevatorSetpoint()); + // double velocity = elevatorEncoder.getVelocity() / 60.0; // Convert RPM to RPS + + // return positionError < ElevatorConstants.positionTolerance + // && Math.abs(velocity) < ElevatorConstants.stableVelocityThreshold; + // } + + private void generateReferenceTrajectory(double targetPosition) { + double currentPosition = elevatorEncoder.getPosition(); + double distance = targetPosition - currentPosition; + + for (int i = 0; i < predictionHorizon; i++) { + double factor = (double) i / (predictionHorizon - 1); + referenceTrajectory[i] = currentPosition + distance * Math.pow(factor, 2); + } + } + + private double computeMPCControl(double currentPosition, double velocity) { + double kP = ElevatorConstants.kP; + double kD = ElevatorConstants.kD; + double kFF = 1 / ElevatorConstants.kV; + + double[] predictedPosition = new double[predictionHorizon]; + double[] predictedVelocity = new double[predictionHorizon]; + + predictedPosition[0] = currentPosition; + predictedVelocity[0] = velocity; + + double totalVoltage = 0.0; + + for (int i = 1; i < predictionHorizon; i++) { + double positionError = referenceTrajectory[i] - predictedPosition[i - 1]; + velocityError = + (referenceTrajectory[i] - referenceTrajectory[i - 1]) / controlLoopPeriod + - predictedVelocity[i - 1]; + velocityError = + Util.clamp(-ElevatorConstants.maxVel, ElevatorConstants.maxVel, velocityError); + + double stepVoltage = kP * positionError + kD * velocityError; + + if (Math.abs(positionError) < ElevatorConstants.positionTolerance) { + stepVoltage *= Math.abs(positionError) / ElevatorConstants.positionTolerance; + if (Math.abs(stepVoltage) < 0.1) { + stepVoltage = 0; + } + } + + predictedVelocity[i] = + predictedVelocity[i - 1] + + ((stepVoltage - ElevatorConstants.friction * predictedVelocity[i - 1]) + / ElevatorConstants.kV) + * ElevatorConstants.kA + * controlLoopPeriod; + + predictedPosition[i] = + predictedPosition[i - 1] + predictedVelocity[i - 1] * controlLoopPeriod; + + if (Math.abs(predictedPosition[i] - referenceTrajectory[i]) + < ElevatorConstants.positionTolerance) { + predictedVelocity[i] = 0; + } + + totalVoltage = stepVoltage; + totalVoltage += + kFF * velocity + + ElevatorConstants.kA + * (predictedVelocity[i] - predictedVelocity[i - 1]) + / controlLoopPeriod; + } + + totalVoltage += kFF * velocity; + return clampVoltage(totalVoltage); + } + + private void adjustPredictionHorizon(double currentPosition, double targetPosition) { + double distanceToTarget = Math.abs(targetPosition - currentPosition); + predictionHorizon = (distanceToTarget < 0.1) ? 5 : (distanceToTarget < 0.5) ? 10 : 20; + referenceTrajectory = new double[predictionHorizon]; + } + + private boolean isWithinDeadband(double positionError, double velocity) { + return Math.abs(positionError) < (0.5 * ElevatorConstants.positionTolerance) + && Math.abs(velocity) < (0.5 * ElevatorConstants.stableVelocityThreshold); + } + + private double clampVoltageForDeadband(double voltage, double positionError, double velocity) { + if (isWithinDeadband(positionError, velocity)) { + return 0.0; + } + return voltage; + } + + private double reduceVoltageNearSetpoint(double voltage, double positionError) { + if (positionError < ElevatorConstants.positionTolerance) { + return 0.0; + } + double scalingFactor = + Math.max(0.1, 1.0 - Math.abs(positionError) / (2 * ElevatorConstants.positionTolerance)); + return voltage * scalingFactor; + } + + private double clampVoltage(double voltage) { + return Math.max(ElevatorConstants.kMinOutput, Math.min(ElevatorConstants.kMaxOutput, voltage)); + } + + // @Override + // public void stop() { + // elevatorMotor.setVoltage(0); + // } +} diff --git a/src/main/java/org/curtinfrc/frc2025/util/Util.java b/src/main/java/org/curtinfrc/frc2025/util/Util.java new file mode 100644 index 00000000..f2e2a58d --- /dev/null +++ b/src/main/java/org/curtinfrc/frc2025/util/Util.java @@ -0,0 +1,20 @@ +package org.curtinfrc.frc2025.util; + +public final class Util { + /** + * Clamps a value to a specified inclusive range [min, max]. + * + * @param min The minimum allowable value (inclusive). + * @param max The maximum allowable value (inclusive). + * @param value The value to clamp. + * @return The clamped value. + * @throws IllegalArgumentException if min > max. + */ + public static double clamp(double min, double max, double value) { + if (min > max) { + throw new IllegalArgumentException( + String.format("Invalid range: min (%.2f) cannot be greater than max (%.2f)", min, max)); + } + return Math.min(max, Math.max(min, value)); + } +} From b4f8617c067ecc94acd8ac9c8c0b37bcc91ac74c Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Sun, 2 Feb 2025 12:15:31 +0800 Subject: [PATCH 07/19] did :skull: :pensive: elevator vision setpoints etc --- .../java/org/curtinfrc/frc2025/Constants.java | 92 +++++++++++++++---- .../java/org/curtinfrc/frc2025/Robot.java | 17 ++-- .../org/curtinfrc/frc2025/Superstructure.java | 5 +- .../frc2025/subsystems/drive/Drive.java | 46 +++++++--- .../frc2025/subsystems/vision/Vision.java | 3 + .../subsystems/vision/VisionConstants.java | 10 +- .../subsystems/vision/VisionIOLimelight.java | 5 + .../org/curtinfrc/frc2025/util/PoseUtil.java | 8 +- 8 files changed, 139 insertions(+), 47 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index 6519fc52..6a837a3e 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -16,11 +16,12 @@ import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.aprilTagLayout; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; -import java.util.ArrayList; import java.util.List; import org.curtinfrc.frc2025.util.PoseUtil; +import org.littletonrobotics.junction.Logger; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -29,8 +30,8 @@ */ public final class Constants { public static final RobotType robotType = RobotType.DEVBOT; - public static final double ROBOT_X = 550; // mm - public static final double ROBOT_Y = 570; + public static final double ROBOT_X = 660; // mm + public static final double ROBOT_Y = 680; public static final Mode getMode() { return switch (robotType) { @@ -71,20 +72,22 @@ public static void main(String... args) { // TODO: MAKE SETPOINTS public enum Setpoints { /* in mm */ - COLLECT(0, List.of(13, 12), List.of(1, 2)), + COLLECT(0, List.of(13, 12), List.of(1, 2), true), // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO // TODO actually subtract - L2(11, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), - L3(32.8, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)); + L2(11, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), false), + L3(32.8, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), false); private final double _elevatorSetpoint; private final List _tagIdsBlue; private final List _tagIdsRed; + private final boolean flip; - Setpoints(double elevator, List tagIdsBlue, List tagIdsRed) { + Setpoints(double elevator, List tagIdsBlue, List tagIdsRed, boolean flip) { this._elevatorSetpoint = elevator; this._tagIdsBlue = tagIdsBlue; this._tagIdsRed = tagIdsRed; + this.flip = flip; } public double elevatorSetpoint() { @@ -102,20 +105,75 @@ public Pose3d toPose(Pose3d currentPose) { private Pose3d resolvePose(List tagIds, Pose3d currentPose) { if (tagIds.isEmpty()) return new Pose3d(); - ArrayList poses = new ArrayList<>(); + double sideOffset = Math.max(ROBOT_X, ROBOT_Y) / 2000.0; + + class ClosestPose { + Pose3d pose = null; + double distance = Double.MAX_VALUE; + } + + ClosestPose closest = new ClosestPose(); + for (int tagId : tagIds) { aprilTagLayout .getTagPose(tagId) - .ifPresent(pose -> poses.add(PoseUtil.mapPose(pose.toPose2d()))); + .ifPresent( + tagPose -> { + Pose3d mappedPose = PoseUtil.mapPose(tagPose); + // mappedPose = mappedPose.rotateBy(new Rotation3d(0, 0, Math.PI)); + Rotation3d rotation = mappedPose.getRotation(); + double baseAngle = rotation.getAngle(), + cos = Math.cos(baseAngle), + sin = Math.sin(baseAngle); + double xOffset = sideOffset * sin, yOffset = sideOffset * cos; + + for (Pose3d pose : + new Pose3d[] { + new Pose3d( + mappedPose.getX() + xOffset, + mappedPose.getY() - yOffset, + mappedPose.getZ(), + rotation.rotateBy(new Rotation3d(0, 0, this.flip ? Math.PI : 0))), + new Pose3d( + mappedPose.getX() - xOffset, + mappedPose.getY() + yOffset, + mappedPose.getZ(), + rotation.rotateBy(new Rotation3d(0, 0, this.flip ? Math.PI : 0))) + }) { + double distance = distanceBetween(pose, currentPose); + if (distance < closest.distance) { + closest.pose = pose; + closest.distance = distance; + } + } + + String tagPrefix = String.format("resolvePose.tagId_%d", tagId); + Logger.recordOutput(tagPrefix + "/baseAngle", String.format("%.2f", baseAngle)); + Logger.recordOutput( + tagPrefix + "/leftPose", + formatPose( + new Pose3d( + mappedPose.getX() + xOffset, + mappedPose.getY() - yOffset, + mappedPose.getZ(), + rotation))); + Logger.recordOutput( + tagPrefix + "/rightPose", + formatPose( + new Pose3d( + mappedPose.getX() - xOffset, + mappedPose.getY() + yOffset, + mappedPose.getZ(), + rotation))); + Logger.recordOutput(tagPrefix + "/currentPose", formatPose(currentPose)); + Logger.recordOutput(tagPrefix + "/tagPose", formatPose(tagPose)); + }); } - if (poses.isEmpty()) return new Pose3d(); - - return poses.stream() - .min( - (pose1, pose2) -> - Double.compare( - distanceBetween(pose1, currentPose), distanceBetween(pose2, currentPose))) - .orElse(new Pose3d()); + return closest.pose != null ? closest.pose : new Pose3d(); + } + + private String formatPose(Pose3d pose) { + return String.format("(%.2f, %.2f, %.2f)", pose.getX(), pose.getY(), pose.getZ()); } private double distanceBetween(Pose3d pose1, Pose3d pose2) { diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 3178c7ee..cb61e72d 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -158,9 +158,10 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - new VisionIO() {}, - new VisionIOPhotonVision(camera1Name, robotToCamera1) {}, - new VisionIO() {}); + new VisionIOPhotonVision(camera0Name, robotToCamera0), + new VisionIOLimelight(camera1Name, drive::getRotation), + new VisionIOLimelight(camera2Name, drive::getRotation)); + elevator = new Elevator(new ElevatorIONEO()); intake = new Intake(new IntakeIONEO()); ejector = new Ejector(new EjectorIONEO()); @@ -319,7 +320,8 @@ public Robot() { .onTrue( Commands.runOnce( () -> - drive.setPose(new Pose2d(drive.getPose().getTranslation(), Rotation2d.kPi)), + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); @@ -329,8 +331,11 @@ public Robot() { controller .leftBumper() .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L2), Set.of(elevator))); - - controller.pov(0).whileTrue(drive.autoAlign(Setpoints.COLLECT)); + + // controller.pov(0).whileTrue(superstructure.align(Setpoints.L1)); + controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3)); + controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2)); + controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT)); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/org/curtinfrc/frc2025/Superstructure.java b/src/main/java/org/curtinfrc/frc2025/Superstructure.java index fa2f0c12..30ef73a1 100644 --- a/src/main/java/org/curtinfrc/frc2025/Superstructure.java +++ b/src/main/java/org/curtinfrc/frc2025/Superstructure.java @@ -1,6 +1,5 @@ package org.curtinfrc.frc2025; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import java.util.Set; @@ -21,8 +20,8 @@ public Command align(Setpoints setpoint) { return Commands.defer( () -> Commands.parallel( - drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))), + // drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))), elevator.goToSetpoint(setpoint)), - Set.of(drive, elevator)); + Set.of(elevator)); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index ad161b84..52031158 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -76,14 +76,20 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero); + private double p = 6; + private double d = 0.2; + private double i = 0.03; + private final PIDController xController = new PIDController(10.0, 0.0, 0.0); private final PIDController yController = new PIDController(10.0, 0.0, 0.0); - private final PIDController headingController = new PIDController(7.5, 0.0, 0.0); + private final PIDController headingController = new PIDController(p, i, d); private final SlewRateLimiter xLimiter = new SlewRateLimiter(7); // Limits acceleration to 3 mps private final SlewRateLimiter yLimiter = new SlewRateLimiter(7); // private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1); + private final SlewRateLimiter omegaAutoLimiter = new SlewRateLimiter(100); + RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner(); public Drive( @@ -127,6 +133,10 @@ public Drive( (voltage) -> runSteerCharacterization(voltage.in(Volts)), null, this)); headingController.enableContinuousInput(-Math.PI, Math.PI); + + // SmartDashboard.putNumber("P", 0.9); + // SmartDashboard.putNumber("D", 0.02); + // SmartDashboard.putNumber("I", 0.01); } @Override @@ -139,6 +149,10 @@ public void periodic() { } odometryLock.unlock(); + // headingController.setP(SmartDashboard.getNumber("P", 2)); + // headingController.setD(SmartDashboard.getNumber("D", 0.01)); + // headingController.setI(SmartDashboard.getNumber("I", 0)); + // Stop moving when disabled if (DriverStation.isDisabled()) { for (var module : modules) { @@ -194,6 +208,9 @@ public void periodic() { * @param speeds Speeds in meters/sec */ private void runVelocity(ChassisSpeeds speeds) { + speeds.vxMetersPerSecond = xLimiter.calculate(speeds.vxMetersPerSecond); + speeds.vyMetersPerSecond = yLimiter.calculate(speeds.vyMetersPerSecond); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(ChassisSpeeds.discretize(speeds, 0.02)); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); @@ -292,8 +309,8 @@ public Command joystickDrive( ChassisSpeeds speeds = new ChassisSpeeds( - xLimiter.calculate(linearVelocity.getX() * getMaxLinearSpeedMetersPerSec()), - yLimiter.calculate(linearVelocity.getY() * getMaxLinearSpeedMetersPerSec()), + linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(), omega * getMaxAngularSpeedRadPerSec()); // Logger.recordOutput("Drive/OmegaLimited", limited); @@ -358,14 +375,18 @@ public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot Pose2d pose = getPose(); Logger.recordOutput("Odometry/TrajectorySetpoint", pose); + Logger.recordOutput("Drive/PID/error", headingController.getError()); + var out = headingController.calculate(pose.getRotation().getRadians(), sample.heading); + Logger.recordOutput("Drive/PID/out", out); // Generate the next speeds for the robot ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - sample.vx + xController.calculate(pose.getX(), sample.x), - sample.vy + yController.calculate(pose.getY(), sample.y), - sample.omega - + headingController.calculate(pose.getRotation().getRadians(), sample.heading), + // 0, + // 0, + -(sample.vx + xController.calculate(pose.getX(), sample.x)), + -(sample.vy + yController.calculate(pose.getY(), sample.y)), + omegaAutoLimiter.calculate(out), getRotation()); // Apply the generated speeds @@ -624,10 +645,10 @@ public Command autoAlign(Pose3d _setpoint) { true); // Include rotational alignment using the existing heading controller - double adjustedOmega = - headingController.calculate( - robotPose.getRotation().getRadians(), - _setpoint.toPose2d().getRotation().getRadians()); + // double adjustedOmega = + // headingController.calculate( + // robotPose.getRotation().getRadians(), + // _setpoint.toPose2d().getRotation().getRadians()); // Apply the trajectory with rotation adjustment SwerveSample adjustedSample = @@ -638,7 +659,8 @@ public Command autoAlign(Pose3d _setpoint) { _setpoint.toPose2d().getRotation().getRadians(), cmd.vx, cmd.vy, - adjustedOmega, + // adjustedOmega, + 0, cmd.ax, cmd.ay, cmd.alpha, diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java index ba10c266..a1cb0b00 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java @@ -146,6 +146,9 @@ public void periodic() { } // Send vision observation + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/CurrentPose", + observation.pose().toPose2d()); consumer.accept( observation.pose().toPose2d(), observation.timestamp(), diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java index 075150b4..cfabb9ed 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java @@ -25,14 +25,16 @@ public class VisionConstants { AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); // Camera names, must match names configured on coprocessor - public static String camera0Name = "limelight-front"; - public static String camera1Name = "limelight-back"; + public static String camera0Name = "limelight-back"; + public static String camera1Name = "limelight-left"; + public static String camera2Name = "limelight-right"; // Robot to camera transforms // (Not used by Limelight, configure in web UI instead) public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = new Transform3d(0.25, 0.0, 0, Rotation3d.kZero); + new Transform3d(0.01, 0.06, 0.360, new Rotation3d(0, -Math.PI / 6, 0)); + public static Transform3d robotToCamera1 = new Transform3d(0.215, 0.16, 0.305, Rotation3d.kZero); + public static Transform3d robotToCamera2 = new Transform3d(-0.215, 0.16, 0.305, Rotation3d.kZero); // Basic filtering thresholds public static double maxAmbiguity = 0.3; diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java index 7d3aacb0..aba8dd99 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; @@ -32,6 +33,7 @@ public class VisionIOLimelight implements VisionIO { private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; + private final DoublePublisher imuModeSet; private final DoubleSubscriber latencySubscriber; private final DoubleSubscriber txSubscriber; @@ -55,6 +57,7 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + imuModeSet = table.getDoubleTopic("imumode_set").publish(); } @Override @@ -71,6 +74,8 @@ public void updateInputs(VisionIOInputs inputs) { // Update orientation for MegaTag 2 orientationPublisher.accept( new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + // Set IMU mode + imuModeSet.set(1); NetworkTableInstance.getDefault() .flush(); // Increases network traffic but recommended by Limelight diff --git a/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java b/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java index 04f32076..c07ac9c8 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java +++ b/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java @@ -1,17 +1,15 @@ package org.curtinfrc.frc2025.util; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import org.curtinfrc.frc2025.Constants; public final class PoseUtil { - public static Pose3d mapPose(Pose2d pose) { - double angle = pose.getRotation().getRadians(); + public static Pose3d mapPose(Pose3d pose) { + double angle = pose.getRotation().getAngle(); return new Pose3d( pose.getX() + Math.cos(angle) * Constants.ROBOT_X / 2000.0, pose.getY() + Math.sin(angle) * Constants.ROBOT_Y / 2000.0, 0.0, - new Rotation3d(0.0, 0.0, angle)); + pose.getRotation()); } } From 228031466f60c9390fd45ff39ed83adebfe4b0ff Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Sun, 2 Feb 2025 15:07:31 +0800 Subject: [PATCH 08/19] auto align --- src/main/java/org/curtinfrc/frc2025/Constants.java | 6 +++--- src/main/java/org/curtinfrc/frc2025/Superstructure.java | 5 +++-- .../java/org/curtinfrc/frc2025/subsystems/drive/Drive.java | 6 +++--- .../org/curtinfrc/frc2025/subsystems/vision/Vision.java | 5 +++-- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index 6a837a3e..2376eb13 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -72,11 +72,11 @@ public static void main(String... args) { // TODO: MAKE SETPOINTS public enum Setpoints { /* in mm */ - COLLECT(0, List.of(13, 12), List.of(1, 2), true), + COLLECT(0, List.of(13, 12), List.of(1, 2), false), // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO // TODO actually subtract - L2(11, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), false), - L3(32.8, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), false); + L2(11, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), true), + L3(32.8, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), true); private final double _elevatorSetpoint; private final List _tagIdsBlue; diff --git a/src/main/java/org/curtinfrc/frc2025/Superstructure.java b/src/main/java/org/curtinfrc/frc2025/Superstructure.java index 30ef73a1..b5cd9648 100644 --- a/src/main/java/org/curtinfrc/frc2025/Superstructure.java +++ b/src/main/java/org/curtinfrc/frc2025/Superstructure.java @@ -1,5 +1,6 @@ package org.curtinfrc.frc2025; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import java.util.Set; @@ -20,8 +21,8 @@ public Command align(Setpoints setpoint) { return Commands.defer( () -> Commands.parallel( - // drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))), + drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))), elevator.goToSetpoint(setpoint)), - Set.of(elevator)); + Set.of(elevator, drive)); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index f68a3021..5b371f08 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -89,7 +89,7 @@ public class Drive extends SubsystemBase { // private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1); private final SlewRateLimiter omegaAutoLimiter = new SlewRateLimiter(100); - // private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1); + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1); RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner(); @@ -385,8 +385,8 @@ public void followTrajectory(SwerveSample sample) { ChassisSpeeds.fromFieldRelativeSpeeds( // 0, // 0, - -(sample.vx + xController.calculate(pose.getX(), sample.x)), - -(sample.vy + yController.calculate(pose.getY(), sample.y)), + (sample.vx + xController.calculate(pose.getX(), sample.x)), + (sample.vy + yController.calculate(pose.getY(), sample.y)), omegaAutoLimiter.calculate(out), getRotation()); diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java index a1cb0b00..4695ad2f 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java @@ -138,12 +138,13 @@ public void periodic() { double angularStdDev = angularStdDevBaseline * stdDevFactor; if (observation.type() == PoseObservationType.MEGATAG_2) { linearStdDev *= linearStdDevMegatag2Factor; - angularStdDev *= angularStdDevMegatag2Factor; + // angularStdDev *= angularStdDevMegatag2Factor; } if (cameraIndex < cameraStdDevFactors.length) { linearStdDev *= cameraStdDevFactors[cameraIndex]; - angularStdDev *= cameraStdDevFactors[cameraIndex]; + // angularStdDev *= cameraStdDevFactors[cameraIndex]; } + angularStdDev = 99999; // Send vision observation Logger.recordOutput( From 30497c3c0277e295a69a97f7a0d2450635a5b143 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Sun, 2 Feb 2025 16:03:44 +0800 Subject: [PATCH 09/19] did some stuff --- src/main/java/org/curtinfrc/frc2025/Constants.java | 6 +++--- src/main/java/org/curtinfrc/frc2025/Robot.java | 3 +++ .../org/curtinfrc/frc2025/subsystems/intake/Intake.java | 2 +- .../java/org/curtinfrc/frc2025/util/RepulsorConstants.java | 2 +- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index 2376eb13..d26fa7ca 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -75,8 +75,8 @@ public enum Setpoints { COLLECT(0, List.of(13, 12), List.of(1, 2), false), // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO // TODO actually subtract - L2(11, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), true), - L3(32.8, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6), true); + L2(11, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true), + L3(32.8, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true); private final double _elevatorSetpoint; private final List _tagIdsBlue; @@ -105,7 +105,7 @@ public Pose3d toPose(Pose3d currentPose) { private Pose3d resolvePose(List tagIds, Pose3d currentPose) { if (tagIds.isEmpty()) return new Pose3d(); - double sideOffset = Math.max(ROBOT_X, ROBOT_Y) / 2000.0; + double sideOffset = 0.32 / 2; class ClosestPose { Pose3d pose = null; diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 223e128e..8f7d4b7c 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -279,6 +279,9 @@ public Robot() { intake.setDefaultCommand(intake.intake(intakeVolts)); ejector.setDefaultCommand(ejector.stop()); + // elevator.toZero.whileTrue(intake.intake(intakeVolts)); + elevator.toZero.negate().whileTrue(intake.stop()); + intake .backSensor .and(intake.frontSensor.negate()) diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/Intake.java b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/Intake.java index 2dd83dad..147f87e1 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/intake/Intake.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/intake/Intake.java @@ -23,7 +23,7 @@ public void periodic() { } public Command stop() { - return runOnce(() -> io.setVoltage(0)); + return run(() -> io.setVoltage(0)); } public Command intake(double volts) { diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java index 33a364cd..4227fcd0 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java @@ -8,7 +8,7 @@ import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.VerticalObstacle; public class RepulsorConstants { - public static final double GOAL_STRENGTH = 0.65; + public static final double GOAL_STRENGTH = 0.1; public static final List FIELD_OBSTACLES = List.of( From af653f6a076346f8a6433248c77e0081a9d2c79c Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 3 Feb 2025 15:01:58 +0800 Subject: [PATCH 10/19] :skull: --- .../curtinfrc/frc2025/subsystems/vision/VisionConstants.java | 2 +- src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java | 2 +- .../java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java index 502d2fa1..8bb1a50f 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionConstants.java @@ -32,7 +32,7 @@ public class VisionConstants { // Robot to camera transforms // (Not used by Limelight, configure in web UI instead) public static Transform3d robotToCamera0 = - new Transform3d(0.01, 0.06, 0.360, new Rotation3d(0, -Math.PI / 6, 0)); + new Transform3d(0.01, 0.06, 0.360, new Rotation3d(0, -Math.PI / 6, Math.PI)); public static Transform3d robotToCamera1 = new Transform3d(0.16, 0.215, 0.305, Rotation3d.kZero); public static Transform3d robotToCamera2 = new Transform3d(0.16, -0.215, 0.305, Rotation3d.kZero); diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java index 4227fcd0..33a364cd 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java @@ -8,7 +8,7 @@ import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.VerticalObstacle; public class RepulsorConstants { - public static final double GOAL_STRENGTH = 0.1; + public static final double GOAL_STRENGTH = 0.65; public static final List FIELD_OBSTACLES = List.of( diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index c74f411d..b1f40286 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -231,8 +231,10 @@ Force getGoalForce(Translation2d curLocation, Translation2d goal) { return new Force(); } var direction = displacement.getAngle(); + var distanceFactor = Math.min(1.0, displacement.getNorm() / 8.0); var mag = RepulsorConstants.GOAL_STRENGTH + * distanceFactor * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); return new Force(mag, direction); } From 37e07ff2836faf2809684d140d975032451d6e21 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 3 Feb 2025 15:37:27 +0800 Subject: [PATCH 11/19] skibidi --- .../frc2025/subsystems/drive/Drive.java | 14 +++-- .../frc2025/util/RepulsorConstants.java | 44 +++++++-------- .../frc2025/util/RepulsorFieldPlanner.java | 54 +++++++++++-------- 3 files changed, 59 insertions(+), 53 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 5b371f08..55d8ae4b 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -383,14 +383,12 @@ public void followTrajectory(SwerveSample sample) { // Generate the next speeds for the robot ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - // 0, - // 0, - (sample.vx + xController.calculate(pose.getX(), sample.x)), - (sample.vy + yController.calculate(pose.getY(), sample.y)), - omegaAutoLimiter.calculate(out), - getRotation()); - - // Apply the generated speeds + sample.vx + xController.calculate(pose.getX(), sample.x), + sample.vy + yController.calculate(pose.getY(), sample.y), + sample.omega + + headingController.calculate(pose.getRotation().getRadians(), sample.heading), + getRotation()); // Apply the generated speeds + runVelocity(speeds); } diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java index 33a364cd..3f1ef1b3 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorConstants.java @@ -1,25 +1,25 @@ -package org.curtinfrc.frc2025.util; +// package org.curtinfrc.frc2025.util; -import edu.wpi.first.math.geometry.Translation2d; -import java.util.List; -import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.HorizontalObstacle; -import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.Obstacle; -import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.SnowmanObstacle; -import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.VerticalObstacle; +// import edu.wpi.first.math.geometry.Translation2d; +// import java.util.List; +// import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.HorizontalObstacle; +// import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.Obstacle; +// import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.SnowmanObstacle; +// import org.curtinfrc.frc2025.util.RepulsorFieldPlanner.VerticalObstacle; -public class RepulsorConstants { - public static final double GOAL_STRENGTH = 0.65; +// public class RepulsorConstants { +// public static final double GOAL_STRENGTH = 0.65; - public static final List FIELD_OBSTACLES = - List.of( - new SnowmanObstacle(new Translation2d(4.49, 4), 1, true), - new SnowmanObstacle(new Translation2d(13.08, 4), 1, true)); - public static final double FIELD_LENGTH = 16.42; - public static final double FIELD_WIDTH = 8.16; - public static final List WALLS = - List.of( - new HorizontalObstacle(0.0, 0.5, true), - new HorizontalObstacle(FIELD_WIDTH, 0.5, false), - new VerticalObstacle(0.0, 0.5, true), - new VerticalObstacle(FIELD_LENGTH, 0.5, false)); -} +// public static final List FIELD_OBSTACLES = +// List.of( +// new SnowmanObstacle(new Translation2d(4.49, 4), 1, true), +// new SnowmanObstacle(new Translation2d(13.08, 4), 1, true)); +// public static final double FIELD_LENGTH = 16.42; +// public static final double FIELD_WIDTH = 8.16; +// public static final List WALLS = +// List.of( +// new HorizontalObstacle(0.0, 0.5, true), +// new HorizontalObstacle(FIELD_WIDTH, 0.5, false), +// new VerticalObstacle(0.0, 0.5, true), +// new VerticalObstacle(FIELD_LENGTH, 0.5, false)); +// } diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index b1f40286..3eb77d1a 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -5,16 +5,17 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableEvent.Kind; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableListener; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.EnumSet; import java.util.List; import java.util.Optional; -import org.littletonrobotics.junction.AutoLogOutput; public class RepulsorFieldPlanner { @@ -79,9 +80,10 @@ static class SnowmanObstacle extends Obstacle { Translation2d loc; double radius = 0.5; - public SnowmanObstacle(Translation2d loc, double strength, boolean positive) { + public SnowmanObstacle(Translation2d loc, double strength, double radius, boolean positive) { super(strength, positive); this.loc = loc; + this.radius = radius; } public Force getForceAtPosition(Translation2d position, Translation2d target) { @@ -92,7 +94,7 @@ public Force getForceAtPosition(Translation2d position, Translation2d target) { var dist = loc.getDistance(position); var sidewaysDist = sidewaysCircle.getDistance(position); var sidewaysMag = distToForceMag(sidewaysCircle.getDistance(position)); - var outwardsMag = distToForceMag(loc.getDistance(position)); + var outwardsMag = distToForceMag(Math.max(0.01, loc.getDistance(position) - radius)); var initial = new Force(outwardsMag, position.minus(loc).getAngle()); // flip the sidewaysMag based on which side of the goal-sideways circle the robot is on @@ -131,23 +133,38 @@ public Force getForceAtPosition(Translation2d position, Translation2d target) { } } - private List fixedObstacles = new ArrayList<>(); + public static final double GOAL_STRENGTH = 0.65; + + public static final List FIELD_OBSTACLES = + List.of( + new SnowmanObstacle( + new Translation2d(4.49, 4), 0.6, Units.inchesToMeters(65.5 / 2.0), true), + new SnowmanObstacle( + new Translation2d(13.08, 4), 0.6, Units.inchesToMeters(65.5 / 2.0), true)); + static final double FIELD_LENGTH = 16.42; + static final double FIELD_WIDTH = 8.16; + public static final List WALLS = + List.of( + new HorizontalObstacle(0.0, 0.5, true), + new HorizontalObstacle(FIELD_WIDTH, 0.5, false), + new VerticalObstacle(0.0, 0.5, true), + new VerticalObstacle(FIELD_LENGTH, 0.5, false)); + private List fixedObstacles = new ArrayList<>(); private Optional goalOpt = Optional.empty(); - @AutoLogOutput(key = "RepulsorFieldPlanner/GoalPose") public Pose2d goal() { return new Pose2d(goalOpt.orElse(Translation2d.kZero), Rotation2d.kZero); } - private static final int ARROWS_X = 40; - private static final int ARROWS_Y = 20; + private static final int ARROWS_X = RobotBase.isSimulation() ? 40 : 0; + private static final int ARROWS_Y = RobotBase.isSimulation() ? 20 : 0; private static final int ARROWS_SIZE = (ARROWS_X + 1) * (ARROWS_Y + 1); private ArrayList arrows = new ArrayList<>(ARROWS_SIZE); public RepulsorFieldPlanner() { - fixedObstacles.addAll(RepulsorConstants.FIELD_OBSTACLES); - fixedObstacles.addAll(RepulsorConstants.WALLS); + fixedObstacles.addAll(FIELD_OBSTACLES); + fixedObstacles.addAll(WALLS); for (int i = 0; i < ARROWS_SIZE; i++) { arrows.add(new Pose2d()); } @@ -202,9 +219,7 @@ void updateArrows() { for (int x = 0; x <= ARROWS_X; x++) { for (int y = 0; y <= ARROWS_Y; y++) { var translation = - new Translation2d( - x * RepulsorConstants.FIELD_LENGTH / ARROWS_X, - y * RepulsorConstants.FIELD_WIDTH / ARROWS_Y); + new Translation2d(x * FIELD_LENGTH / ARROWS_X, y * FIELD_WIDTH / ARROWS_Y); var force = Force.kZero; if (useObstaclesInArrows) force = force.plus(getObstacleForce(translation, goal().getTranslation())); @@ -224,40 +239,33 @@ void updateArrows() { } } - @AutoLogOutput(key = "RepulsorFieldPlanner/GoalForce") Force getGoalForce(Translation2d curLocation, Translation2d goal) { var displacement = goal.minus(curLocation); if (displacement.getNorm() == 0) { return new Force(); } var direction = displacement.getAngle(); - var distanceFactor = Math.min(1.0, displacement.getNorm() / 8.0); var mag = - RepulsorConstants.GOAL_STRENGTH - * distanceFactor - * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); + GOAL_STRENGTH * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); return new Force(mag, direction); } - @AutoLogOutput(key = "RepulsorFieldPlanner/WallForce") Force getWallForce(Translation2d curLocation, Translation2d target) { var force = Force.kZero; - for (Obstacle obs : RepulsorConstants.WALLS) { + for (Obstacle obs : WALLS) { force = force.plus(obs.getForceAtPosition(curLocation, target)); } return force; } - @AutoLogOutput(key = "RepulsorFieldPlanner/ObstacleForce") Force getObstacleForce(Translation2d curLocation, Translation2d target) { var force = Force.kZero; - for (Obstacle obs : RepulsorConstants.FIELD_OBSTACLES) { + for (Obstacle obs : FIELD_OBSTACLES) { force = force.plus(obs.getForceAtPosition(curLocation, target)); } return force; } - @AutoLogOutput(key = "RepulsorFieldPlanner/Force") Force getForce(Translation2d curLocation, Translation2d target) { var goalForce = getGoalForce(curLocation, target) @@ -266,7 +274,7 @@ Force getForce(Translation2d curLocation, Translation2d target) { return goalForce; } - private SwerveSample sample( + public static SwerveSample sample( Translation2d trans, Rotation2d rot, double vx, double vy, double omega) { return new SwerveSample( 0, From f5a7e9a9b2e8d05a4dcf993d3ffcd4c04b43f35c Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 3 Feb 2025 16:54:05 +0800 Subject: [PATCH 12/19] ohio --- .../java/org/curtinfrc/frc2025/subsystems/drive/Drive.java | 5 ++--- .../curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java | 2 +- .../java/org/curtinfrc/frc2025/subsystems/vision/Vision.java | 3 --- .../org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java | 5 ++++- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 55d8ae4b..b45a84fa 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -310,8 +310,8 @@ public Command joystickDrive( ChassisSpeeds speeds = new ChassisSpeeds( - xLimiter.calculate(linearVelocity.getX() * getMaxLinearSpeedMetersPerSec()), - yLimiter.calculate(linearVelocity.getY() * getMaxLinearSpeedMetersPerSec()), + linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(), omega * getMaxAngularSpeedRadPerSec()); // Logger.recordOutput("Drive/OmegaLimited", limited); @@ -325,7 +325,6 @@ public Command joystickDrive( speeds, isFlipped ? getRotation().plus(Rotation2d.kPi) : getRotation())); }); } - /** * Field relative drive command using joystick for linear control and PID for angular control. * Possible use cases include snapping to an angle, aiming at a vision target, or controlling diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java index c86bdb6e..d2875e1d 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java @@ -46,7 +46,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.point = this.setpoint; - inputs.touchSensor = !touch.get(); + inputs.touchSensor = touch.get(); } @Override diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java index 2921be54..a9f939a3 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java @@ -147,9 +147,6 @@ public void periodic() { angularStdDev = 999999; // Send vision observation - Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/CurrentPose", - observation.pose().toPose2d()); consumer.accept( observation.pose().toPose2d(), observation.timestamp(), diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index 3eb77d1a..6bb60449 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -245,8 +245,11 @@ Force getGoalForce(Translation2d curLocation, Translation2d goal) { return new Force(); } var direction = displacement.getAngle(); + var distanceFactor = Math.min(1.0, displacement.getNorm() / 4.0); // Adjust scaling var mag = - GOAL_STRENGTH * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); + GOAL_STRENGTH + * distanceFactor + * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); return new Force(mag, direction); } From ef39e7f4ae47cbe3c9a6f7cbb39c022b74bb9f4d Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Thu, 6 Feb 2025 17:45:38 +0800 Subject: [PATCH 13/19] did --- src/main/java/org/curtinfrc/frc2025/Robot.java | 14 ++++++-------- .../frc2025/subsystems/drive/Drive.java | 11 +++++++---- .../frc2025/util/RepulsorFieldPlanner.java | 17 ++++++++++++----- 3 files changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 8f7d4b7c..72f82b70 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Set; import org.curtinfrc.frc2025.Constants.Mode; import org.curtinfrc.frc2025.Constants.Setpoints; import org.curtinfrc.frc2025.generated.TunerConstants; @@ -327,13 +326,6 @@ public Robot() { drive) .ignoringDisable(true)); - controller - .rightBumper() - .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L3), Set.of(elevator))); - controller - .leftBumper() - .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L2), Set.of(elevator))); - // controller.pov(0).whileTrue(superstructure.align(Setpoints.L1)); controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3)); controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2)); @@ -408,6 +400,12 @@ public void teleopPeriodic() { } else { Logger.recordOutput("IntakeCommand", "null"); } + + if (intake.getCurrentCommand() != null) { + Logger.recordOutput("DriveCommand", drive.getCurrentCommand().getName()); + } else { + Logger.recordOutput("DriveCommand", "null"); + } } /** This function is called once when test mode is enabled. */ diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index b45a84fa..4448de47 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -80,10 +80,13 @@ public class Drive extends SubsystemBase { private double d = 0.2; private double i = 0.03; - private final PIDController xController = new PIDController(10.0, 0.0, 0.0); - private final PIDController yController = new PIDController(10.0, 0.0, 0.0); + private final PIDController xController = new PIDController(5.0, 0.0, 0.0); + private final PIDController yController = new PIDController(5.0, 0.0, 0.0); private final PIDController headingController = new PIDController(p, i, d); + private final PIDController xSetpointController = new PIDController(6.0, 0.0, 0.1); + private final PIDController ySetpointController = new PIDController(6.0, 0.0, 0.1); + private final SlewRateLimiter xLimiter = new SlewRateLimiter(7); // Limits acceleration to 3 mps private final SlewRateLimiter yLimiter = new SlewRateLimiter(7); // private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1); @@ -382,8 +385,8 @@ public void followTrajectory(SwerveSample sample) { // Generate the next speeds for the robot ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - sample.vx + xController.calculate(pose.getX(), sample.x), - sample.vy + yController.calculate(pose.getY(), sample.y), + sample.vx + xSetpointController.calculate(pose.getX(), sample.x), + sample.vy + ySetpointController.calculate(pose.getY(), sample.y), sample.omega + headingController.calculate(pose.getRotation().getRadians(), sample.heading), getRotation()); // Apply the generated speeds diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index 6bb60449..b35ebade 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -16,6 +16,7 @@ import java.util.EnumSet; import java.util.List; import java.util.Optional; +import org.littletonrobotics.junction.Logger; public class RepulsorFieldPlanner { @@ -245,11 +246,8 @@ Force getGoalForce(Translation2d curLocation, Translation2d goal) { return new Force(); } var direction = displacement.getAngle(); - var distanceFactor = Math.min(1.0, displacement.getNorm() / 4.0); // Adjust scaling var mag = - GOAL_STRENGTH - * distanceFactor - * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); + GOAL_STRENGTH * (1 + 1.0 / (0.0001 + displacement.getNorm() * displacement.getNorm())); return new Force(mag, direction); } @@ -273,7 +271,8 @@ Force getForce(Translation2d curLocation, Translation2d target) { var goalForce = getGoalForce(curLocation, target) .plus(getObstacleForce(curLocation, target)) - .plus(getWallForce(curLocation, target)); + .plus(getWallForce(curLocation, target)) + .times(Math.min(1.0, curLocation.getDistance(target))); // Adjust scaling return goalForce; } @@ -322,6 +321,9 @@ public SwerveSample getCmd( var goal = goalOpt.get(); var curTrans = pose.getTranslation(); var err = curTrans.minus(goal); + + Logger.recordOutput("Repulsor/err", err.getNorm()); + Logger.recordOutput("Repulsor/step", stepSize_m * 1.5); if (useGoal && err.getNorm() < stepSize_m * 1.5) { return sample(goal, goalRotation, 0, 0, 0); } else { @@ -335,6 +337,11 @@ public SwerveSample getCmd( stepSize_m = Math.min(maxSpeed, closeToGoalMax) * 0.02; } + + netForce = netForce.times(Math.min(1.0, curTrans.getDistance(goal))); // Adjust scaling + + Logger.recordOutput("Repulsor/net", netForce); + var step = new Translation2d(stepSize_m, netForce.getAngle()); var intermediateGoal = curTrans.plus(step); var endTime = System.nanoTime(); From c10c8c5d6e941254d79ccaa262d97a202d617729 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Thu, 6 Feb 2025 19:45:35 +0800 Subject: [PATCH 14/19] auto pathing --- .../java/org/curtinfrc/frc2025/Robot.java | 26 +++++++++++------ .../frc2025/subsystems/drive/Drive.java | 28 +++++++++++++++---- .../frc2025/subsystems/elevator/Elevator.java | 6 ++-- .../frc2025/util/RepulsorFieldPlanner.java | 11 +++++--- 4 files changed, 49 insertions(+), 22 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 72f82b70..f407b317 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.Set; import org.curtinfrc.frc2025.Constants.Mode; import org.curtinfrc.frc2025.Constants.Setpoints; import org.curtinfrc.frc2025.generated.TunerConstants; @@ -268,41 +269,48 @@ public Robot() { () -> -controller.getLeftX(), () -> -controller.getRightX())); - elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT)); + // elevator.setDefaultCommand( + // Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); + controller .rightBumper() .negate() .and(controller.leftBumper().negate()) - .onTrue(elevator.goToSetpoint(Setpoints.COLLECT)); + .onTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); intake.setDefaultCommand(intake.intake(intakeVolts)); ejector.setDefaultCommand(ejector.stop()); + drive.atSetpointPose.and(elevator.isNotAtCollect).whileTrue(ejector.eject(1000)); + // elevator.toZero.whileTrue(intake.intake(intakeVolts)); - elevator.toZero.negate().whileTrue(intake.stop()); + // elevator.toZero.().whileTrue(intake.stop()); intake .backSensor .and(intake.frontSensor.negate()) + .and(elevator.isNotAtCollect.negate()) .whileTrue( Commands.parallel(intake.intake(intakeVolts), ejector.eject(5)).withName("front")); intake .backSensor .and(intake.frontSensor) + .and(elevator.isNotAtCollect.negate()) .whileTrue( Commands.parallel(intake.intake(intakeVolts), ejector.eject(5)) .withName("front and back")); intake .backSensor .and(intake.frontSensor.negate()) + .and(elevator.isNotAtCollect.negate()) .whileTrue(Commands.parallel(intake.stop(), ejector.stop()).withName("not front and back")); - elevator.toZero.whileTrue( - elevator - .zero() - .until(elevator.toZero.negate()) - .ignoringDisable(true) - .withName("ElevatorZero")); + // elevator.toZero.whileTrue( + // elevator + // .zero() + // .until(elevator.toZero.negate()) + // .ignoringDisable(true) + // .withName("ElevatorZero")); // elevator.isNotAtCollect.negate().whileTrue(new PrintCommand("pls pls work")); controller.b().onTrue(elevator.zero()); diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 4448de47..fa765d80 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.text.DecimalFormat; import java.text.NumberFormat; @@ -80,12 +81,15 @@ public class Drive extends SubsystemBase { private double d = 0.2; private double i = 0.03; - private final PIDController xController = new PIDController(5.0, 0.0, 0.0); - private final PIDController yController = new PIDController(5.0, 0.0, 0.0); + private final PIDController xController = new PIDController(5.0, 0.0, 0.1); + private final PIDController yController = new PIDController(5.0, 0.0, 0.1); private final PIDController headingController = new PIDController(p, i, d); - private final PIDController xSetpointController = new PIDController(6.0, 0.0, 0.1); - private final PIDController ySetpointController = new PIDController(6.0, 0.0, 0.1); + private final PIDController xSetpointController = new PIDController(25.0, 0.0, 0.1); + private final PIDController ySetpointController = new PIDController(25.0, 0.0, 0.1); + + public Trigger atSetpointPose = + new Trigger(() -> xController.atSetpoint() && yController.atSetpoint()); private final SlewRateLimiter xLimiter = new SlewRateLimiter(7); // Limits acceleration to 3 mps private final SlewRateLimiter yLimiter = new SlewRateLimiter(7); @@ -202,6 +206,14 @@ public void periodic() { poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } + Logger.recordOutput("Drive/xPID/setpoint", xController.getSetpoint()); + Logger.recordOutput("Drive/xPID/error", xController.getError()); + Logger.recordOutput("Drive/xPID/atSetpoint", xController.atSetpoint()); + + Logger.recordOutput("Drive/yPID/setpoint", yController.getSetpoint()); + Logger.recordOutput("Drive/yPID/error", yController.getError()); + Logger.recordOutput("Drive/yPID/atSetpoint", yController.atSetpoint()); + // Update gyro alert gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); } @@ -385,8 +397,12 @@ public void followTrajectory(SwerveSample sample) { // Generate the next speeds for the robot ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - sample.vx + xSetpointController.calculate(pose.getX(), sample.x), - sample.vy + ySetpointController.calculate(pose.getY(), sample.y), + sample.vx + sample.vx != 0 + ? xSetpointController.calculate(pose.getX(), sample.x) + : xController.calculate(pose.getX(), sample.x), + sample.vy + sample.vy != 0 + ? ySetpointController.calculate(pose.getY(), sample.y) + : yController.calculate(pose.getY(), sample.y), sample.omega + headingController.calculate(pose.getRotation().getRadians(), sample.heading), getRotation()); // Apply the generated speeds diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java index d25eed0c..597cadaa 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java @@ -15,7 +15,7 @@ public class Elevator extends SubsystemBase { private final PIDController pid = new PIDController(kP, 0, kD); private Setpoints setpoint = Setpoints.COLLECT; - public final Trigger isNotAtCollect = new Trigger(() -> inputs.point != Setpoints.COLLECT); + public final Trigger isNotAtCollect = new Trigger(() -> setpoint != Setpoints.COLLECT); public final Trigger toZero = new Trigger(() -> inputs.touchSensor); public Elevator(ElevatorIO io) { @@ -26,7 +26,7 @@ public Elevator(ElevatorIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); - Logger.recordOutput("Elevator/toZero", toZero.getAsBoolean()); + Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect.getAsBoolean()); Logger.recordOutput("Elevator/setpoint", setpoint); // Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect); } @@ -47,7 +47,7 @@ public Command goToSetpoint(Setpoints point) { } public Command zero() { - return run(() -> io.zero()); + return runOnce(() -> io.zero()); } // public Command goToSetpoint(Setpoints point) { diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index b35ebade..30c3861a 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -322,7 +322,7 @@ public SwerveSample getCmd( var curTrans = pose.getTranslation(); var err = curTrans.minus(goal); - Logger.recordOutput("Repulsor/err", err.getNorm()); + Logger.recordOutput("Repulsor/err", curTrans.getDistance(goal)); Logger.recordOutput("Repulsor/step", stepSize_m * 1.5); if (useGoal && err.getNorm() < stepSize_m * 1.5) { return sample(goal, goalRotation, 0, 0, 0); @@ -338,15 +338,18 @@ public SwerveSample getCmd( stepSize_m = Math.min(maxSpeed, closeToGoalMax) * 0.02; } - netForce = netForce.times(Math.min(1.0, curTrans.getDistance(goal))); // Adjust scaling - Logger.recordOutput("Repulsor/net", netForce); var step = new Translation2d(stepSize_m, netForce.getAngle()); var intermediateGoal = curTrans.plus(step); var endTime = System.nanoTime(); SmartDashboard.putNumber("repulsorTimeS", (endTime - startTime) / 1e9); - return sample(intermediateGoal, goalRotation, step.getX() / 0.02, step.getY() / 0.02, 0); + return sample( + intermediateGoal, + goalRotation, + (step.getX() / 0.02) * Math.min(1.0, curTrans.getDistance(goal) / 3), + (step.getY() / 0.02) * Math.min(1.0, curTrans.getDistance(goal) / 3), + 0); } } } From 6f466dac4fa8720cb9014c71fc693012435939d2 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Sun, 9 Feb 2025 16:37:40 +0800 Subject: [PATCH 15/19] full auto cycles? --- .../java/org/curtinfrc/frc2025/Constants.java | 4 +- .../java/org/curtinfrc/frc2025/Robot.java | 37 ++++++---- .../org/curtinfrc/frc2025/Superstructure.java | 13 +++- .../frc2025/subsystems/drive/Drive.java | 67 ++++++++++++++----- .../subsystems/ejector/EjectorConstants.java | 2 +- .../frc2025/subsystems/elevator/Elevator.java | 12 +++- .../elevator/ElevatorConstants.java | 4 +- .../subsystems/elevator/ElevatorIONEO.java | 6 +- .../frc2025/util/RepulsorFieldPlanner.java | 33 ++++++--- 9 files changed, 126 insertions(+), 52 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index d26fa7ca..d00ccc97 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -75,8 +75,8 @@ public enum Setpoints { COLLECT(0, List.of(13, 12), List.of(1, 2), false), // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO // TODO actually subtract - L2(11, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true), - L3(32.8, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true); + L2(11.5, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true), + L3(30 /* 33.5 */, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true); private final double _elevatorSetpoint; private final List _tagIdsBlue; diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index f407b317..09e31ae2 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -269,19 +269,26 @@ public Robot() { () -> -controller.getLeftX(), () -> -controller.getRightX())); - // elevator.setDefaultCommand( - // Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); + elevator.setDefaultCommand( + Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); - controller - .rightBumper() - .negate() - .and(controller.leftBumper().negate()) - .onTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); + // controller + // .rightBumper() + // .negate() + // .and(controller.leftBumper().negate()) + // .onTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); + + // controller.a().whileTrue(elevator.zero()); intake.setDefaultCommand(intake.intake(intakeVolts)); ejector.setDefaultCommand(ejector.stop()); - drive.atSetpointPose.and(elevator.isNotAtCollect).whileTrue(ejector.eject(1000)); + drive + .atSetpointPose + .and(elevator.isNotAtCollect) + .and(elevator.atSetpoint) + .onTrue( + ejector.eject(1000).until(elevator.isNotAtCollect.negate()).andThen(ejector.stop())); // elevator.toZero.whileTrue(intake.intake(intakeVolts)); // elevator.toZero.().whileTrue(intake.stop()); @@ -306,14 +313,14 @@ public Robot() { .whileTrue(Commands.parallel(intake.stop(), ejector.stop()).withName("not front and back")); // elevator.toZero.whileTrue( - // elevator - // .zero() - // .until(elevator.toZero.negate()) - // .ignoringDisable(true) - // .withName("ElevatorZero")); + // elevator + // .zero() + // .until(elevator.toZero.negate()) + // .ignoringDisable(true) + // .withName("ElevatorZero")); // elevator.isNotAtCollect.negate().whileTrue(new PrintCommand("pls pls work")); - controller.b().onTrue(elevator.zero()); + controller.b().whileTrue(elevator.zero()); controller.rightTrigger().whileTrue(ejector.eject(1000)); // Lock to 0° when A button is held @@ -338,6 +345,8 @@ public Robot() { controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3)); controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2)); controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT)); + + // controller.rightBumper().whileTrue(elevator.setVoltage(2)); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/org/curtinfrc/frc2025/Superstructure.java b/src/main/java/org/curtinfrc/frc2025/Superstructure.java index b5cd9648..018e07bb 100644 --- a/src/main/java/org/curtinfrc/frc2025/Superstructure.java +++ b/src/main/java/org/curtinfrc/frc2025/Superstructure.java @@ -20,9 +20,16 @@ public Superstructure(Drive drive, Elevator elevator) { public Command align(Setpoints setpoint) { return Commands.defer( () -> - Commands.parallel( - drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))), - elevator.goToSetpoint(setpoint)), + drive + .autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))) + .until( + () -> + Math.abs(drive.xController.getError()) < 0.2 + && Math.abs(drive.yController.getError()) < 0.2) + .andThen( + elevator + .goToSetpoint(setpoint) + .alongWith(drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))))), Set.of(elevator, drive)); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index fa765d80..718bf1f9 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -81,15 +81,19 @@ public class Drive extends SubsystemBase { private double d = 0.2; private double i = 0.03; - private final PIDController xController = new PIDController(5.0, 0.0, 0.1); - private final PIDController yController = new PIDController(5.0, 0.0, 0.1); + public final PIDController xController = new PIDController(3.0, 0, 0.1); + public final PIDController yController = new PIDController(3.0, 0, 0.1); private final PIDController headingController = new PIDController(p, i, d); - private final PIDController xSetpointController = new PIDController(25.0, 0.0, 0.1); - private final PIDController ySetpointController = new PIDController(25.0, 0.0, 0.1); + private final PIDController xSetpointController = new PIDController(0.0, 0.0, 0); + private final PIDController ySetpointController = new PIDController(0.0, 0.0, 0); public Trigger atSetpointPose = - new Trigger(() -> xController.atSetpoint() && yController.atSetpoint()); + new Trigger( + () -> + xController.atSetpoint() + && yController.atSetpoint() + && headingController.atSetpoint()); private final SlewRateLimiter xLimiter = new SlewRateLimiter(7); // Limits acceleration to 3 mps private final SlewRateLimiter yLimiter = new SlewRateLimiter(7); @@ -112,6 +116,7 @@ public Drive( modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft); modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight); + // this.xController.setTolerance(0.05); // Usage reporting for swerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); @@ -234,6 +239,9 @@ private void runVelocity(ChassisSpeeds speeds) { // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); + Logger.recordOutput( + "SwerveChassisSpeeds/SetpointMag", + Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -391,22 +399,41 @@ public void followTrajectory(SwerveSample sample) { Pose2d pose = getPose(); Logger.recordOutput("Odometry/TrajectorySetpoint", pose); Logger.recordOutput("Drive/PID/error", headingController.getError()); - var out = headingController.calculate(pose.getRotation().getRadians(), sample.heading); - Logger.recordOutput("Drive/PID/out", out); - + // Logger.recordOutput("Drive/PID/out", out); + Logger.recordOutput("Drive/sample", sample); + + var err = new Transform2d(sample.x - pose.getX(), sample.y - pose.getY(), new Rotation2d()); + var dist = Math.hypot(err.getX(), err.getY()); + Logger.recordOutput("Drive/dist", dist); + + var target_pose = + (DriverStation.getAlliance().get() == Alliance.Blue + ? new Pose2d(4.476, 4.026, new Rotation2d()) + : new Pose2d(13.071, 4.026, new Rotation2d())); + var transform = target_pose.relativeTo(pose).rotateBy(pose.getRotation()); + Logger.recordOutput("Drive/targetpose", target_pose); + Logger.recordOutput("Drive/transform", transform); + Logger.recordOutput("Drive/theta", Math.atan2(transform.getY(), transform.getX())); + Logger.recordOutput( + "Drive/projected", + new Pose2d( + pose.getX(), + pose.getY(), + new Rotation2d(Math.atan2(transform.getY(), transform.getX())))); // Generate the next speeds for the robot + xController.setSetpoint(sample.x); + yController.setSetpoint(sample.y); ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - sample.vx + sample.vx != 0 - ? xSetpointController.calculate(pose.getX(), sample.x) - : xController.calculate(pose.getX(), sample.x), - sample.vy + sample.vy != 0 - ? ySetpointController.calculate(pose.getY(), sample.y) - : yController.calculate(pose.getY(), sample.y), - sample.omega - + headingController.calculate(pose.getRotation().getRadians(), sample.heading), + sample.vx + (sample.vx != 0 ? 0 : xController.calculate(pose.getX(), sample.x)), + sample.vy + (sample.vy != 0 ? 0 : yController.calculate(pose.getY(), sample.y)), + dist < 1 + ? headingController.calculate(pose.getRotation().getRadians(), sample.heading) + : headingController.calculate( + pose.getRotation().getRadians(), + Math.atan2(transform.getY(), transform.getX())), getRotation()); // Apply the generated speeds - + Logger.recordOutput("Drive/ChassisSpeeds1", speeds); runVelocity(speeds); } @@ -581,6 +608,12 @@ private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } + @AutoLogOutput(key = "SwerveChassisSpeeds/MeasuredMag") + private double getMeasuredChassisSpeedMag() { + var speeds = kinematics.toChassisSpeeds(getModuleStates()); + return Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + } + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java index c93d7e96..53007042 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java @@ -3,7 +3,7 @@ public class EjectorConstants { public static final int motorId = 30; public static final int sensorPort = 99; - public static final int currentLimit = 60; + public static final int currentLimit = 10; public static final double ejectorMoi = 0.025; public static final double ejectorReduction = 1; public static final double kP = 0; diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java index 597cadaa..032c3485 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java @@ -20,6 +20,7 @@ public class Elevator extends SubsystemBase { public Elevator(ElevatorIO io) { this.io = io; + this.pid.setTolerance(0.5); } @Override @@ -39,7 +40,12 @@ public Command goToSetpoint(Setpoints point) { setpoint = point; return run( () -> { - var out = pid.calculate(inputs.positionRotations, point.elevatorSetpoint()); + // if (setpoint == Setpoints.COLLECT) { + // pid.setP(0.1); + // } else { + // pid.setP(ElevatorConstants.kP); + // } + var out = pid.calculate(inputs.encoderReading, point.elevatorSetpoint()); Logger.recordOutput("Elevator/Output", out); Logger.recordOutput("Elevator/Error", pid.getError()); io.setVoltage(out); @@ -54,6 +60,10 @@ public Command zero() { // return run(() -> io.goToSetpoint(point)); // } + public Command setVoltage(double volts) { + return run(() -> io.setVoltage(volts)); + } + public Command stop() { return runOnce(() -> io.setVoltage(0)); } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java index f9bae532..72b3181f 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java @@ -39,11 +39,13 @@ public class ElevatorConstants { public static double allowedErr = 0.002; // Allowable position error for stability // Model Predictive Control Gains - public static double kP = 0.5; // Proportional gain TODO + public static double kP = 1; // Proportional gain TODO public static double kI = 0; // Integral gain TODO public static double kD = 0; // Derivative gain TODO public static double kA = 0; public static double kMinOutput = -12.0; // Minimum voltage output public static double kMaxOutput = 12.0; // Maximum voltage output public static double kV = 0; // Voltage constant (i for it from the datasheet) + + public static int currentLimit = 60; } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java index d2875e1d..499f879e 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONEO.java @@ -1,6 +1,5 @@ package org.curtinfrc.frc2025.subsystems.elevator; -import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.currentLimit; import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.*; import com.revrobotics.RelativeEncoder; @@ -23,7 +22,10 @@ public class ElevatorIONEO implements ElevatorIO { public ElevatorIONEO() { SparkMaxConfig config = new SparkMaxConfig(); - config.smartCurrentLimit(0, currentLimit).idleMode(IdleMode.kCoast).inverted(true); + config + .smartCurrentLimit(0, ElevatorConstants.currentLimit) + .idleMode(IdleMode.kCoast) + .inverted(true); SparkUtil.tryUntilOk( 5, diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index 30c3861a..c891b7a1 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -139,9 +139,9 @@ public Force getForceAtPosition(Translation2d position, Translation2d target) { public static final List FIELD_OBSTACLES = List.of( new SnowmanObstacle( - new Translation2d(4.49, 4), 0.6, Units.inchesToMeters(65.5 / 2.0), true), + new Translation2d(4.49, 4), 1.5, Units.inchesToMeters(65.5 / 2.0), true), new SnowmanObstacle( - new Translation2d(13.08, 4), 0.6, Units.inchesToMeters(65.5 / 2.0), true)); + new Translation2d(13.08, 4), 1.5, Units.inchesToMeters(65.5 / 2.0), true)); static final double FIELD_LENGTH = 16.42; static final double FIELD_WIDTH = 8.16; public static final List WALLS = @@ -313,7 +313,7 @@ public SwerveSample getCmd( new Translation2d(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); double currentSpeed = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); - double stepSize_m = maxSpeed * 0.02; // TODO + double stepSize_m = 10 * 0.02; // TODO if (goalOpt.isEmpty()) { return sample(pose.getTranslation(), pose.getRotation(), 0, 0, 0); } else { @@ -323,8 +323,9 @@ public SwerveSample getCmd( var err = curTrans.minus(goal); Logger.recordOutput("Repulsor/err", curTrans.getDistance(goal)); - Logger.recordOutput("Repulsor/step", stepSize_m * 1.5); - if (useGoal && err.getNorm() < stepSize_m * 1.5) { + Logger.recordOutput("Repulsor/toggle_dist", stepSize_m * 1.5); + + if (useGoal && err.getNorm() < stepSize_m * 5) { return sample(goal, goalRotation, 0, 0, 0); } else { var obstacleForce = getObstacleForce(curTrans, goal).plus(getWallForce(curTrans, goal)); @@ -334,21 +335,31 @@ public SwerveSample getCmd( SmartDashboard.putNumber("forceLog", netForce.getNorm()); // Calculate how quickly to move in this direction var closeToGoalMax = maxSpeed * Math.min(err.getNorm() / 2, 1); - - stepSize_m = Math.min(maxSpeed, closeToGoalMax) * 0.02; + var dist = err.getNorm(); + // var tts = 5/7; + // u = sqrt(2(7)(err)) + // stepSize_m = Math.min(maxSpeed, closeToGoalMax) * 0.02; + stepSize_m = Math.min(5.14, Math.sqrt(6 /* 14 */ * dist)) * 0.02; } - Logger.recordOutput("Repulsor/net", netForce); + Logger.recordOutput("Repulsor/step", stepSize_m); + Logger.recordOutput("Repulsor/net", netForce); + Logger.recordOutput("Repulsor/targetMag", stepSize_m); var step = new Translation2d(stepSize_m, netForce.getAngle()); var intermediateGoal = curTrans.plus(step); + var endTime = System.nanoTime(); SmartDashboard.putNumber("repulsorTimeS", (endTime - startTime) / 1e9); return sample( - intermediateGoal, + goal, goalRotation, - (step.getX() / 0.02) * Math.min(1.0, curTrans.getDistance(goal) / 3), - (step.getY() / 0.02) * Math.min(1.0, curTrans.getDistance(goal) / 3), + (step.getX() / 0.02) + /** Math.min(1.0, curTrans.getDistance(goal) / 10) */ + , + (step.getY() / 0.02) + /** Math.min(1.0, curTrans.getDistance(goal) / 10) */ + , 0); } } From d5d7f76ac6e169b8d108bb06c979b31b11e406c8 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 10 Feb 2025 15:08:21 +0800 Subject: [PATCH 16/19] add maple-sim :skull: --- .../java/org/curtinfrc/frc2025/Constants.java | 2 +- .../java/org/curtinfrc/frc2025/Robot.java | 100 ++++++++++++++---- .../frc2025/subsystems/drive/GyroIOSim.java | 81 +++----------- .../frc2025/subsystems/drive/ModuleIOSim.java | 89 ++++++++-------- .../curtinfrc/frc2025/util/PhoenixUtil.java | 14 +++ .../frc2025/util/RepulsorFieldPlanner.java | 15 +-- vendordeps/maple-sim.json | 26 +++++ 7 files changed, 180 insertions(+), 147 deletions(-) create mode 100644 vendordeps/maple-sim.json diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index 9dad8fa2..bcbd9a69 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -16,7 +16,7 @@ * (log replay from a file). */ public final class Constants { - public static final RobotType robotType = RobotType.DEVBOT; + public static final RobotType robotType = RobotType.SIMBOT; public static final double ROBOT_X = 660; // mm public static final double ROBOT_Y = 680; diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index cb42250d..fe1acaf7 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -1,5 +1,6 @@ package org.curtinfrc.frc2025; +import static edu.wpi.first.units.Units.Inches; import static org.curtinfrc.frc2025.subsystems.intake.IntakeConstants.intakeVolts; import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.*; @@ -7,15 +8,22 @@ import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Threads; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.HashMap; +import java.util.Map; import java.util.Set; import org.curtinfrc.frc2025.Constants.Mode; +import org.curtinfrc.frc2025.Constants.RobotType; import org.curtinfrc.frc2025.Constants.Setpoints; import org.curtinfrc.frc2025.generated.TunerConstants; import org.curtinfrc.frc2025.subsystems.drive.Drive; @@ -45,6 +53,10 @@ import org.curtinfrc.frc2025.subsystems.vision.VisionIOQuestNav; import org.curtinfrc.frc2025.util.AutoChooser; import org.curtinfrc.frc2025.util.VirtualSubsystem; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -76,6 +88,11 @@ public class Robot extends LoggedRobot { private final AutoFactory autoFactory; private final Autos autos; + private Joystick simJoystick; // Joystick for simulation input + private final Map simBindings = new HashMap<>(); + private final Trigger Z = new Trigger(() -> simJoystick.getRawAxis(0) == 1); + private final Trigger X = new Trigger(() -> simJoystick.getRawAxis(1) == 1); + public Robot() { // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); @@ -168,15 +185,39 @@ public Robot() { case SIMBOT -> { // Sim robot, instantiate physics sim IO implementations + // Create and configure a drivetrain simulation configuration + final DriveTrainSimulationConfig driveTrainSimulationConfig = + DriveTrainSimulationConfig.Default() + // Specify gyro type (for realistic gyro drifting and error simulation) + .withGyro(COTS.ofPigeon2()) + // Specify swerve module (for realistic swerve dynamics) + .withSwerveModule( + COTS.ofMark4( + DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60 + DCMotor.getFalcon500(1), // Steer motor is a Falcon 500 + COTS.WHEELS.COLSONS.cof, // Use the COF for Colson Wheels + 3)) // L3 Gear ratio + // Configures the track length and track width (spacing between swerve modules) + .withTrackLengthTrackWidth(Inches.of(24), Inches.of(24)) + // Configures the bumper size (dimensions of the robot bumper) + .withBumperSize(Inches.of(30), Inches.of(30)); + + final SwerveDriveSimulation swerveDriveSimulation = + new SwerveDriveSimulation( + // Specify Configuration + driveTrainSimulationConfig, + // Specify starting pose + new Pose2d(3, 3, new Rotation2d())); + + SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation); + drive = new Drive( - new GyroIOSim( - () -> drive.getKinematics(), - () -> drive.getModuleStates()) {}, // work around crash - new ModuleIOSim(TunerConstants.FrontLeft), - new ModuleIOSim(TunerConstants.FrontRight), - new ModuleIOSim(TunerConstants.BackLeft), - new ModuleIOSim(TunerConstants.BackRight)); + new GyroIOSim(swerveDriveSimulation.getGyroSimulation()) {}, // work around crash + new ModuleIOSim(swerveDriveSimulation.getModules()[0]), + new ModuleIOSim(swerveDriveSimulation.getModules()[1]), + new ModuleIOSim(swerveDriveSimulation.getModules()[2]), + new ModuleIOSim(swerveDriveSimulation.getModules()[3])); vision = new Vision( drive::addVisionMeasurement, @@ -187,6 +228,18 @@ public Robot() { elevator = new Elevator(new ElevatorIO() {}); intake = new Intake(new IntakeIOSim()); ejector = new Ejector(new EjectorIOSim()); + + simJoystick = new Joystick(0); // Assuming keyboard/joystick is ID 0 + + // Example keybindings (customize as needed) + simBindings.put("W", drive.joystickDrive(() -> 1.0, () -> 0.0, () -> 0.0)); // Forward + simBindings.put("S", drive.joystickDrive(() -> -1.0, () -> 0.0, () -> 0.0)); // Backward + simBindings.put("A", drive.joystickDrive(() -> 0.0, () -> 1.0, () -> 0.0)); // Left + simBindings.put("D", drive.joystickDrive(() -> 0.0, () -> -1.0, () -> 0.0)); // Right + simBindings.put("Q", drive.joystickDrive(() -> 0.0, () -> 0.0, () -> 0.5)); // Rotate CCW + simBindings.put("E", drive.joystickDrive(() -> 0.0, () -> 0.0, () -> -0.5)); // Rotate CW + // simBindings.put("Space", new InstantCommand(elevator::zero)); // Example elevator zero + // simBindings.put("R", new InstantCommand(ejector::eject)); // Example eject } } } else { @@ -263,11 +316,24 @@ public Robot() { RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler()); // Default command, normal field-relative drive - drive.setDefaultCommand( - drive.joystickDrive( - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); + if (Constants.robotType == RobotType.SIMBOT) { + // drive.setDefaultCommand( + // drive.joystickDrive( + // () -> -simJoystick.getRawAxis(0), () -> -simJoystick.getRawAxis(1), () -> 0)); + drive.setDefaultCommand(drive.joystickDrive(() -> 0, () -> 0, () -> 0)); + Z.whileTrue(superstructure.align(Setpoints.L2)); + X.whileTrue(superstructure.align(Setpoints.COLLECT)); + } else { + drive.setDefaultCommand( + drive.joystickDrive( + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + + controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3)); + controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2)); + controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT)); + } elevator.setDefaultCommand( Commands.defer(() -> elevator.goToSetpoint(Setpoints.COLLECT), Set.of(elevator))); @@ -337,10 +403,6 @@ public Robot() { new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); - - controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3)); - controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2)); - controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT)); } /** This function is called periodically during all modes. */ @@ -412,7 +474,7 @@ public void teleopPeriodic() { Logger.recordOutput("IntakeCommand", "null"); } - if (intake.getCurrentCommand() != null) { + if (drive.getCurrentCommand() != null) { Logger.recordOutput("DriveCommand", drive.getCurrentCommand().getName()); } else { Logger.recordOutput("DriveCommand", "null"); @@ -436,5 +498,7 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + SimulatedArena.getInstance().simulationPeriodic(); + } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIOSim.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIOSim.java index 300749d3..8cc32dba 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIOSim.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/GyroIOSim.java @@ -1,83 +1,26 @@ package org.curtinfrc.frc2025.subsystems.drive; -import static org.curtinfrc.frc2025.subsystems.drive.DriveConstants.ODOMETRY_FREQUENCY; +import static edu.wpi.first.units.Units.RadiansPerSecond; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.sim.Pigeon2SimState; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import java.util.Queue; -import java.util.function.Supplier; -import org.curtinfrc.frc2025.generated.TunerConstants; -import org.curtinfrc.frc2025.util.DeltaTime; +import org.curtinfrc.frc2025.util.PhoenixUtil; +import org.ironmaple.simulation.drivesims.GyroSimulation; -/** Simulated implementation of GyroIO for testing purposes. */ public class GyroIOSim implements GyroIO { - private final Pigeon2 pigeon = - new Pigeon2( - TunerConstants.DrivetrainConstants.Pigeon2Id, - TunerConstants.DrivetrainConstants.CANBusName); + private final GyroSimulation gyroSimulation; - private final StatusSignal yaw = pigeon.getYaw(); - private final Pigeon2SimState pigeonSimState; - - private final Supplier kinematics; - - private final Queue yawPositionQueue; - private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - - private Rotation2d rawGyroYaw = Rotation2d.fromDegrees(0); - private final DeltaTime deltaTime; - private final Supplier getStates; - - public GyroIOSim( - Supplier kinematics, Supplier getStates) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(50.0); - pigeon.optimizeBusUtilization(); - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - pigeonSimState = pigeon.getSimState(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); - - this.getStates = getStates; - this.deltaTime = new DeltaTime(true); - this.kinematics = kinematics; + public GyroIOSim(GyroSimulation gyroSimulation) { + this.gyroSimulation = gyroSimulation; } @Override public void updateInputs(GyroIOInputs inputs) { - final SwerveModuleState[] moduleStates = getStates.get(); - - rawGyroYaw = - rawGyroYaw.plus( - Rotation2d.fromRadians( - kinematics.get().toChassisSpeeds(moduleStates).omegaRadiansPerSecond - * deltaTime.get())); - - pigeonSimState.setRawYaw(rawGyroYaw.getDegrees()); - - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.connected = true; + inputs.yawPosition = gyroSimulation.getGyroReading(); + inputs.yawVelocityRadPerSec = + Units.degreesToRadians(gyroSimulation.getMeasuredAngularVelocity().in(RadiansPerSecond)); - inputs.odometryYawTimestamps = - yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) - .toArray(Rotation2d[]::new); - yawTimestampQueue.clear(); - yawPositionQueue.clear(); + inputs.odometryYawTimestamps = PhoenixUtil.getSimulationOdometryTimeStamps(); + inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java index 060b44d1..2e83fb93 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java @@ -13,15 +13,14 @@ package org.curtinfrc.frc2025.subsystems.drive; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import edu.wpi.first.math.MathUtil; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import java.util.Arrays; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; /** * Physics sim implementation of module IO. The sim models are configured using a set of module @@ -29,40 +28,34 @@ */ public class ModuleIOSim implements ModuleIO { // 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; - private static final double DRIVE_KS = 0.0; + private static final double DRIVE_KS = 0.03; private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); - private static final double TURN_KP = 8.0; - private static final double TURN_KD = 0.0; - private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); - private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); - private final DCMotorSim driveSim; - private final DCMotorSim turnSim; + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotor; + private final SimulatedMotorController.GenericMotorController turnMotor; private boolean driveClosedLoop = false; private boolean turnClosedLoop = false; - private PIDController driveController = new PIDController(DRIVE_KP, 0, DRIVE_KD); - private PIDController turnController = new PIDController(TURN_KP, 0, TURN_KD); + private final PIDController driveController; + private final PIDController turnController; private double driveFFVolts = 0.0; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim(SwerveModuleConstants constants) { - // Create drive and turn sim models - driveSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio), - DRIVE_GEARBOX); - turnSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), - TURN_GEARBOX); + public ModuleIOSim(SwerveModuleSimulation moduleSimulation) { + this.moduleSimulation = moduleSimulation; + this.driveMotor = + moduleSimulation + .useGenericMotorControllerForDrive() + .withCurrentLimit( + Amps.of(org.curtinfrc.frc2025.generated.TunerConstants.FrontLeft.SlipCurrent)); + this.turnMotor = moduleSimulation.useGenericControllerForSteer().withCurrentLimit(Amps.of(20)); + + this.driveController = new PIDController(0.05, 0.0, 0.0); + this.turnController = new PIDController(8.0, 0.0, 0.0); // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); @@ -73,42 +66,48 @@ public void updateInputs(ModuleIOInputs inputs) { // Run closed-loop control if (driveClosedLoop) { driveAppliedVolts = - driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); + driveFFVolts + + driveController.calculate( + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond)); } else { driveController.reset(); } if (turnClosedLoop) { - turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); + turnAppliedVolts = + turnController.calculate(moduleSimulation.getSteerAbsoluteFacing().getRadians()); } else { turnController.reset(); } // Update simulation state - driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); - driveSim.update(0.02); - turnSim.update(0.02); + driveMotor.requestVoltage(Volts.of(driveAppliedVolts)); + turnMotor.requestVoltage(Volts.of(turnAppliedVolts)); // Update drive inputs inputs.driveConnected = true; - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.drivePositionRad = moduleSimulation.getDriveWheelFinalPosition().in(Radians); + inputs.driveVelocityRadPerSec = moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + inputs.driveCurrentAmps = Math.abs(moduleSimulation.getDriveMotorStatorCurrent().in(Amps)); // Update turn inputs inputs.turnConnected = true; inputs.turnEncoderConnected = true; - inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAbsolutePosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnPosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnVelocityRadPerSec = + moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnCurrentAmps = Math.abs(moduleSimulation.getSteerMotorStatorCurrent().in(Amps)); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + inputs.odometryTimestamps = + org.curtinfrc.frc2025.util.PhoenixUtil.getSimulationOdometryTimeStamps(); + inputs.odometryDrivePositionsRad = + Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositions()) + .mapToDouble(angle -> angle.in(Radians)) + .toArray(); + inputs.odometryTurnPositions = moduleSimulation.getCachedSteerAbsolutePositions(); } @Override diff --git a/src/main/java/org/curtinfrc/frc2025/util/PhoenixUtil.java b/src/main/java/org/curtinfrc/frc2025/util/PhoenixUtil.java index 597150b7..c2c3905b 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/PhoenixUtil.java +++ b/src/main/java/org/curtinfrc/frc2025/util/PhoenixUtil.java @@ -13,8 +13,12 @@ package org.curtinfrc.frc2025.util; +import static edu.wpi.first.units.Units.Seconds; + import com.ctre.phoenix6.StatusCode; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; +import org.ironmaple.simulation.SimulatedArena; public class PhoenixUtil { /** Attempts to run the command until no error is produced. */ @@ -24,4 +28,14 @@ public static void tryUntilOk(int maxAttempts, Supplier command) { if (error.isOK()) break; } } + + public static double[] getSimulationOdometryTimeStamps() { + final double[] odometryTimeStamps = new double[SimulatedArena.getSimulationSubTicksIn1Period()]; + for (int i = 0; i < odometryTimeStamps.length; i++) { + odometryTimeStamps[i] = + Timer.getFPGATimestamp() - 0.02 + i * SimulatedArena.getSimulationDt().in(Seconds); + } + + return odometryTimeStamps; + } } diff --git a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java index 95f39664..8fe93d74 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java +++ b/src/main/java/org/curtinfrc/frc2025/util/RepulsorFieldPlanner.java @@ -325,12 +325,8 @@ public SwerveSample getCmd( if (useGoal) { netForce = getGoalForce(curTrans, goal).plus(netForce); SmartDashboard.putNumber("forceLog", netForce.getNorm()); - // Calculate how quickly to move in this direction var closeToGoalMax = maxSpeed * Math.min(err.getNorm() / 2, 1); var dist = err.getNorm(); - // var tts = 5/7; - // u = sqrt(2(7)(err)) - // stepSize_m = Math.min(maxSpeed, closeToGoalMax) * 0.02; stepSize_m = Math.min(5.14, Math.sqrt(6 /* 14 */ * dist)) * 0.02; } @@ -343,16 +339,7 @@ public SwerveSample getCmd( var endTime = System.nanoTime(); SmartDashboard.putNumber("repulsorTimeS", (endTime - startTime) / 1e9); - return sample( - goal, - goalRotation, - (step.getX() / 0.02) - /** Math.min(1.0, curTrans.getDistance(goal) / 10) */ - , - (step.getY() / 0.02) - /** Math.min(1.0, curTrans.getDistance(goal) / 10) */ - , - 0); + return sample(goal, goalRotation, (step.getX() / 0.02), (step.getY() / 0.02), 0); } } } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 00000000..68ae20d5 --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.3.8", + "frcYear": "2025", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.3.8" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} From 06c0ce31ed1c23a1a1495ea3a2df20b51428c9fa Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 10 Feb 2025 15:12:05 +0800 Subject: [PATCH 17/19] idk bro --- .../java/org/curtinfrc/frc2025/subsystems/drive/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 37958e45..d9c62094 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -81,8 +81,8 @@ public class Drive extends SubsystemBase { private double d = 0.2; private double i = 0.03; - public final PIDController xController = new PIDController(3.0, 0, 0.1); - public final PIDController yController = new PIDController(3.0, 0, 0.1); + public final PIDController xController = new PIDController(2.0, 0, 0); + public final PIDController yController = new PIDController(2.0, 0, 0); private final PIDController headingController = new PIDController(p, i, d); private final PIDController xSetpointController = new PIDController(0.0, 0.0, 0); From bcd7d01cd5be7c05c4cf7bc94dcf32adf2a41c70 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 10 Feb 2025 15:14:13 +0800 Subject: [PATCH 18/19] remove sm bs code --- src/main/java/org/curtinfrc/frc2025/Constants.java | 2 +- src/main/java/org/curtinfrc/frc2025/Robot.java | 10 ---------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index bcbd9a69..9dad8fa2 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -16,7 +16,7 @@ * (log replay from a file). */ public final class Constants { - public static final RobotType robotType = RobotType.SIMBOT; + public static final RobotType robotType = RobotType.DEVBOT; public static final double ROBOT_X = 660; // mm public static final double ROBOT_Y = 680; diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index fe1acaf7..386f7433 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -89,7 +89,6 @@ public class Robot extends LoggedRobot { private final Autos autos; private Joystick simJoystick; // Joystick for simulation input - private final Map simBindings = new HashMap<>(); private final Trigger Z = new Trigger(() -> simJoystick.getRawAxis(0) == 1); private final Trigger X = new Trigger(() -> simJoystick.getRawAxis(1) == 1); @@ -231,15 +230,6 @@ public Robot() { simJoystick = new Joystick(0); // Assuming keyboard/joystick is ID 0 - // Example keybindings (customize as needed) - simBindings.put("W", drive.joystickDrive(() -> 1.0, () -> 0.0, () -> 0.0)); // Forward - simBindings.put("S", drive.joystickDrive(() -> -1.0, () -> 0.0, () -> 0.0)); // Backward - simBindings.put("A", drive.joystickDrive(() -> 0.0, () -> 1.0, () -> 0.0)); // Left - simBindings.put("D", drive.joystickDrive(() -> 0.0, () -> -1.0, () -> 0.0)); // Right - simBindings.put("Q", drive.joystickDrive(() -> 0.0, () -> 0.0, () -> 0.5)); // Rotate CCW - simBindings.put("E", drive.joystickDrive(() -> 0.0, () -> 0.0, () -> -0.5)); // Rotate CW - // simBindings.put("Space", new InstantCommand(elevator::zero)); // Example elevator zero - // simBindings.put("R", new InstantCommand(ejector::eject)); // Example eject } } } else { From 2997d86d54e00fea5d81a46ac84a8c238f673f4d Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Mon, 10 Feb 2025 15:21:17 +0800 Subject: [PATCH 19/19] changes --- .../java/org/curtinfrc/frc2025/Robot.java | 6 +----- .../frc2025/subsystems/drive/ModuleIOSim.java | 20 ++++++++++--------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 386f7433..c5eba8d5 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -12,15 +12,12 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.HashMap; -import java.util.Map; import java.util.Set; import org.curtinfrc.frc2025.Constants.Mode; import org.curtinfrc.frc2025.Constants.RobotType; @@ -192,7 +189,7 @@ public Robot() { // Specify swerve module (for realistic swerve dynamics) .withSwerveModule( COTS.ofMark4( - DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60 + DCMotor.getKrakenX60Foc(1), // Drive motor is a Kraken X60 DCMotor.getFalcon500(1), // Steer motor is a Falcon 500 COTS.WHEELS.COLSONS.cof, // Use the COF for Colson Wheels 3)) // L3 Gear ratio @@ -229,7 +226,6 @@ public Robot() { ejector = new Ejector(new EjectorIOSim()); simJoystick = new Joystick(0); // Assuming keyboard/joystick is ID 0 - } } } else { diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java index 2e83fb93..695e578c 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java @@ -27,12 +27,6 @@ * constants from Phoenix. Simulation is always based on voltage control. */ public class ModuleIOSim implements ModuleIO { - // TunerConstants doesn't support separate sim constants, so they are declared locally - private static final double DRIVE_KS = 0.03; - private static final double DRIVE_KV_ROT = - 0.91035; // Same units as TunerConstants: (volt * secs) / rotation - private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); - private final SwerveModuleSimulation moduleSimulation; private final SimulatedMotorController.GenericMotorController driveMotor; private final SimulatedMotorController.GenericMotorController turnMotor; @@ -45,6 +39,15 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; + private static final double DRIVE_KP = 0.05; + private static final double DRIVE_KD = 0.0; + private static final double DRIVE_KS = 0.0; + private static final double DRIVE_KV_ROT = + 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + private static final double TURN_KP = 8.0; + private static final double TURN_KD = 0.0; + public ModuleIOSim(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; this.driveMotor = @@ -54,9 +57,8 @@ public ModuleIOSim(SwerveModuleSimulation moduleSimulation) { Amps.of(org.curtinfrc.frc2025.generated.TunerConstants.FrontLeft.SlipCurrent)); this.turnMotor = moduleSimulation.useGenericControllerForSteer().withCurrentLimit(Amps.of(20)); - this.driveController = new PIDController(0.05, 0.0, 0.0); - this.turnController = new PIDController(8.0, 0.0, 0.0); - + this.driveController = new PIDController(DRIVE_KP, 0, DRIVE_KD); + this.turnController = new PIDController(TURN_KP, 0, TURN_KD); // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); }