From 4df940729612a28feac27aff7cf5f42407242655 Mon Sep 17 00:00:00 2001 From: Isaac Thoman Date: Wed, 10 Jan 2024 15:36:23 -0500 Subject: [PATCH] 2023 import --- .gitignore | 2 + src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/New New New Path.path | 52 +++ .../deploy/pathplanner/paths/New Path.path | 55 +++ src/main/java/frc/robot/Constants.java | 186 +++++++++ src/main/java/frc/robot/FMSGetter.java | 18 + src/main/java/frc/robot/MBUtils.java | 111 ++++++ src/main/java/frc/robot/RobotContainer.java | 67 +++- src/main/java/frc/robot/commands/Autos.java | 32 +- src/main/java/frc/robot/commands/Balance.java | 53 +++ .../java/frc/robot/commands/TravelToPose.java | 122 ++++++ .../robot/subsystems/drive/SwerveModule.java | 190 +++++++++ .../subsystems/drive/SwerveSubsystem.java | 367 ++++++++++++++++++ .../subsystems/drive/VisionSubsystem.java | 63 +++ vendordeps/Phoenix5.json | 151 +++++++ 15 files changed, 1440 insertions(+), 30 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/New New New Path.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/java/frc/robot/FMSGetter.java create mode 100644 src/main/java/frc/robot/MBUtils.java create mode 100644 src/main/java/frc/robot/commands/Balance.java create mode 100644 src/main/java/frc/robot/commands/TravelToPose.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveModule.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/drive/VisionSubsystem.java create mode 100644 vendordeps/Phoenix5.json diff --git a/.gitignore b/.gitignore index 5528d4f..8e97aa0 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,5 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +.run/ diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path new file mode 100644 index 0000000..538c1bb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3421030350832635, + "y": 5.5555704093864335 + }, + "prevControl": null, + "nextControl": { + "x": 1.5506731013462027, + "y": 5.664389574393184 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.728651702917442, + "y": 6.761649488211257 + }, + "prevControl": { + "x": 6.347784625393813, + "y": 6.670966850705631 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -2.2457425658951253, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..5d79ab9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.323966507582138, + "y": 5.537433881885308 + }, + "prevControl": null, + "nextControl": { + "x": 1.1591350101928453, + "y": 6.472239756329568 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 5.479309367425445, + "y": 1.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 179.0122396003603, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..18c57f1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,14 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; + +import java.util.ArrayList; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -13,7 +21,185 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + + public static class AutoConstants { + public static final double translationkP = 4; + public static final double rotationkP = 4; + + public static final double maxTranslation = 6.5; + public static final double maxRotation = 2; + + } public static class OperatorConstants { public static final int kDriverControllerPort = 0; + + public static final double driveExponent = 1.8; + public static final double driveMaxSpeed = 5; //5 + public static final double turnExponent = 1.8; + public static final double turnMaxSpeed = 11; //11 + + + public static final double maxDrivePower = 1; + + public static final double controllerDeadband = 0.06; + + public static final double radFeedClamp = 0.5; //max heading adjustment speed + } + + public static final class DriveConstants{ + + /*Physical Characteristics*/ + public static final double TRACK_WIDTH = Units.inchesToMeters(23.625); //need to find + // Distance between right and left wheels + public static final double WHEEL_BASE = Units.inchesToMeters(23.625); //need to find + + + // Distance between front and back wheels + public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( + new Translation2d(WHEEL_BASE / 2, TRACK_WIDTH / 2), + new Translation2d(WHEEL_BASE / 2, -TRACK_WIDTH / 2), + new Translation2d(-WHEEL_BASE / 2, TRACK_WIDTH / 2), + new Translation2d(-WHEEL_BASE / 2, -TRACK_WIDTH / 2) + ); + + + public static final double wheelDiameterMeters = Units.inchesToMeters(3.8); + public static final double wheelCircumferenceMeters = wheelDiameterMeters * Math.PI; + public static final double driveMotorGearRatio = 6.75; + public static final double turningGearRatio = 21.4286; + public static final double falconTicks = 2048; + + public static final double radToFalcon = falconTicks / (2*Math.PI); + + public static final double radPerSecondDeadband = 0.01; + + + + + /*Motor IDs and offsets */ + public static final int frontLeftDriveID = 1; + public static final int frontLeftTurningID = 4; + public static final double frontLeftOffsetRad = 0.945; + + public static final int frontRightDriveID = 6; + public static final int frontRightTurningID = 5; + public static final double frontRightOffsetRad = -5.1929; + + public static final int rearRightDriveID = 3; + public static final int rearRightTurningID = 7; + public static final double rearRightOffsetRad = 2.361; + + public static final int rearLeftDriveID = 8; + public static final int rearLeftTurningID = 2; + public static final double rearLeftOffsetRad = -2.296; + + + + + + /*Drive Motor Constants */ + public static final double driveMotorkP = 0.1; + public static final double driveMotorkI = 0.0; + public static final double driveMotorkD = 0.0; + public static final double driveMotorkF = 0.045; + + public static final double driveNeutralDeadband = 0.01; + + + + /*Turning Motor Constants */ + + public static final double turningMotorkP = 0.2; + public static final double turningMotorkI = 0.0; + public static final double turningMotorkD = 0.01; + public static final double turningMotorkF = 0.0; + + + } + + + public static class ArmConstants { + + public static final double stageOnekP = 6; + public static final double stageOnekI = 0.0; + public static final double stageOnekD = 0.0; + public static final double stageOnekF = 0.0; + + public static final double stageTwokP = 0.6; //0.8 + public static final double stageTwokI = 0.001; //0.001 + public static final double stageTwokD = 3; //2 + public static final double stageTwokF = 0.0; + + + + public static final int stageOneLeftId = 11; + public static final int stageOneRightId = 13; + + public static final double continuousCurrentLimit = 20; + public static final double peakCurrentLimit = 40; + public static final double peakCurrentTime = 250; + + public static final int stageTwoPrimaryId = 49; + public static final int stageTwoSecondaryId = 48; + + public static final int stageTwoSmartCurrentLimit = 40; + public static final double stageTwoSecondaryCurrentLimit = 60; + + public static final double magEncoderCountsPerRotation = 4096;//4096 + public static final double radiansPerRotation = 2 * Math.PI; + + public static final double stageOneEncoderTicksToRadians = (radiansPerRotation/magEncoderCountsPerRotation); + + public static final double stageOneEncoderOffset = Units.degreesToRadians(291.9 + 90 - .145); + + public static final double stageOneLength = Units.inchesToMeters(28.75); + public static final double[] stageOnePivotCoordinate = {-4.864, 18.66}; + + public static final double stageTwoLength = Units.inchesToMeters(28.75); + public static final double stageTwoEncoderOffset = Units.degreesToRadians(43.6);//180 - 43.6 //43.6 + 180 + public static final double stageTwoEncoderRatio = 1;//32.0/22 + + + // public static final double[] affAnglesDegreesX = {-90,-73,-62,-54,-39,-30,-24,-17,-13,-11,-7,0, + // 7,11,13,17,24,30,39,54,62,73,90}; + // public static final double[] affPercentOutsY = {0.01,0.02,0.025,0.03,0.035,0.0375,0.04,0.042,0.043,0.046,0.047, + // 0.047,0.046,0.043,0.042,0.04,0.0375,0.035,0.03,0.025,0.02,0.01,0}; + } + + public static class EfConstants { //end effector motor ids + public static int EF_UPPER_PORT = 12; + public static int EF_LOWER_PORT = 14; + } + + + public static final class FieldConstants{ + + public static Pose2d[] alignmentPoses = new Pose2d[18]; + + public static final double[] nodeYValues = new double[] { + Units.inchesToMeters(20.19 + 22.0 * 0), + Units.inchesToMeters(20.19 + 22.0 * 1), + Units.inchesToMeters(20.19 + 22.0 * 2), + Units.inchesToMeters(20.19 + 22.0 * 3), + Units.inchesToMeters(20.19 + 22.0 * 4), + Units.inchesToMeters(20.19 + 22.0 * 5), + Units.inchesToMeters(20.19 + 22.0 * 6), + Units.inchesToMeters(20.19 + 22.0 * 7), + Units.inchesToMeters(20.19 + 22.0 * 8) + }; + + + + public static final double blueAlignmentX = Units.inchesToMeters(69.0625); + public static final double fieldLength = Units.inchesToMeters(651.25); + public static final double redAlignmentX = fieldLength - blueAlignmentX; + + static{ + for(int i = 0; i<9; i++){ + alignmentPoses[i] = new Pose2d(blueAlignmentX,nodeYValues[i], Rotation2d.fromDegrees(180)); + alignmentPoses[i+9] = new Pose2d(redAlignmentX,nodeYValues[i], Rotation2d.fromDegrees(0)); + } + } + } } diff --git a/src/main/java/frc/robot/FMSGetter.java b/src/main/java/frc/robot/FMSGetter.java new file mode 100644 index 0000000..ff98be6 --- /dev/null +++ b/src/main/java/frc/robot/FMSGetter.java @@ -0,0 +1,18 @@ +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + + +public class FMSGetter { + + static NetworkTableInstance inst = NetworkTableInstance.getDefault(); + static NetworkTable table = inst.getTable("FMSInfo"); + + public static boolean isRedAlliance(){ + if(table.getValue("IsRedAlliance").isBoolean()) + return table.getValue("IsRedAlliance").getBoolean(); + return false; + } + +} diff --git a/src/main/java/frc/robot/MBUtils.java b/src/main/java/frc/robot/MBUtils.java new file mode 100644 index 0000000..3395b8b --- /dev/null +++ b/src/main/java/frc/robot/MBUtils.java @@ -0,0 +1,111 @@ +package frc.robot; + +public class MBUtils { + + public static double lerp(double value1, double value2, double t) { //linear interpolation!! (for shrimp) + return (1 - t) * value1 + t * value2; + } + + public static double slerp(double a, double b, double t) { //spherical interpolation (yum!) + // Normalize angles + double delta = ((b - a) + Math.PI) % (2 * Math.PI) - Math.PI; + + // Make sure we interpolate in the short direction + if (delta > Math.PI) { + delta -= 2 * Math.PI; + } else if (delta < -Math.PI) { + delta += 2 * Math.PI; + } + + return a + delta * t; + } + + + public static double clamp(double input, double absMax){ + absMax = Math.abs(absMax); + + if(input>absMax) + input = absMax; + if(input<-absMax) + input = -absMax; + + return input; + } + + public static double angleDiffDeg(double ang1, double ang2){ + if(Math.abs(ang1-ang2)>180){ + if(ang1>ang2) + return -ang1+ang2+360; + else + return -ang1+ang2-360; + } + return ang2-ang1; + } + + public static double clamp(double input, double min, double max){ + if(input>max) input = max; + if(input= xValues[i] && x <= xValues[i + 1]) { + indexBelow = i; + indexAbove = i + 1; + break; + } + } + + // If indices found, interpolate. Otherwise, return -1 (or any other error value). + if (indexBelow != -1) { + double x1 = xValues[indexBelow]; + double x2 = xValues[indexAbove]; + double y1 = yValues[indexBelow]; + double y2 = yValues[indexAbove]; + + return y1 + ((x - x1) / (x2 - x1)) * (y2 - y1); + } else { + // You can throw an exception or return a default/error value. + // In this example, we return 0 to indicate an error. + return 0; + } + } + + +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..144d5b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,18 @@ package frc.robot; -import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.Autos; +import frc.robot.subsystems.drive.SwerveSubsystem; +import frc.robot.subsystems.drive.VisionSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -20,16 +25,42 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + CommandXboxController driver = new CommandXboxController(0); + + VisionSubsystem visionSub = new VisionSubsystem(); + SwerveSubsystem swerveSubsystem = new SwerveSubsystem(visionSub); + RunCommand drive = new RunCommand(()->swerveSubsystem.joystickDrive(driver.getLeftX(),driver.getLeftY(),driver.getRightX()),swerveSubsystem); + + + SendableChooser autoChooser = new SendableChooser<>(); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + + + // driver.rightTrigger(0.5).onTrue(ArmCommands.placeCubeL2orL3(arm)); +// driver.leftTrigger(0.5).onTrue(ArmCommands.retractCubeFromL2orL3(arm)); + + swerveSubsystem.setDefaultCommand(drive); + +// + driver.b().whileTrue(new RunCommand(swerveSubsystem::autoBalanceForward,swerveSubsystem)); + + driver.start().onTrue(new InstantCommand(swerveSubsystem::resetOdometry)); + + driver.back().onTrue(new InstantCommand(swerveSubsystem::toggleFieldOriented)); + + driver.x().whileTrue(new RunCommand(swerveSubsystem::xConfig,swerveSubsystem)); + + driver.a().onTrue(Autos.ballerAuto(swerveSubsystem)); + // Configure the trigger bindings configureBindings(); + createAutos(); } /** @@ -41,14 +72,18 @@ public RobotContainer() { * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight * joysticks}. */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + private void createAutos(){ + SmartDashboard.putData(autoChooser); + + autoChooser.setDefaultOption("no auto :'( ", null); + + + + } + + private void configureBindings() { + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } /** @@ -58,6 +93,6 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 107aad7..8c47623 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -1,20 +1,24 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.commands; -import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.drive.SwerveSubsystem; +import static frc.robot.Constants.FieldConstants.*; + +public class Autos { + + public static Command ballerAuto(SwerveSubsystem swerveSubsystem){ + Command resetPose = new InstantCommand(swerveSubsystem::resetOdometry); + + TravelToPose pose1 = new TravelToPose(swerveSubsystem, new Pose2d(0,2,Rotation2d.fromDegrees(90)),1.5,0); + -public final class Autos { - /** Example static factory for an autonomous command. */ - public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); - } + return resetPose.andThen(pose1); + // return resetPose.andThen(pose1); + } - private Autos() { - throw new UnsupportedOperationException("This is a utility class!"); - } } diff --git a/src/main/java/frc/robot/commands/Balance.java b/src/main/java/frc/robot/commands/Balance.java new file mode 100644 index 0000000..1691f08 --- /dev/null +++ b/src/main/java/frc/robot/commands/Balance.java @@ -0,0 +1,53 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.MBUtils; +import frc.robot.subsystems.drive.SwerveSubsystem; + + +public class Balance extends Command { + private final SwerveSubsystem swerveSubsystem; + + + double initTime; + double secondsToTake; + + public Balance(SwerveSubsystem swerveSubsystem, double secondsToTake) { + this.swerveSubsystem = swerveSubsystem; + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem); + + this.secondsToTake = secondsToTake; + + } + + + + @Override + public void initialize() { + initTime = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + + + + swerveSubsystem.autoBalanceForward(); + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return Timer.getFPGATimestamp() - initTime > secondsToTake; + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/robot/commands/TravelToPose.java b/src/main/java/frc/robot/commands/TravelToPose.java new file mode 100644 index 0000000..c7fcee4 --- /dev/null +++ b/src/main/java/frc/robot/commands/TravelToPose.java @@ -0,0 +1,122 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.MBUtils; +import frc.robot.subsystems.drive.SwerveSubsystem; + + +public class TravelToPose extends Command { + private final SwerveSubsystem swerveSubsystem; + + Pose2d initialPose; + Pose2d desiredPose; + + double initTime; + double secondsToTake; + double overtime; + + public TravelToPose(SwerveSubsystem swerveSubsystem, Pose2d desiredPose, double secondsToTake, double overtime) { + this.swerveSubsystem = swerveSubsystem; + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem); + + this.desiredPose = desiredPose; + this.secondsToTake = secondsToTake; + this.overtime = overtime; + } + + public TravelToPose (SwerveSubsystem swerveSubsystem, Pose2d desiredPose, double secondsToTake) { + this(swerveSubsystem,desiredPose,secondsToTake,0); + } + + @Override + public void initialize() { + initialPose = swerveSubsystem.getOdometry().getEstimatedPosition(); + initTime = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + if(Timer.getFPGATimestamp() - initTime > secondsToTake){ + // swerveSubsystem.driveToPose(desiredPose); + return; + } + double aggression = 2; + + double interpolatedX = MBUtils.easeInOut(initialPose.getX(),desiredPose.getX(),(Timer.getFPGATimestamp() - initTime) / secondsToTake, aggression); + double interpolatedY = MBUtils.easeInOut(initialPose.getY(),desiredPose.getY(),(Timer.getFPGATimestamp() - initTime) / secondsToTake,aggression); + double interpolatedAngle = MBUtils.easeInOutSlerp(initialPose.getRotation().getRadians(), desiredPose.getRotation().getRadians(), (Timer.getFPGATimestamp() - initTime) / secondsToTake,aggression); + + Pose2d pPose = new Pose2d(interpolatedX,interpolatedY, Rotation2d.fromRadians(interpolatedAngle)); + + + Translation2d transDiff = desiredPose.getTranslation().minus(initialPose.getTranslation()); + // double transSpeed = Math.hypot(transDiff.getX(), transDiff.getY()) / secondsToTake; + // double rotVelocity = desiredPose.getRotation().minus(initialPose.getRotation()).getRadians() / secondsToTake; + + /** new */ + double currentTime = (Timer.getFPGATimestamp() - initTime) / secondsToTake; + double deltaT = 0.001; // small time increment + double futureTime = currentTime + deltaT; + +// Calculate current and future positions + double currentX = MBUtils.easeInOut(initialPose.getX(), desiredPose.getX(), currentTime,aggression); + double futureX = MBUtils.easeInOut(initialPose.getX(), desiredPose.getX(), futureTime,aggression); + double currentY = MBUtils.easeInOut(initialPose.getY(), desiredPose.getY(), currentTime,aggression); + double futureY = MBUtils.easeInOut(initialPose.getY(), desiredPose.getY(), futureTime,aggression); + +// Calculate velocity + double velocityX = (futureX - currentX) / (deltaT * secondsToTake); + double velocityY = (futureY - currentY) / (deltaT * secondsToTake); + + double transSpeed = Math.hypot(velocityX,velocityY); + +// Calculate current and future angles + double currentAngle = MBUtils.easeInOutSlerp(initialPose.getRotation().getRadians(), desiredPose.getRotation().getRadians(), currentTime,aggression); + double futureAngle = MBUtils.easeInOutSlerp(initialPose.getRotation().getRadians(), desiredPose.getRotation().getRadians(), futureTime,aggression); + +// Calculate angular velocity + double rotVelocity = (futureAngle - currentAngle) / (deltaT * secondsToTake); + + +/**End new*/ + + double angleOfDiff = Math.atan2(transDiff.getY(),transDiff.getX()); //angle between two poses + + double angleOfTravel = angleOfDiff - interpolatedAngle; + + //SmartDashboard.putString() + + + SmartDashboard.putNumber("transSpeed",transSpeed); + + //swerveSubsystem.drive(0,0,0); + //swerveSubsystem.driveToPose(pPose); + //swerveSubsystem.driveToPose(pPose,0,0,Math.cos(angleOfTravel)*transSpeed, Math.sin(angleOfTravel)*transSpeed, rotVelocity); + + swerveSubsystem.driveToPose(pPose,0.2,0.2,Math.cos(angleOfTravel)*transSpeed, Math.sin(angleOfTravel)*transSpeed, rotVelocity); + + + + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return Timer.getFPGATimestamp() - initTime > secondsToTake + overtime; + } + + @Override + public void end(boolean interrupted) { + swerveSubsystem.drive(0,0,0); + + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java new file mode 100644 index 0000000..a08cf57 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -0,0 +1,190 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import static frc.robot.Constants.DriveConstants.*; +import static frc.robot.Constants.OperatorConstants.maxDrivePower; + +public class SwerveModule extends SubsystemBase { + + private WPI_TalonFX driveMotor; + private WPI_TalonFX turningMotor; + private DutyCycleEncoder absoluteEncoder; + private boolean absoluteEncoderReversed; + double absoluteEncoderOffset; + + + public SwerveModule(int driveMotorID, int turningMotorID, boolean driveMotorReversed, boolean turningMotorReversed, int absoluteEncoderID, double absoluteEncoderOffset){ + + /*Motor Creation */ + driveMotor = new WPI_TalonFX(driveMotorID); + turningMotor = new WPI_TalonFX(turningMotorID); + absoluteEncoder = new DutyCycleEncoder(absoluteEncoderID); + + driveMotor.configFactoryDefault(); + turningMotor.configFactoryDefault(); + + /*Encoder Configs */ + absoluteEncoder.setDistancePerRotation(1.0); + this.absoluteEncoderOffset = absoluteEncoderOffset; + + + + /*Drive Motor Configs */ + driveMotor.config_kP(0,driveMotorkP); + driveMotor.config_kI(0,driveMotorkI); + driveMotor.config_kD(0,driveMotorkD); + driveMotor.config_kF(0,driveMotorkF); + + driveMotor.configVoltageCompSaturation(12.0); + driveMotor.enableVoltageCompensation(true); + + driveMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + driveMotor.configIntegratedSensorInitializationStrategy(SensorInitializationStrategy.BootToZero); + + StatorCurrentLimitConfiguration driveStatorConfig = new StatorCurrentLimitConfiguration(false,40,40,0); + SupplyCurrentLimitConfiguration driveSupplyConfig = new SupplyCurrentLimitConfiguration(true,45,55,50); + + driveMotor.configStatorCurrentLimit(driveStatorConfig); + driveMotor.configSupplyCurrentLimit(driveSupplyConfig); + + driveMotor.configNeutralDeadband(driveNeutralDeadband); + + driveMotor.setNeutralMode(NeutralMode.Brake); + driveMotor.setInverted(driveMotorReversed); + + driveMotor.configPeakOutputForward(maxDrivePower,1000); + driveMotor.configPeakOutputReverse(-maxDrivePower,1000); + + /*Turning Motor Configs */ + turningMotor.config_kP(0,turningMotorkP); + turningMotor.config_kI(0,turningMotorkI); + turningMotor.config_kD(0,turningMotorkD); + turningMotor.config_kF(0,turningMotorkF); + + turningMotor.configVoltageCompSaturation(12.0); + turningMotor.enableVoltageCompensation(true); + + turningMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + turningMotor.configIntegratedSensorInitializationStrategy(SensorInitializationStrategy.BootToZero); + turningMotor.setInverted(true); + + StatorCurrentLimitConfiguration turningStatorConfig = new StatorCurrentLimitConfiguration(false,40,40,0); + SupplyCurrentLimitConfiguration turningSupplyConfig = new SupplyCurrentLimitConfiguration(true,20,30,50); + + turningMotor.configStatorCurrentLimit(turningStatorConfig); + turningMotor.configSupplyCurrentLimit(turningSupplyConfig); + + turningMotor.setNeutralMode(NeutralMode.Coast); + turningMotor.setInverted(turningMotorReversed); + + new Thread(()->{ + try{ + // for(int i = 0; i<10; i++){ + Thread.sleep(1000); + driveMotor.setSelectedSensorPosition(0); + turningMotor.setSelectedSensorPosition(radToTurning(getAbsoluteEncoderRad())); + //z } + } catch (InterruptedException e) {} + }).start(); + } + double getAbsoluteEncoderRad(){ + return absoluteEncoder.getAbsolutePosition()*2*Math.PI + absoluteEncoderOffset; + } + + static double turningToRad(double turning){ + turning/=falconTicks; + turning/=turningGearRatio; + turning*= 2*Math.PI; + return turning; + } + + static double radToTurning(double rad){ + rad *= falconTicks; + rad *= turningGearRatio; + rad /= 2*Math.PI; + return rad; + } + + public double getAngle(){ + return turningToRad(turningMotor.getSelectedSensorPosition()); + } + + public void setState(SwerveModuleState state){ + + if(state.speedMetersPerSecond<0.01){ + driveMotor.set(ControlMode.Velocity,0); + return; + } + + setStateWithoutDeadband(state); + + } + + public void setStateWithoutDeadband(SwerveModuleState state) { + + state = SwerveModuleState.optimize(state,Rotation2d.fromRadians(getAngle())); //minimize change in heading + + double delta = state.angle.getRadians() - getAngle(); //error + double deltaConverted = delta % Math.PI; //error converted to representative of the actual gap; error > pi indicates we aren't taking the shortest route to setpoint, but rather doing one or more 180* rotations.this is caused by the discontinuity of numbers(pi is the same location as -pi, yet -pi is less than pi) + double setAngle = Math.abs(deltaConverted) < (Math.PI / 2) ? getAngle() + deltaConverted : getAngle() - ((deltaConverted/Math.abs(deltaConverted)) * (Math.PI-Math.abs(deltaConverted))); //makes set angle +/- 1/2pi of our current position(capable of pointing all directions) + + + turningMotor.set(ControlMode.Position,radToTurning(setAngle)); + driveMotor.set(ControlMode.Velocity, driveMetersPerSecondToFalcon(state.speedMetersPerSecond)); + + + } + + public double driveMetersPerSecondToFalcon(double metersPerSecond){ //untested + metersPerSecond /= 10; //convert from 1000ms to 100ms + metersPerSecond = driveMetersToFalcon(metersPerSecond); + return metersPerSecond; + } + + public double driveMetersToFalcon(double meters){ + meters /= wheelCircumferenceMeters; //meters to rotations + meters *= Constants.DriveConstants.falconTicks; //rotations to sensor units + meters *= driveMotorGearRatio; //wheel rotations to motor rotations + + return meters; + } + + public double falconToDriveMeters(double falconTicksIn){ + falconTicksIn *= wheelCircumferenceMeters; + falconTicksIn /= Constants.DriveConstants.falconTicks; + falconTicksIn /= driveMotorGearRatio; + return falconTicksIn; + } + + public SwerveModulePosition getModulePosition(){ + return new SwerveModulePosition(falconToDriveMeters(driveMotor.getSelectedSensorPosition()),new Rotation2d(getAngle())); + } + + public SwerveModulePosition getPosition(){ + // SmartDashboard.putNumber("sensorPosDrive",falconToDriveMeters(driveMotor.getSelectedSensorPosition())); + return new SwerveModulePosition(falconToDriveMeters(driveMotor.getSelectedSensorPosition()),new Rotation2d(getAngle())); + } + + public double getModuleVelocity(){ + // return driveMotor.getSelectedSensorVelocity(); + return falconToDriveMeters(driveMotor.getSelectedSensorVelocity()) * 10; + } + + + + public void periodic(){ + + + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java new file mode 100644 index 0000000..04f6671 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java @@ -0,0 +1,367 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants; +import frc.robot.FMSGetter; +import frc.robot.MBUtils; +import frc.robot.Robot; +import org.photonvision.EstimatedRobotPose; + +import java.util.Optional; + +import static frc.robot.Constants.AutoConstants.*; +import static frc.robot.Constants.DriveConstants.*; +import static frc.robot.Constants.FieldConstants.alignmentPoses; +import static frc.robot.Constants.FieldConstants.nodeYValues; +import static frc.robot.Constants.OperatorConstants.*; + + +public class SwerveSubsystem extends SubsystemBase { + /** Creates a new SwerveSubsystem. */ + + SwerveModule frontLeft = new SwerveModule(frontLeftDriveID,frontLeftTurningID,false,true,0,frontLeftOffsetRad); + SwerveModule frontRight = new SwerveModule(frontRightDriveID,frontRightTurningID,false,true,1,frontRightOffsetRad); + + SwerveModule rearLeft = new SwerveModule(rearLeftDriveID,rearLeftTurningID,false,true,2,rearLeftOffsetRad); + SwerveModule rearRight = new SwerveModule(rearRightDriveID,rearRightTurningID,false,true,3,rearRightOffsetRad); + + WPI_Pigeon2 pigeon = new WPI_Pigeon2(40); + AHRS navx = new AHRS(SPI.Port.kMXP); + + SwerveDrivePoseEstimator odometry; +VisionSubsystem visionSubsystem; + + Field2d field2d = new Field2d(); + + Field2d visionField = new Field2d(); + + boolean beFieldOriented = true; + + + public SwerveSubsystem(VisionSubsystem visionSubsystem) { + pigeon.configFactoryDefault(); + pigeon.zeroGyroBiasNow(); + odometry = new SwerveDrivePoseEstimator(kinematics,pigeon.getRotation2d(),getPositions(),new Pose2d()); + odometry.setVisionMeasurementStdDevs(VecBuilder.fill(7, 7, Units.degreesToRadians(400))); + this.visionSubsystem = visionSubsystem; + + SmartDashboard.putNumber("debugGoTo_x",0); + SmartDashboard.putNumber("debugGoTo_y",0); + SmartDashboard.putNumber("debugGoTo_deg",0); + + } + + public SwerveModulePosition[] getPositions(){ + return new SwerveModulePosition[]{frontLeft.getPosition(),frontRight.getPosition(),rearLeft.getPosition(),rearRight.getPosition()}; + } + + + + public void joystickDrive(double joystickX, double joystickY, double rad){ + + if(Math.abs(rad)0) + rad-=controllerDeadband; + else if(rad<0) + rad+=controllerDeadband; + + + double hypot = Math.hypot(joystickX,joystickY); + + if(hypot0){ + rad = Math.pow(rad,turnExponent) * turnMaxSpeed; + }else{ + rad = -Math.pow(-rad,turnExponent) * turnMaxSpeed; + } + + +/* Angular adjustment stuff */ +// if(Math.abs(rad)>radPerSecondDeadband || lastStillHeading.getDegrees() == 0){ +// lastStillHeading = Rotation2d.fromDegrees(pigeon.getAngle()); +// } + + + + drive(-joystickY ,-joystickX ,-rad); +} + + + + +public void xConfig() { + + setStatesNoDeadband(new SwerveModuleState[]{ + new SwerveModuleState(0,Rotation2d.fromDegrees(45)), + new SwerveModuleState(0,Rotation2d.fromDegrees(-45)), + new SwerveModuleState(0,Rotation2d.fromDegrees(-45)), + new SwerveModuleState(0,Rotation2d.fromDegrees(45)) + + }); + +} + + +Rotation2d lastStillHeading = new Rotation2d(); +public void drive(double xMeters,double yMeters, double rad){ + if(Math.abs(rad)>radPerSecondDeadband || lastStillHeading.getDegrees() == 0){ + lastStillHeading = Rotation2d.fromDegrees(pigeon.getAngle()); + } + + double diffDeg = MBUtils.angleDiffDeg(pigeon.getAngle(),lastStillHeading.getDegrees()); + double radFeed = diffDeg * (1.0/25) ; //this was responsible for the slap + + radFeed = MBUtils.clamp(radFeed,radFeedClamp); + + if(Math.abs(diffDeg)>25) radFeed = 0; + if(!beFieldOriented) radFeed = 0; + + + + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xMeters,yMeters,rad - radFeed)); + + setStates(states); + + if(Robot.isSimulation()){ + if(lastSimDriveUpdateTime == 0) + lastSimDriveUpdateTime = Timer.getFPGATimestamp(); + + Rotation2d newAngle = Rotation2d.fromRadians(odometry.getEstimatedPosition().getRotation().getRadians() + rad * (Timer.getFPGATimestamp() - lastSimDriveUpdateTime)); + + double hypot = Math.hypot(xMeters,yMeters); + double angOfTranslation = Math.atan2(yMeters,xMeters); + + Pose2d newPose = new Pose2d( + odometry.getEstimatedPosition().getX() + hypot*Math.cos(angOfTranslation+newAngle.getRadians()) * (Timer.getFPGATimestamp() - lastSimDriveUpdateTime), + odometry.getEstimatedPosition().getY() + hypot*Math.sin(angOfTranslation+newAngle.getRadians()) *(Timer.getFPGATimestamp() - lastSimDriveUpdateTime), + newAngle + ); + + // odometry.addVisionMeasurement(newPose, Timer.getFPGATimestamp(), VecBuilder.fill(0, 0, Units.degreesToRadians(0))); //trust, bro + + resetOdometry(newPose); + lastSimDriveUpdateTime = Timer.getFPGATimestamp(); + } + +} +double lastSimDriveUpdateTime = 0; + + + void setStates(SwerveModuleState[] states){ + frontLeft.setState(states[0]); + frontRight.setState(states[1]); + rearLeft.setState(states[2]); + rearRight.setState(states[3]); + } + + void setStatesNoDeadband(SwerveModuleState[] states){ + frontLeft.setStateWithoutDeadband(states[0]); + frontRight.setStateWithoutDeadband(states[1]); + rearLeft.setStateWithoutDeadband(states[2]); + rearRight.setStateWithoutDeadband(states[3]); + } + + public void autoBalanceForward(){ + double roll = pigeon.getRoll(); //probably in degrees + double speedForward = roll*(1.0/100); + + if(roll>15) + speedForward = 0.5; + if(roll<-15) + speedForward = -0.5; + + + speedForward = MBUtils.clamp(speedForward, 0.5); + + drive(-speedForward,0,0); + } + +void updatePoseFromVision(){ + Optional result = visionSubsystem.getEstimatedGlobalPose(odometry.getEstimatedPosition()); + if(result.isPresent()){ + odometry.addVisionMeasurement(result.get().estimatedPose.toPose2d(), result.get().timestampSeconds); + SmartDashboard.putNumber("lastVisionX",result.get().estimatedPose.getX()); + SmartDashboard.putNumber("lastVisionY",result.get().estimatedPose.getY()); + visionField.setRobotPose(result.get().estimatedPose.toPose2d()); + SmartDashboard.putData("visionField",visionField); + + + SmartDashboard.putNumber("resultWasPresent",Timer.getFPGATimestamp()); + } + //add vision measurement if present while passing in current reference pose +} + + @Override + public void periodic() { + // This method will be called once per scheduler run + + SmartDashboard.putNumber("FLVel",frontLeft.getModuleVelocity()); + +odometry.updateWithTime(Timer.getFPGATimestamp(),pigeon.getRotation2d(),getPositions()); + +updatePoseFromVision(); + + + field2d.setRobotPose(odometry.getEstimatedPosition()); + //SmartDashboard.putData("field",field2d); + + SmartDashboard.putNumber("odom_x",odometry.getEstimatedPosition().getX()); + SmartDashboard.putNumber("odom_y",odometry.getEstimatedPosition().getY()); + SmartDashboard.putNumber("odom_deg",odometry.getEstimatedPosition().getRotation().getDegrees()); + + + SmartDashboard.putNumberArray("odometry",new double[]{ + odometry.getEstimatedPosition().getX(), + odometry.getEstimatedPosition().getY(), + odometry.getEstimatedPosition().getRotation().getRadians() + }); + + + + + SmartDashboard.putNumber("closestNodeY",getClosestNodeY()); + + } + + public double getClosestNodeY(){ + double closestY = nodeYValues[0]; + double smallestDist = Math.abs(nodeYValues[0] - odometry.getEstimatedPosition().getY()); + + for(int i = 0; i< nodeYValues.length; i++){ + double dist = Math.abs(nodeYValues[i] - odometry.getEstimatedPosition().getY()); + if(dist selectedPoseStrategy = new SendableChooser<>(); + + + AprilTagFieldLayout aprilTagFieldLayout; + PhotonCamera cam; + PhotonPoseEstimator photonPoseEstimator; + public VisionSubsystem() { + selectedPoseStrategy.addOption("LOWEST_AMBIGUITY", PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + selectedPoseStrategy.setDefaultOption("CLOSEST_TO_REFERENCE", PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + selectedPoseStrategy.addOption("CLOSEST_TO_LAST", PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_LAST_POSE); + selectedPoseStrategy.addOption("CLOSEST_TO_CAM_HEIGHT", PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); + + // SmartDashboard.putData("poseStrategy",selectedPoseStrategy); + + try{ + aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); + }catch (Exception e){ + e.printStackTrace(); + } + + cam = new PhotonCamera("Arducam_OV2311_USB_Camera"); + Transform3d robotToCam = new Transform3d(new Translation3d(Units.inchesToMeters(-9.5), Units.inchesToMeters(6.5), Units.inchesToMeters(26)), new Rotation3d(0,0,0)); //Cam mounted facing forward, half a meter forward of center, half a meter up from center. + photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_RIO, cam, robotToCam); + + //1st + is cam forward + //2nd + is cam to left + //3rd + is cam up + + + } + + public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + return photonPoseEstimator.update(); + + } + + public void periodic(){ + photonPoseEstimator.setMultiTagFallbackStrategy(selectedPoseStrategy.getSelected()); + + } + + +} + diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88a68dd --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file