From d9f042262bce4af2efa722b3ecc7a041f2e08942 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Fri, 20 Sep 2024 09:35:51 +0800 Subject: [PATCH 1/8] added maple-sim library to dependencies --- example/5516-2024-OffSeason/build.gradle | 12 ++++++++++++ .../java/frc/robot/subsystems/drive/SwerveDrive.java | 4 ---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/example/5516-2024-OffSeason/build.gradle b/example/5516-2024-OffSeason/build.gradle index d4fcab4..92b8b51 100644 --- a/example/5516-2024-OffSeason/build.gradle +++ b/example/5516-2024-OffSeason/build.gradle @@ -67,6 +67,16 @@ repositories { password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" } } + + maven { + url = uri("https://maven.pkg.github.com/Shenzhen-Robotics-Alliance/maple-sim") + credentials { + username = "Shenzhen-Robotics-Alliance" + password = "\u0067\u0069\u0074\u0068\u0075\u0062\u005F\u0070\u0061\u0074\u005F\u0031\u0031\u0041\u0052\u0037\u0033\u0059\u004C\u0049\u0030\u0034\u0030\u0044\u004E\u006B\u0032\u006C\u0038\u004F\u004A\u006E\u0059\u005F\u0036\u0045\u0030\u006F\u0037\u004D\u0052\u004D\u0053\u0065\u006D\u0044\u0072\u0056\u006B\u0079\u0041\u006F\u0048\u004F\u0064\u0052\u007A\u0056\u0062\u0054\u0058\u0046\u004A\u0062\u0067\u006F\u0032\u0032\u0055\u0056\u0064\u0058\u0069\u004F\u0037\u0079\u0041\u004F\u0053\u0052\u004F\u005A\u0032\u0032\u0054\u0079\u006E\u0031\u0056\u0054\u004B\u006C\u0042" + } + } + + mavenLocal() } @@ -105,6 +115,8 @@ dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" + + implementation group: 'org.ironmaple', name: 'maple-sim', version: '0.0.1' } test { diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index fc8fcb2..04f21b9 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -249,10 +249,6 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix { From d76e53398d8969fdddca21f6ce33afe2b1aeadfb Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 13:51:42 +0800 Subject: [PATCH 2/8] fine tuned the chassis constants on the real robot --- .../robot/constants/DriveControlLoops.java | 4 +- .../frc/robot/constants/TunerConstants.java | 112 ++++++++++-------- 2 files changed, 62 insertions(+), 54 deletions(-) diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveControlLoops.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveControlLoops.java index cf9bdba..1623f92 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveControlLoops.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveControlLoops.java @@ -40,8 +40,8 @@ public class DriveControlLoops { 12/DriveTrainConstants.CHASSIS_MAX_VELOCITY ); public static final MaplePIDController.MaplePIDConfig DRIVE_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( - 5, - 2, + 3, + 3, 0, 0, 0, diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/TunerConstants.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/TunerConstants.java index f0dac7d..b500ec9 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/TunerConstants.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/TunerConstants.java @@ -1,33 +1,33 @@ -// TODO: For CTRE Chassis, generate this file using the Tuner -// after generated, you MUST replace all "private" labels with "public" -// you can do this with your IDE -// -// you MUST also change the numbers in package frc.robot.constants; -import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; + import edu.wpi.first.math.util.Units; -// Calibration File for 5516 Iron Maple's 2024 Off-Season Robot "11:59" // 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. // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput public static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.2) - .withKS(0).withKV(1.5).withKA(0); + .withKP(100).withKI(0).withKD(0.2) + .withKS(0).withKV(1.5).withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput public static final Slot0Configs driveGains = new Slot0Configs() - .withKP(3).withKI(0).withKD(0) - .withKS(0).withKV(0).withKA(0); + .withKP(3).withKI(0).withKD(0) + .withKS(0).withKV(0).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -41,30 +41,37 @@ public class TunerConstants { public static final double kSlipCurrentA = 150.0; // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. - // TODO: ALL current limits here will be overridden + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - public static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration(); + public static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true) + ); public static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs public static final Pigeon2Configuration pigeonConfigs = null; // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot - public static final double kSpeedAt12VoltsMps = 3.92; + public static final double kSpeedAt12VoltsMps = 5.96; // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - public static final double kCoupleRatio = 3.5714285714285716; + public static final double kCoupleRatio = 3.125; - public static final double kDriveGearRatio = 5.9; - public static final double kSteerGearRatio = 12.8; + public static final double kDriveGearRatio = 5.357142857142857; + public static final double kSteerGearRatio = 21.428571428571427; public static final double kWheelRadiusInches = 2; - public static final boolean kInvertLeftSide = true; - public static final boolean kInvertRightSide = false; + public static final boolean kInvertLeftSide = false; + public static final boolean kInvertRightSide = true; public static final String kCANbusName = "ChassisCanivore"; - public static final int kPigeonId = 15; + public static final int kPigeonId = 0; // These are only used for simulation @@ -73,7 +80,7 @@ public class TunerConstants { // Simulated voltage necessary to overcome friction public static final double kSteerFrictionVoltage = 0.25; public static final double kDriveFrictionVoltage = 0.25; - + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withCANbusName(kCANbusName) .withPigeon2Id(kPigeonId) @@ -101,55 +108,56 @@ public class TunerConstants { // Front Left - public static final int kFrontLeftDriveMotorId = 2; - public static final int kFrontLeftSteerMotorId = 1; - public static final int kFrontLeftEncoderId = 9; - public static final double kFrontLeftEncoderOffset = -0.181640625; + public static final int kFrontLeftDriveMotorId = 1; + public static final int kFrontLeftSteerMotorId = 2; + public static final int kFrontLeftEncoderId = 1; + public static final double kFrontLeftEncoderOffset = 0.303955078125; public static final boolean kFrontLeftSteerInvert = true; - public static final double kFrontLeftXPosInches = 10; - public static final double kFrontLeftYPosInches = 10; + public static final double kFrontLeftXPosInches = 9.25197; + public static final double kFrontLeftYPosInches = 11.614175; // Front Right - public static final int kFrontRightDriveMotorId = 4; - public static final int kFrontRightSteerMotorId = 3; - public static final int kFrontRightEncoderId = 10; - public static final double kFrontRightEncoderOffset = 0.06982421875; + public static final int kFrontRightDriveMotorId = 3; + public static final int kFrontRightSteerMotorId = 4; + public static final int kFrontRightEncoderId = 2; + public static final double kFrontRightEncoderOffset = -0.230224609375; public static final boolean kFrontRightSteerInvert = true; - public static final double kFrontRightXPosInches = 10; - public static final double kFrontRightYPosInches = -10; + public static final double kFrontRightXPosInches = 9.25197; + public static final double kFrontRightYPosInches = -11.614175; // Back Left - public static final int kBackLeftDriveMotorId = 8; - public static final int kBackLeftSteerMotorId = 7; - public static final int kBackLeftEncoderId = 12; - public static final double kBackLeftEncoderOffset = -0.0263671875; + public static final int kBackLeftDriveMotorId = 5; + public static final int kBackLeftSteerMotorId = 6; + public static final int kBackLeftEncoderId = 3; + public static final double kBackLeftEncoderOffset = -0.218017578125; public static final boolean kBackLeftSteerInvert = true; - public static final double kBackLeftXPosInches = -10; - public static final double kBackLeftYPosInches = 10; + public static final double kBackLeftXPosInches = -9.25197; + public static final double kBackLeftYPosInches = 11.614175; // Back Right - public static final int kBackRightDriveMotorId = 6; - public static final int kBackRightSteerMotorId = 5; - public static final int kBackRightEncoderId = 11; - public static final double kBackRightEncoderOffset = -0.114013671875; + public static final int kBackRightDriveMotorId = 7; + public static final int kBackRightSteerMotorId = 8; + public static final int kBackRightEncoderId = 4; + public static final double kBackRightEncoderOffset = -0.071533203125; public static final boolean kBackRightSteerInvert = true; - public static final double kBackRightXPosInches = -10; - public static final double kBackRightYPosInches = -10; - + public static final double kBackRightXPosInches = -9.25197; + public static final double kBackRightYPosInches = -11.614175; + + public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) .withSteerMotorInverted(kFrontLeftSteerInvert); public static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) .withSteerMotorInverted(kFrontRightSteerInvert); public static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide) + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide) .withSteerMotorInverted(kBackLeftSteerInvert); public static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide) + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); } From c552a9a00d7c2b6fbf0ffaedc44304cf90b0f772 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 13:52:17 +0800 Subject: [PATCH 3/8] moved simulation dependency to json file --- .../AdvantageScope-Simulation.json | 212 ++++++++++++++++++ example/5516-2024-OffSeason/build.gradle | 4 - .../CompetitionFieldSimulation.java | 2 +- .../Simulations/SwerveDriveSimulation.java | 8 +- .../5516-2024-OffSeason/vendordeps/Dyn4j.json | 23 -- .../vendordeps/maple-sim.json | 40 ++++ 6 files changed, 254 insertions(+), 35 deletions(-) create mode 100644 example/5516-2024-OffSeason/AdvantageScope-Simulation.json delete mode 100644 example/5516-2024-OffSeason/vendordeps/Dyn4j.json create mode 100644 example/5516-2024-OffSeason/vendordeps/maple-sim.json diff --git a/example/5516-2024-OffSeason/AdvantageScope-Simulation.json b/example/5516-2024-OffSeason/AdvantageScope-Simulation.json new file mode 100644 index 0000000..209d5be --- /dev/null +++ b/example/5516-2024-OffSeason/AdvantageScope-Simulation.json @@ -0,0 +1,212 @@ +{ + "hubs": [ + { + "x": 302, + "y": 184, + "width": 1106, + "height": 650, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/LiveWindow", + "/AdvantageKit", + "/AdvantageKit/RealOutputs", + "/AdvantageKit/RealOutputs/Field", + "/AdvantageKit/RealOutputs/Shooter", + "/AdvantageKit/RealOutputs/SwerveStates", + "/AdvantageKit/RealOutputs/Odometry", + "/AdvantageKit/RealOutputs/FieldSimulation", + "/SmartDashboard", + "/SmartDashboard/MaplePhysicsSimulation", + "/SmartDashboard/MapleArenaSimulation", + "/AdvantageKit/RealOutputs/Odometry/Robot" + ] + }, + "tabs": { + "selected": 2, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "t651a9wcsm4uv2nn7i1w8unwiw3a5n84", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Line Graph", + "controller": { + "leftSources": [], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "a40mbm1463684stb0lw9q99k08q674jn", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "3D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs//Field/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "11:59" + } + }, + { + "type": "component", + "logKey": "NT:/AdvantageKit/RealOutputs/Shooter/MechanismPose", + "logType": "Pose3d[]", + "visible": true, + "options": {} + }, + { + "type": "swerveStates", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs//Field/OpponentRobot", + "logType": "Pose3d[]", + "visible": true, + "options": { + "model": "2024 KitBot" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/Shooter/NoteInShooter", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Note" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs//Field/Note", + "logType": "Pose3d[]", + "visible": true, + "options": { + "variant": "Note" + } + }, + { + "type": "trajectory", + "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Trajectory", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#00ffff", + "size": "normal" + } + }, + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs//Field/AlliancePartnerRobot", + "logType": "Pose3d[]", + "visible": true, + "options": { + "model": "2024 KitBot" + } + } + ], + "game": "2024 Field", + "origin": "blue" + }, + "controllerUUID": "z31ichy013czxpxjwwhbvvdciqbfun51", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + -13.099515367064587, + 5.622905314106006, + -1.1187082198216616 + ], + "cameraTarget": [ + 2.1564592708968564, + -3.4497661190395785, + 1.6037949750703593 + ] + }, + "controlsHeight": 200 + }, + { + "type": 4, + "title": "Table", + "controller": [ + "NT:/SmartDashboard/MapleArenaSimulation/difference", + "NT:/SmartDashboard/MapleArenaSimulation/chassisFrictionForce" + ], + "controllerUUID": "lx7skwjvbdch6kn71vxrmu6561rbc2q3", + "renderer": null, + "controlsHeight": 0 + }, + { + "type": 9, + "title": "Swerve", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Setpoints", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "x7ibv6cg5jcq5xxna17n6t1e1gzc18bu", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.0.0-beta-1" +} diff --git a/example/5516-2024-OffSeason/build.gradle b/example/5516-2024-OffSeason/build.gradle index 92b8b51..dbed367 100644 --- a/example/5516-2024-OffSeason/build.gradle +++ b/example/5516-2024-OffSeason/build.gradle @@ -58,7 +58,6 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true -// Configuration for AdvantageKit repositories { maven { url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") @@ -76,7 +75,6 @@ repositories { } } - mavenLocal() } @@ -115,8 +113,6 @@ dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" - - implementation group: 'org.ironmaple', name: 'maple-sim', version: '0.0.1' } test { diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java index ba65a5a..023b9c3 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java @@ -35,7 +35,7 @@ public abstract class CompetitionFieldSimulation { protected final HolonomicChassisSimulation mainRobot; protected final Set gamePieces; - private List intakeSimulations = new ArrayList<>(); + private final List intakeSimulations = new ArrayList<>(); public CompetitionFieldSimulation(HolonomicChassisSimulation mainRobot, FieldObstaclesMap obstaclesMap) { this.competitionField = new CompetitionFieldVisualizer(mainRobot); diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java index 122c289..7f6867c 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import frc.robot.Robot; -import frc.robot.constants.LogPaths; import frc.robot.subsystems.drive.IO.GyroIOSim; import frc.robot.subsystems.drive.IO.ModuleIOSim; import frc.robot.subsystems.drive.IO.OdometryThread; @@ -30,8 +29,6 @@ * and feed the result of the physics simulation back to ModuleIOSim, to simulate the odometry encoders' readings * */ public class SwerveDriveSimulation extends HolonomicChassisSimulation { - private static final String LOG_PATH = LogPaths.PHYSICS_SIMULATION_PATH + "SwerveDriveSim/"; - private final GyroIOSim gyroIOSim; private final ModuleIOSim[] modules; private final Consumer resetOdometryCallBack; @@ -87,10 +84,7 @@ private void moduleSimulationSubTick( /* simulate the propelling force of the module */ final Rotation2d moduleWorldFacing = module.getSimulationSteerFacing().plus(robotWorldPose.getRotation()); - final Vector2 moduleWorldPosition = GeometryConvertor.toDyn4jVector2( - robotWorldPose.getTranslation() - .plus(moduleTranslationOnRobot.rotateBy(robotWorldPose.getRotation())) - ); + final Vector2 moduleWorldPosition = getWorldPoint(GeometryConvertor.toDyn4jVector2(moduleTranslationOnRobot)); double actualPropellingForceOnFloorNewtons = module.getSimulationTorque() / WHEEL_RADIUS_METERS; final boolean skidding = Math.abs(actualPropellingForceOnFloorNewtons) > MAX_FRICTION_FORCE_PER_MODULE; if (skidding) diff --git a/example/5516-2024-OffSeason/vendordeps/Dyn4j.json b/example/5516-2024-OffSeason/vendordeps/Dyn4j.json deleted file mode 100644 index 4f7c996..0000000 --- a/example/5516-2024-OffSeason/vendordeps/Dyn4j.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "fileName": "Dyn4j.json", - "name": "MapleSim", - "version": "0.0.1", - "uuid": "123e4567-e89b-12d3-a456-426614174000", - "frcYear": "2024", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "junit", - "artifactId": "junit", - "version": "4.13.1" - }, - { - "groupId": "org.dyn4j", - "artifactId": "dyn4j", - "version": "5.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} diff --git a/example/5516-2024-OffSeason/vendordeps/maple-sim.json b/example/5516-2024-OffSeason/vendordeps/maple-sim.json new file mode 100644 index 0000000..0655d1e --- /dev/null +++ b/example/5516-2024-OffSeason/vendordeps/maple-sim.json @@ -0,0 +1,40 @@ +{ + "fileName": "maple-sim.json", + "name": "MapleSim", + "version": "0.0.1", + "uuid": "123e4567-e89b-12d3-a456-426614174000", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "junit", + "artifactId": "junit", + "version": "4.13.1" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.2.1" + }, + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.8" + }, + { + "groupId": "org.ironmaple", + "artifactId": "maple-sim", + "version": "0.0.1" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} From 8d6313d04dbd657389cad4af06bb26005719e52b Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 13:52:54 +0800 Subject: [PATCH 4/8] code readability --- .../frc/robot/subsystems/vision/apriltags/AprilTagVision.java | 3 +-- .../subsystems/vision/apriltags/AprilTagVisionIOReal.java | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java index e3f4a31..5c36967 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java @@ -43,7 +43,6 @@ public AprilTagVision(AprilTagVisionIO io, List camerasP this.driveSubsystem = driveSubsystem; } - private Optional result = Optional.empty(); @Override public void periodic(double dt, boolean enabled) { io.updateInputs(inputs); @@ -52,7 +51,7 @@ public void periodic(double dt, boolean enabled) { for (int i = 0; i < inputs.camerasInputs.length; i++) this.camerasDisconnectedAlerts[i].setActivated(!inputs.camerasInputs[i].cameraConnected); - result = multiTagPoseEstimator.estimateRobotPose(inputs.camerasInputs, driveSubsystem.getPose()); + Optional result = multiTagPoseEstimator.estimateRobotPose(inputs.camerasInputs, driveSubsystem.getPose()); result = discardResultIfOverThreshold(result); result.ifPresent(robotPoseEstimationResult -> driveSubsystem.addVisionMeasurement( robotPoseEstimationResult.pointEstimation, diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIOReal.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIOReal.java index d95e5ac..f48fb8c 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIOReal.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIOReal.java @@ -23,7 +23,6 @@ public AprilTagVisionIOReal(List cameraProperties) { public void updateInputs(VisionInputs inputs) { if (inputs.camerasAmount != cameras.length) throw new IllegalStateException("inputs camera amount (" + inputs.camerasAmount + ") does not match actual cameras amount"); - for (int i = 0; i < cameras.length; i++) if (cameras[i].isConnected()) inputs.camerasInputs[i].fromPhotonPipeLine( From 76f13da307243fb00284489555169694d15e7431 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 13:52:59 +0800 Subject: [PATCH 5/8] better auto stage --- .../robot/commands/shooter/AimAndShootSequence.java | 8 ++++---- .../java/frc/robot/commands/shooter/PrepareToAim.java | 2 +- .../main/java/frc/robot/constants/PitchConstants.java | 10 +++++----- .../frc/robot/subsystems/drive/IO/ModuleIOSim.java | 1 - .../java/frc/robot/subsystems/shooter/FlyWheels.java | 7 +++++++ .../main/java/frc/robot/subsystems/shooter/Pitch.java | 4 ++++ .../java/frc/robot/subsystems/shooter/PitchIOSim.java | 5 +++-- .../robot/subsystems/shooter/ShooterVisualizer.java | 2 +- 8 files changed, 25 insertions(+), 14 deletions(-) diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java index 245b4f7..22c7d1c 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java @@ -60,10 +60,10 @@ public AimAndShootSequence( super.addRequirements(pitch, flyWheels, intake); super.addCommands(Commands.runOnce(intake::runIdle)); -// super.addCommands( -// new PrepareToAim(flyWheels, pitch, shooterOptimization, ledStatusLight, robotScoringPositionSupplier, targetPositionSupplier) -// .untilReady() -// ); + super.addCommands( + new PrepareToAim(flyWheels, pitch, shooterOptimization, ledStatusLight, robotScoringPositionSupplier, targetPositionSupplier) + .untilReady() + ); final AimAtSpeakerContinuously aimAtSpeakerContinuously = new AimAtSpeakerContinuously( flyWheels, pitch, ledStatusLight, shooterOptimization, drive, targetPositionSupplier, externalShootCondition ); diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java index 8c18d7e..ffee937 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java @@ -75,6 +75,6 @@ public Command untilReady() { } public boolean isReady() { - return running && flyWheels.flyWheelsReady() && pitch.inPosition(); + return running && flyWheels.flyWheelsRoughlyReady() && pitch.roughlyInPosition(); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/PitchConstants.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/PitchConstants.java index 510f993..1faeaa8 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/PitchConstants.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/PitchConstants.java @@ -9,13 +9,13 @@ public class PitchConstants { public static final double PITCH_HIGHER_LIMIT_RAD = Math.toRadians(100); public static final double PITCH_KS = 0.03; - public static final double PITCH_KG = 0.1; - public static final double PITCH_KV = 3.2; - public static final double PITCH_KA = 0.01; + public static final double PITCH_KG = 0.17; + public static final double PITCH_KV = 3.5; + public static final double PITCH_KA = 0.03; public static final MaplePIDController.MaplePIDConfig PITCH_PID = new MaplePIDController.MaplePIDConfig( 7.5, - Math.toRadians(26), + Math.toRadians(15), 0, Math.toRadians(2), 0.05, @@ -24,6 +24,6 @@ public class PitchConstants { ); public static final TrapezoidProfile.Constraints PITCH_PROFILE_CONSTRAIN = new TrapezoidProfile.Constraints( - Math.toRadians(360), Math.toRadians(720) + Math.toRadians(360.0), Math.toRadians(360.0 / 0.2) ); } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java index fcb4701..f9bcd57 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java @@ -97,7 +97,6 @@ public double getSimulationTorque() { driveAppliedVolts ); - return DRIVE_MOTOR.getTorque(driveActualCurrent) * DRIVE_GEAR_RATIO; } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java index 32eb454..fb08f90 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java @@ -147,4 +147,11 @@ public boolean flyWheelsReady() { return false; return true; } + + public boolean flyWheelsRoughlyReady() { + for (FlyWheelIO.FlyWheelsInputs input:inputs) + if (Math.abs(input.flyWheelVelocityRevsPerSec * 60 - goalRPM) > 300) + return false; + return true; + } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java index 651cc29..5fb8bf6 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java @@ -136,4 +136,8 @@ private void runControlLoops(double currentPositionSetPointRad, double currentVe public boolean inPosition() { return Math.abs(inputs.pitchAngleRad - setPointRad) < PITCH_PID.errorTolerance; } + + public boolean roughlyInPosition() { + return Math.abs(inputs.pitchAngleRad - setPointRad) < Math.toRadians(7); + } } \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/PitchIOSim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/PitchIOSim.java index 5eb6e7e..3d8b80b 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/PitchIOSim.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/PitchIOSim.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Robot; @@ -13,7 +14,7 @@ public PitchIOSim() { this.armSim = new SingleJointedArmSim( DCMotor.getFalcon500(2), GEAR_RATIO, - SingleJointedArmSim.estimateMOI(0.5, 11), + SingleJointedArmSim.estimateMOI(0.5, 15), 0.5, PITCH_LOWEST_ROTATION_RAD, PITCH_HIGHER_LIMIT_RAD, @@ -33,6 +34,6 @@ public void updateInputs(PitchInputs pitchInputs) { @Override public void runPitchVoltage(double volts) { - armSim.setInputVoltage(volts); + armSim.setInputVoltage(MathUtil.clamp(volts, -12, 12)); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/ShooterVisualizer.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/ShooterVisualizer.java index cc30c8c..f12674c 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/ShooterVisualizer.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/ShooterVisualizer.java @@ -38,7 +38,7 @@ public static void showResultsToDashboard(Pose3d robotPose) { final Pose3d shooterPoseToRobot = new Pose3d( SHOOTER_TRANSLATION_ON_ROBOT, new Rotation3d(0, -pitchAngleRad, 0)); - Logger.recordOutput(LogPaths.SHOOTER_PATH + "MechanismPose", shooterPoseToRobot); + Logger.recordOutput(LogPaths.SHOOTER_PATH + "MechanismPose", new Pose3d[] {shooterPoseToRobot}); final Transform3d robotToNote = new Transform3d( NOTE_AT_SHOOTER_AXLE_TRANSLATION_ON_ROBOT, new Rotation3d(0, -pitchAngleRad, 0) From 17df6dbc2f03c28fba3dd024d8157bcdfe9c6b29 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 15:04:24 +0800 Subject: [PATCH 6/8] fixed a bug in pitch idle --- .../src/main/java/frc/robot/autos/AutoUtils.java | 6 +++--- .../main/java/frc/robot/subsystems/shooter/FlyWheels.java | 4 ++-- .../src/main/java/frc/robot/subsystems/shooter/Pitch.java | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java index c4bba7e..e63a5ca 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java @@ -6,11 +6,11 @@ public class AutoUtils { public static Command setIdleForSuperStructureCommand(RobotContainer robot) { - return Commands.runOnce(() -> setIdleForSuperStructuer(robot), robot.intake, robot.pitch, robot.flyWheels); + return Commands.runOnce(() -> setIdleForSuperStructure(robot), robot.intake, robot.pitch, robot.flyWheels); } - public static void setIdleForSuperStructuer(RobotContainer robot) { + public static void setIdleForSuperStructure(RobotContainer robot) { robot.intake.runIdle(); robot.flyWheels.runIdle(); - robot.pitch.setIdle(); + robot.pitch.runIdle(); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java index fb08f90..134684d 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java @@ -67,6 +67,7 @@ public void periodic(double dt, boolean enabled) { flyWheelPeriodic(i); Logger.recordOutput("Shooter/FlyWheelsGoalRPM", goalRPM); + Logger.recordOutput("Shooter/Control Loop Profiled Set point (RPM)", currentStateRPM.position); SmartDashboard.putBoolean("FlyWheels Ready", flyWheelsReady()); SmartDashboard.putNumber("FlyWheels Actual RPM", inputs[0].flyWheelVelocityRevsPerSec * 60); } @@ -93,6 +94,7 @@ private Consumer> voltageMeasureRunner(int index) { public void runIdle() { for (FlyWheelIO io : IOs) io.runVoltage(0); + currentStateRPM = speedRPMProfile.calculate(Robot.defaultPeriodSecs, currentStateRPM, new TrapezoidProfile.State(0, 0)); goalRPM = 0; } @@ -123,8 +125,6 @@ public void runStaticRPMSetPoint(double rpm, double rateOfChangePerSec) { } private void runControlLoops() { - Logger.recordOutput("Shooter/Control Loop Setpoint (RPM)", currentStateRPM.position); - for (int i = 0; i < IOs.length; i++) { final double flyWheelVelocityRevPerSec = currentStateRPM.position / 60, flyWheelAccelerationRevPerSec = currentStateRPM.velocity / 60, diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java index 5fb8bf6..568a304 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java @@ -41,7 +41,7 @@ public Pitch(PitchIO io) { } public Command getPitchDefaultCommand() { - return Commands.run(() -> runSetPointProfiled(PITCH_LOWEST_ROTATION_RAD), this); + return Commands.run(() -> this.runSetPointProfiled(PITCH_LOWEST_ROTATION_RAD), this); } @Override @@ -69,7 +69,7 @@ public void periodic(double dt, boolean enabled) { SmartDashboard.putBoolean("Pitch In Position", inPosition()); } - public void setIdle() { + public void runIdle() { io.runPitchVoltage(0); } From e2bf80071c0acf6cd129bc55b9d73132427314b3 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 17:29:42 +0800 Subject: [PATCH 7/8] fixed some bugs in autonomous aiming --- .../frc/robot/autos/AmpSideSixNotesFast.java | 9 +++---- .../main/java/frc/robot/autos/AutoUtils.java | 10 +++----- .../commands/drive/ChassisFaceToRotation.java | 8 ++++++- .../commands/shooter/AimAndShootSequence.java | 6 +---- .../commands/shooter/AimAtSpeakerFactory.java | 6 ++++- .../shooter/FollowPathGrabAndShootStill.java | 8 +++++++ .../robot/commands/shooter/PrepareToAim.java | 2 +- .../robot/subsystems/intake/IntakeIOSim.java | 4 ++-- .../robot/subsystems/shooter/FlyWheels.java | 24 +++++++++++-------- .../frc/robot/subsystems/shooter/Pitch.java | 13 ++++------ 10 files changed, 50 insertions(+), 40 deletions(-) diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java index 19f082b..2248208 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java @@ -12,6 +12,8 @@ import frc.robot.commands.shooter.AimAtSpeakerFactory; import frc.robot.commands.shooter.FollowPathGrabAndShootStill; +import java.util.HashSet; + public class AmpSideSixNotesFast implements Auto { @Override @@ -24,7 +26,6 @@ public Command getAutoCommand(RobotContainer robot) { robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, robot.visualizerForShooter )); - commands.addCommands(AutoUtils.setIdleForSuperStructureCommand(robot)); /* grab second */ final Command chassisMoveToSecond = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to second and grab fast")) @@ -34,7 +35,7 @@ public Command getAutoCommand(RobotContainer robot) { .finallyDo(() -> noteFightFailed[0] = !robot.intake.isNotePresent()) ); commands.addCommands(grabSecondWhenClose.deadlineWith( - chassisMoveToSecond.alongWith(robot.pitch.getPitchDefaultCommand()) + chassisMoveToSecond.alongWith(AutoUtils.getSuperStructureIdleCommand(robot)) )); /* retry grab second */ @@ -54,7 +55,6 @@ public Command getAutoCommand(RobotContainer robot) { robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, robot.visualizerForShooter )); - commands.addCommands(AutoUtils.setIdleForSuperStructureCommand(robot)); /* move to third and grab */ @@ -63,7 +63,7 @@ public Command getAutoCommand(RobotContainer robot) { final Command moveToThirdAlter = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to third fast alter")) .andThen(Commands.runOnce(robot.drive::stop, robot.drive)); final Command moveToThirdDecisive = new CommandOnFly(() -> noteFightFailed[0] ? moveToThirdAlter : moveToThirdNormal) - .deadlineWith(robot.pitch.getPitchDefaultCommand()); + .deadlineWith(AutoUtils.getSuperStructureIdleCommand(robot)); final Command intakeThird = Commands.waitSeconds(1) .andThen(robot.intake.suckNoteUntilTouching().withTimeout(4)); commands.addCommands(intakeThird.deadlineWith(moveToThirdDecisive)); @@ -104,6 +104,7 @@ public Command getAutoCommand(RobotContainer robot) { } @Override public Pose2d getStartingPoseAtBlueAlliance() { + // return new Pose2d(1.47, 6.32, Rotation2d.fromDegrees(180)); return new Pose2d(1.47, 6.32, Rotation2d.fromDegrees(-150)); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java index e63a5ca..8eb149c 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java @@ -5,12 +5,8 @@ import frc.robot.RobotContainer; public class AutoUtils { - public static Command setIdleForSuperStructureCommand(RobotContainer robot) { - return Commands.runOnce(() -> setIdleForSuperStructure(robot), robot.intake, robot.pitch, robot.flyWheels); - } - public static void setIdleForSuperStructure(RobotContainer robot) { - robot.intake.runIdle(); - robot.flyWheels.runIdle(); - robot.pitch.runIdle(); + public static Command getSuperStructureIdleCommand(RobotContainer robot) { + return robot.flyWheels.getFlyWheelsDefaultCommand() + .alongWith(robot.pitch.run(robot.pitch::runIdle)); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java index f0774c2..3ffff08 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java @@ -8,6 +8,7 @@ import frc.robot.constants.DriveControlLoops; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.utils.CustomPIDs.MaplePIDController; +import org.littletonrobotics.junction.Logger; import java.util.function.Supplier; @@ -37,6 +38,7 @@ public void initialize() { public void execute() { final double rotationFeedBack = chassisRotationController.calculate(driveSubsystem.getFacing().getRadians(), targetRotationSupplier.get().getRadians()); driveSubsystem.runRobotCentricChassisSpeeds(new ChassisSpeeds(0, 0, rotationFeedBack)); + Logger.recordOutput("FaceToRotationCommand/ErrorDegrees", getError().getDegrees()); } @Override @@ -46,7 +48,11 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return driveSubsystem.getFacing().minus(targetRotationSupplier.get()).getRadians() < tolerance.getRadians(); + return Math.abs(getError().getRadians()) < tolerance.getRadians(); + } + + private Rotation2d getError() { + return driveSubsystem.getFacing().minus(targetRotationSupplier.get()); } public static ChassisFaceToRotation faceToTarget(HolonomicDriveSubsystem driveSubsystem, Supplier targetPositionSupplier) { diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java index 22c7d1c..00f923f 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAndShootSequence.java @@ -60,10 +60,6 @@ public AimAndShootSequence( super.addRequirements(pitch, flyWheels, intake); super.addCommands(Commands.runOnce(intake::runIdle)); - super.addCommands( - new PrepareToAim(flyWheels, pitch, shooterOptimization, ledStatusLight, robotScoringPositionSupplier, targetPositionSupplier) - .untilReady() - ); final AimAtSpeakerContinuously aimAtSpeakerContinuously = new AimAtSpeakerContinuously( flyWheels, pitch, ledStatusLight, shooterOptimization, drive, targetPositionSupplier, externalShootCondition ); @@ -81,7 +77,7 @@ public AimAndShootSequence( final Command visualizeNoteFullCommand = Commands .waitUntil(() -> !intake.isNotePresent()) .andThen(visualizeNote) - .onlyIf(() -> intake.isNotePresent()); + .onlyIf(intake::isNotePresent); final Command waitForRightTimingAndShoot = Commands.waitUntil(aimAtSpeakerContinuously::readyToShoot) .andThen(intake.executeLaunch().alongWith(visualizeNoteFullCommand)); super.addCommands(aimAtSpeakerContinuously.raceWith(waitForRightTimingAndShoot)); diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java index 053dc1d..598a5a8 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/AimAtSpeakerFactory.java @@ -28,6 +28,10 @@ public static Command shootAtSpeakerStill(HolonomicDriveSubsystem drive, Intake final Command aimAtSpeakerStill = semiAutoAimAndShoot.deadlineWith(chassisAimAtSpeaker); aimAtSpeakerStill.addRequirements(drive, pitch, flyWheels); - return aimAtSpeakerStill.onlyIf(intake::isNotePresent); + return aimAtSpeakerStill.onlyIf(intake::isNotePresent).finallyDo(() -> { + pitch.runIdle(); + intake.runIdle(); + flyWheels.runIdle(); + }); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FollowPathGrabAndShootStill.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FollowPathGrabAndShootStill.java index 25c0c3c..f0acdc6 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FollowPathGrabAndShootStill.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/FollowPathGrabAndShootStill.java @@ -26,6 +26,8 @@ public FollowPathGrabAndShootStill(PathPlannerPath pathAtBlueAlliance, double di () -> MaplePathPlannerLoader.getEndingRobotPoseInCurrentAllianceSupplier(pathAtBlueAlliance).get().getTranslation() .getDistance(driveSubsystem.getPose().getTranslation()) < distanceToTargetMetersStartPreparing ) + .deadlineWith(pitch.run(pitch::runIdle)) + .deadlineWith(flyWheels.getFlyWheelsDefaultCommand()) .andThen(new PrepareToAim( flyWheels, pitch, shooterOptimization, statusLight, () -> MaplePathPlannerLoader.getEndingRobotPoseInCurrentAllianceSupplier(pathAtBlueAlliance).get().getTranslation(), @@ -39,5 +41,11 @@ public FollowPathGrabAndShootStill(PathPlannerPath pathAtBlueAlliance, double di super.addCommands(AimAtSpeakerFactory.shootAtSpeakerStill( driveSubsystem, intake, pitch, flyWheels, shooterOptimization, statusLight, visualizer )); + + super.addCommands(Commands.runOnce(() -> { + intake.runIdle(); + flyWheels.runIdle(); + pitch.runIdle(); + }, intake, pitch, flyWheels)); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java index ffee937..8c18d7e 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/PrepareToAim.java @@ -75,6 +75,6 @@ public Command untilReady() { } public boolean isReady() { - return running && flyWheels.flyWheelsRoughlyReady() && pitch.roughlyInPosition(); + return running && flyWheels.flyWheelsReady() && pitch.inPosition(); } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 55fcd18..8e60c71 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -7,10 +7,10 @@ public class IntakeIOSim extends IntakeSimulation implements IntakeIO { /* running the intake in full voltage for 0.5 seconds is what it takes to get the note from bottom to top */ - private static final double VOLTAGE_INTEGRAL_SINCE_NOTE_IN_INTAKE_WHEN_NOTE_IN_POSITION = 4 * 0.5; + private static final double VOLTAGE_INTEGRAL_SINCE_NOTE_IN_INTAKE_WHEN_NOTE_IN_POSITION = 4 * 0.3; private static final double VOLTAGE_INTEGRAL_SINCE_NOTE_IN_INTAKE_WHEN_NOTE_LEAVE = VOLTAGE_INTEGRAL_SINCE_NOTE_IN_INTAKE_WHEN_NOTE_IN_POSITION - + 4 * 0.5; + + 4 * 0.3; /* the integral of the voltage of the intake ever since the note touches the lower gate */ private double voltageIntegralSinceNoteInIntake; diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java index 134684d..d6aea24 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/FlyWheels.java @@ -8,6 +8,7 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; @@ -17,6 +18,8 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import java.util.Arrays; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.Consumer; import static frc.robot.constants.ShooterConstants.*; @@ -58,7 +61,7 @@ public FlyWheels(FlyWheelIO[] IOs) { this.currentStateRPM = new TrapezoidProfile.State(0, 0); this.goalRPM = 0; - setDefaultCommand(Commands.run(this::runIdle, this)); + setDefaultCommand(getFlyWheelsDefaultCommand()); } @Override @@ -92,9 +95,17 @@ private Consumer> voltageMeasureRunner(int index) { return (voltageMeasure -> runVolts(index, voltageMeasure.in(Units.Volt))); } + public Command getFlyWheelsDefaultCommand() { + return Commands.run(this::runIdle, this); + } public void runIdle() { - for (FlyWheelIO io : IOs) io.runVoltage(0); - currentStateRPM = speedRPMProfile.calculate(Robot.defaultPeriodSecs, currentStateRPM, new TrapezoidProfile.State(0, 0)); + for (FlyWheelIO io : IOs) + io.runVoltage(0); + + double totalRPM = 0.0; + for (FlyWheelIO.FlyWheelsInputs input:inputs) + totalRPM += input.flyWheelVelocityRevsPerSec * 60; + currentStateRPM = new TrapezoidProfile.State(totalRPM / inputs.length, 0); goalRPM = 0; } @@ -147,11 +158,4 @@ public boolean flyWheelsReady() { return false; return true; } - - public boolean flyWheelsRoughlyReady() { - for (FlyWheelIO.FlyWheelsInputs input:inputs) - if (Math.abs(input.flyWheelVelocityRevsPerSec * 60 - goalRPM) > 300) - return false; - return true; - } } diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java index 568a304..64ffe6f 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/shooter/Pitch.java @@ -37,11 +37,7 @@ public Pitch(PitchIO io) { notCalibratedAlert.setActivated(false); - setDefaultCommand(getPitchDefaultCommand()); - } - - public Command getPitchDefaultCommand() { - return Commands.run(() -> this.runSetPointProfiled(PITCH_LOWEST_ROTATION_RAD), this); + setDefaultCommand(Commands.run(() -> this.runSetPointProfiled(PITCH_LOWEST_ROTATION_RAD), this)); } @Override @@ -98,6 +94,9 @@ public void runStaticSetPoint(double setPointRad, double velocitySetPointRadPerS return; } + if (Math.abs(setPointRad - inputs.pitchAngleRad) < Math.toRadians(7)) + runSetPointProfiled(setPointRad); + this.setPointRad = setPointRad; this.currentState = new TrapezoidProfile.State(setPointRad, velocitySetPointRadPerSec); this.previousStateVelocity = velocitySetPointRadPerSec; @@ -136,8 +135,4 @@ private void runControlLoops(double currentPositionSetPointRad, double currentVe public boolean inPosition() { return Math.abs(inputs.pitchAngleRad - setPointRad) < PITCH_PID.errorTolerance; } - - public boolean roughlyInPosition() { - return Math.abs(inputs.pitchAngleRad - setPointRad) < Math.toRadians(7); - } } \ No newline at end of file From 5f94e4b08ebf7c74a0a589f55a6c665e0e3c6775 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 29 Sep 2024 17:34:15 +0800 Subject: [PATCH 8/8] removed redundant code --- src/main/java/frc/robot/subsystems/drive/SwerveDrive.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index fc8fcb2..04f21b9 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -249,10 +249,6 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix {