diff --git a/build.gradle b/build.gradle index 6f266d1..08379d0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2021.1.2" + id "edu.wpi.first.GradleRIO" version "2021.2.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path b/src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path new file mode 100644 index 0000000..7c9bce3 --- /dev/null +++ b/src/main/PathWeaverProject/PathWeaver/Paths/SixForward.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +0.0,0.0,10.0,0.0,true,false, +6.0,0.0,0.0,0.0,true,false, diff --git a/src/main/PathWeaverProject/PathWeaver/pathweaver.json b/src/main/PathWeaverProject/PathWeaver/pathweaver.json new file mode 100644 index 0000000..2c1efaa --- /dev/null +++ b/src/main/PathWeaverProject/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Meter", + "exportUnit": "Same as Project", + "maxVelocity": 3.6576, + "maxAcceleration": 3.048, + "wheelBase": 0.6604, + "gameName": "Infinite Recharge", + "outputDir": "../../deploy" +} \ No newline at end of file diff --git a/src/main/deploy/output/SixForward.wpilib.json b/src/main/deploy/output/SixForward.wpilib.json new file mode 100644 index 0000000..6d30a8a --- /dev/null +++ b/src/main/deploy/output/SixForward.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":9.999999999999998,"pose":{"translation":{"x":0.0,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.12499997033737249,"velocity":1.2499997033737247,"acceleration":10.000000000000002,"pose":{"translation":{"x":0.07812496292172,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1767763612825469,"velocity":1.7677636128254688,"acceleration":10.0,"pose":{"translation":{"x":0.1562494095414877,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.21650497682420286,"velocity":2.1650497682420284,"acceleration":10.0,"pose":{"translation":{"x":0.23437202494824305,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24999625680028503,"velocity":2.49996256800285,"acceleration":10.000000000000005,"pose":{"translation":{"x":0.31249064207077026,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.27950036230118047,"velocity":2.7950036230118047,"acceleration":9.999999999999995,"pose":{"translation":{"x":0.3906022626324557,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.30617089283798543,"velocity":3.061708928379854,"acceleration":10.0,"pose":{"translation":{"x":0.4687030781060457,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3306927548853784,"velocity":3.306927548853784,"acceleration":10.0,"pose":{"translation":{"x":0.5467884906684048,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3535118482187757,"velocity":3.5351184821877566,"acceleration":10.00000000000001,"pose":{"translation":{"x":0.6248531341552734,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3749375668070688,"velocity":3.749375668070688,"acceleration":10.0,"pose":{"translation":{"x":0.7028908950160258,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.39519487174517537,"velocity":3.9519487174517534,"acceleration":9.99999999999999,"pose":{"translation":{"x":0.7808949332684278,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4144533033897536,"velocity":4.1445330338975355,"acceleration":10.0,"pose":{"translation":{"x":0.8588577034533955,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43284430817321656,"velocity":4.328443081732165,"acceleration":10.000000000000023,"pose":{"translation":{"x":0.9367709755897522,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45047216476248275,"velocity":4.5047216476248275,"acceleration":10.000000000000023,"pose":{"translation":{"x":1.014625856128987,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.46742118242758585,"velocity":4.674211824275859,"acceleration":10.0,"pose":{"translation":{"x":1.0924128089100122,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.48376061768480544,"velocity":4.8376061768480545,"acceleration":10.0,"pose":{"translation":{"x":1.1701216761139221,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.49954813566237044,"velocity":4.995481356623705,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.24774169921875,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5148323105544613,"velocity":5.1483231055446135,"acceleration":10.0,"pose":{"translation":{"x":1.3252615399542265,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5296544725113793,"velocity":5.296544725113794,"acceleration":10.0,"pose":{"translation":{"x":1.4026693012565374,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5440500984694483,"velocity":5.440500984694483,"acceleration":10.0,"pose":{"translation":{"x":1.479952548223082,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5580498775319693,"velocity":5.580498775319694,"acceleration":9.999999999999977,"pose":{"translation":{"x":1.5570983290672302,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5716805394751655,"velocity":5.716805394751655,"acceleration":10.0,"pose":{"translation":{"x":1.6340931960730813,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5849655077951557,"velocity":5.849655077951557,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.7109232265502214,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5979254207321313,"velocity":5.979254207321314,"acceleration":10.0,"pose":{"translation":{"x":1.7875740437884815,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6105785515415186,"velocity":6.105785515415188,"acceleration":10.000000000000046,"pose":{"translation":{"x":1.8640308380126953,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6229411508862546,"velocity":6.2294115088625475,"acceleration":10.0,"pose":{"translation":{"x":1.9402783873374574,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6350277283271779,"velocity":6.350277283271781,"acceleration":10.0,"pose":{"translation":{"x":2.016301078721881,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6468512856792286,"velocity":6.468512856792288,"acceleration":10.000000000000046,"pose":{"translation":{"x":2.0920829289243557,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.658423511952194,"velocity":6.584235119521943,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.167607605457306,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6697549473564114,"velocity":6.697549473564117,"acceleration":10.0,"pose":{"translation":{"x":2.2428584475419484,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6808551221901835,"velocity":6.808551221901839,"acceleration":10.0,"pose":{"translation":{"x":2.3178184870630503,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6917326751749819,"velocity":6.9173267517498225,"acceleration":9.999999999999952,"pose":{"translation":{"x":2.392470469523687,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7023954548543148,"velocity":7.02395454854315,"acceleration":10.0,"pose":{"translation":{"x":2.466796875,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7128506069431314,"velocity":7.1285060694313165,"acceleration":10.0,"pose":{"translation":{"x":2.5407799390959553,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7231046499502128,"velocity":7.231046499502131,"acceleration":10.000000000000048,"pose":{"translation":{"x":2.614401673898101,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7331635409552664,"velocity":7.331635409552667,"acceleration":10.0,"pose":{"translation":{"x":2.6876438889303245,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7430327330755504,"velocity":7.430327330755507,"acceleration":9.99999999999995,"pose":{"translation":{"x":2.760488212108612,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7527172258817788,"velocity":7.5271722588177905,"acceleration":10.00000000000005,"pose":{"translation":{"x":2.8329161106958054,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7622216098033903,"velocity":7.622216098033905,"acceleration":10.0,"pose":{"translation":{"x":2.90490891225636,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7715501053866951,"velocity":7.715501053866954,"acceleration":-3.3717588560186713,"pose":{"translation":{"x":2.9764478256111033,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7807795440416799,"velocity":7.684381612345928,"acceleration":-10.0,"pose":{"translation":{"x":3.047513961791992,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7900192272742247,"velocity":7.59198478002048,"acceleration":-10.0,"pose":{"translation":{"x":3.118088354996871,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7993046414541025,"velocity":7.499130638221702,"acceleration":-10.0,"pose":{"translation":{"x":3.1881519835442305,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8086349335117604,"velocity":7.405827717645123,"acceleration":-10.0,"pose":{"translation":{"x":3.2576857908279635,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8180092132949669,"velocity":7.312084919813058,"acceleration":-10.0,"pose":{"translation":{"x":3.3266707062721252,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8274265535274705,"velocity":7.217911517488022,"acceleration":-10.000000000000052,"pose":{"translation":{"x":3.39508766628569,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.836885989741622,"velocity":7.123317155346507,"acceleration":-10.0,"pose":{"translation":{"x":3.462917635217309,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8463865201841579,"velocity":7.028311850921148,"acceleration":-9.999999999999947,"pose":{"translation":{"x":3.530141626310069,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8559271056942862,"velocity":6.9329059958198656,"acceleration":-10.0,"pose":{"translation":{"x":3.59674072265625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8655066695531509,"velocity":6.837110357231218,"acceleration":-10.0,"pose":{"translation":{"x":3.6626960981520824,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8751240973036861,"velocity":6.7409360797258655,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.727989038452506,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8847782365397934,"velocity":6.644394687364793,"acceleration":-10.000000000000055,"pose":{"translation":{"x":3.7926009619259275,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944678966636967,"velocity":6.547498086125758,"acceleration":-10.0,"pose":{"translation":{"x":3.8565134406089783,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9139488245368117,"velocity":6.352688807394609,"acceleration":-10.0,"pose":{"translation":{"x":3.9821672458201647,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9335565809593984,"velocity":6.156611243168741,"acceleration":-10.0,"pose":{"translation":{"x":4.104806900024414,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9532802335489408,"velocity":5.959374717273318,"acceleration":-10.00000000000003,"pose":{"translation":{"x":4.22429264895618,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9731082033060778,"velocity":5.761095019701946,"acceleration":-10.000000000000048,"pose":{"translation":{"x":4.340489208698273,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9930282476692652,"velocity":5.561894576070071,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.4532664362341166,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.013027440538092,"velocity":5.361902647381805,"acceleration":-10.000000000000016,"pose":{"translation":{"x":4.5625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0330921489311773,"velocity":5.16125556345095,"acceleration":-10.000000000000018,"pose":{"translation":{"x":4.668072050437331,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.053208005884192,"velocity":4.960096993920802,"acceleration":-9.999999999999982,"pose":{"translation":{"x":4.769871890544891,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0733598791204104,"velocity":4.758578261558617,"acceleration":-10.0,"pose":{"translation":{"x":4.8677966464310884,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0935318349371121,"velocity":4.556858703391601,"acceleration":-10.0,"pose":{"translation":{"x":4.961751937866211,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1137070966419202,"velocity":4.35510608634352,"acceleration":-10.000000000000021,"pose":{"translation":{"x":5.0516525488346815,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.133867996738349,"velocity":4.153497085379233,"acceleration":-9.999999999999979,"pose":{"translation":{"x":5.137423098087311,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1539959218921958,"velocity":3.952217833840764,"acceleration":-10.0,"pose":{"translation":{"x":5.218998709693551,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1740712495003311,"velocity":3.7514645577594097,"acceleration":-10.000000000000012,"pose":{"translation":{"x":5.29632568359375,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.194073274417897,"velocity":3.5514443085837506,"acceleration":-10.0,"pose":{"translation":{"x":5.369362166151404,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.213980124061222,"velocity":3.3523758121505,"acceleration":-10.000000000000007,"pose":{"translation":{"x":5.438078820705414,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2534143609235189,"velocity":2.958033443527531,"acceleration":-10.000000000000004,"pose":{"translation":{"x":5.562501907348633,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2921714334605936,"velocity":2.5704627181567834,"acceleration":-9.999999999999995,"pose":{"translation":{"x":5.669636070728302,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3300217313168947,"velocity":2.1919597395937727,"acceleration":-10.0,"pose":{"translation":{"x":5.759765625,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3667016944400214,"velocity":1.8251601083625066,"acceleration":-10.0,"pose":{"translation":{"x":5.833439528942108,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.401903648667902,"velocity":1.4731405660837007,"acceleration":-10.0,"pose":{"translation":{"x":5.89149284362793,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4663168090582577,"velocity":0.8290089621801443,"acceleration":-9.999999999999998,"pose":{"translation":{"x":5.96563720703125,"y":8.21055},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5492177052762721,"velocity":0.0,"acceleration":-9.999999999999998,"pose":{"translation":{"x":6.0,"y":8.21055},"rotation":{"radians":0.0}},"curvature":"NaN"}] \ 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 8f6cec9..4d40912 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -77,6 +77,14 @@ public final class Constants { public static final double kHopperSpinPercentage = 1.0; //Todo: set limit + public static final int kNeoEncoderResolution = 42; // Neo encoder has 42 counts per revolution + public static final double kDriveBaseWheelDiameter = 6.0; // in inches + public static final double kGearboxRatio = 9.7; // 9.7 motor rotations : 1 wheel rotation Reduction; slows the rotation down + public static final double kInchesToMeters = 0.0254; + public static final double kPositionConversionFactor = kDriveBaseWheelDiameter * Math.PI / kGearboxRatio * kInchesToMeters; // in meters + + public static final double kWheelBaseTrackWidthInches = 26; + public static final double kWheelBaseTrackWidthMeters = kWheelBaseTrackWidthInches * kInchesToMeters; public static final int kHopperCurrentLimit = 40; //Todo: may change public static final int kIntakeDoubleSolenoidForwardChannel = 0; public static final int kIntakeDoubleSolenoidReverseChannel = 2; diff --git a/src/main/java/frc/robot/MotorController.java b/src/main/java/frc/robot/MotorController.java index 3f0df32..ef37eb1 100644 --- a/src/main/java/frc/robot/MotorController.java +++ b/src/main/java/frc/robot/MotorController.java @@ -37,20 +37,24 @@ public MotorController(String name, int deviceID) { mSparkMax.restoreFactoryDefaults(); } - //Todo: we must add constants for PID values once we have found the right values - public MotorController(String name, int deviceID, int smartCurrentLimit, boolean... enablePid) { - this(name, deviceID); //calls the constructor for correct arguements + public MotorController(String name, int deviceID, int smartCurrentLimit) { + this(name, deviceID); //Current limiting is required to prevent brown outs mSparkMax.setSmartCurrentLimit(smartCurrentLimit); - //If enablePid has any number of booleans greater than 0 we are enabling pid - if (enablePid.length > 0) - { - mP = SmartDashboard.getNumber(mName + " P Value", 1.0); - mI = SmartDashboard.getNumber(mName + " I Value", 0.0); - mD = SmartDashboard.getNumber(mName + " D Value", 0.0); - mPIDController = mSparkMax.getPIDController(); - setPID(); - } + } + + public MotorController(String name, int deviceID, int smartCurrentLimit, boolean enablePid) { + this(name, deviceID, smartCurrentLimit, SmartDashboard.getNumber(name + " P Value", 1.0), + SmartDashboard.getNumber(name + " I Value", 0.0), SmartDashboard.getNumber(name + " D Value", 0.0)); //calls the constructor for correct arguments + } + + public MotorController(String name, int deviceID, int smartCurrentLimit, double P, double I, double D) { + this(name, deviceID, smartCurrentLimit); //calls the constructor for correct arguements + mP = P; + mI = I; + mD = D; + mPIDController = mSparkMax.getPIDController(); + setPID(); mSparkMax.setOpenLoopRampRate(.1); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fee8f76..66135bd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.XboxController; import frc.robot.commands.ShootCommandGroup; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.utilities.path; import frc.robot.commands.DriveBaseTeleopCommand; import frc.robot.commands.IntakeSpinMotorBackwardCommand; import frc.robot.commands.IntakeSpinMotorForwardCommand; @@ -51,6 +52,9 @@ public class RobotContainer { private final ShootCommandGroup mShootCommandGroup = new ShootCommandGroup(mShooterSubsystem, mDriveBaseSubsystem, mHopperSubsystem); private final StartEndCommand mIntakeExtendCommand = new StartEndCommand(mIntakeSubsystem::setIntakeExtended, mIntakeSubsystem::setIntakeRetracted); + //Paths + private final path mSixFeetForward = new path("output/SixForward.wpilib.json", mDriveBaseSubsystem); + public RobotContainer() { // Configure the button bindings for (int i = 1; i < mButtons.length; i++) { @@ -76,6 +80,7 @@ private void configureButtonBindings() { mButtons[Constants.kAButton].whenPressed(mSwitchDriveModeCommand); //The buttons below are just for testing functionality mButtons[Constants.kRightTriggerButton].whileHeld(mCycleHopperCommand); + mButtons[Constants.kLeftTriggerButton].whileHeld(mSixFeetForward.getPathCommand()); } diff --git a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java index 1cfd844..8d3f128 100644 --- a/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveBaseSubsystem.java @@ -9,13 +9,21 @@ import frc.robot.Constants; import frc.robot.MotorController; - +import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class DriveBaseSubsystem extends SubsystemBase { +import java.util.function.*; + +import com.revrobotics.ControlType; + +public class DriveBaseSubsystem extends SubsystemBase implements BiConsumer, Supplier { /** * Creates a new DriveBaseSubsystem. */ @@ -24,13 +32,16 @@ public class DriveBaseSubsystem extends SubsystemBase { private final MotorController[] mMotorControllers = new MotorController[6]; private final DifferentialDrive mDiffDrive; private boolean mIsArcadeDrive = true; + private final DifferentialDriveOdometry mOdometry; + private final Gyro mGyro = new ADXRS450_Gyro(); + private final DifferentialDriveKinematics mDifferentialDriveKinematics = new DifferentialDriveKinematics(Constants.kWheelBaseTrackWidthMeters); public DriveBaseSubsystem(Joystick joystick) { mDriverJoystick = joystick; - mMotorControllers[Constants.kDriveLeftFrontIndex] = new MotorController("Differential Left Front", Constants.kDriveLeftFront, Constants.kDriveBaseCurrentLimit); + mMotorControllers[Constants.kDriveLeftFrontIndex] = new MotorController("Differential Left Front", Constants.kDriveLeftFront, Constants.kDriveBaseCurrentLimit, true); mMotorControllers[Constants.kDriveLeftMiddleIndex] = new MotorController("Differential Left Middle", Constants.kDriveLeftMiddle, Constants.kDriveBaseCurrentLimit); mMotorControllers[Constants.kDriveLeftRearIndex] = new MotorController("Differential Left Rear", Constants.kDriveLeftRear, Constants.kDriveBaseCurrentLimit); - mMotorControllers[Constants.kDriveRightFrontIndex] = new MotorController("Differential Right Front", Constants.kDriveRightFront, Constants.kDriveBaseCurrentLimit); + mMotorControllers[Constants.kDriveRightFrontIndex] = new MotorController("Differential Right Front", Constants.kDriveRightFront, Constants.kDriveBaseCurrentLimit, true); mMotorControllers[Constants.kDriveRightMiddleIndex] = new MotorController("Differential Right Middle", Constants.kDriveRightMiddle, Constants.kDriveBaseCurrentLimit); mMotorControllers[Constants.kDriveRightRearIndex] = new MotorController("Differential Right Rear", Constants.kDriveRightRear, Constants.kDriveBaseCurrentLimit); @@ -39,6 +50,9 @@ public DriveBaseSubsystem(Joystick joystick) { mMotorControllers[Constants.kDriveRightRearIndex].getSparkMax().follow(mMotorControllers[Constants.kDriveRightFrontIndex].getSparkMax()); mMotorControllers[Constants.kDriveRightMiddleIndex].getSparkMax().follow(mMotorControllers[Constants.kDriveRightFrontIndex].getSparkMax()); mDiffDrive = new DifferentialDrive(mMotorControllers[Constants.kDriveLeftFrontIndex].getSparkMax(), mMotorControllers[Constants.kDriveRightFrontIndex].getSparkMax()); + mOdometry = new DifferentialDriveOdometry(mGyro.getRotation2d()); + mMotorControllers[Constants.kDriveLeftFrontIndex].getEncoder().setPositionConversionFactor(Constants.kPositionConversionFactor); + mMotorControllers[Constants.kDriveRightFrontIndex].getEncoder().setPositionConversionFactor(Constants.kPositionConversionFactor); } public void arcadeDrive() { @@ -75,6 +89,14 @@ public void stopMotors() { mDiffDrive.arcadeDrive(0.0, 0.0); } + public Pose2d getPose() { + return mOdometry.getPoseMeters(); + } + + public DifferentialDriveKinematics getDifferentialDriveKinematics() { + return mDifferentialDriveKinematics; + } + @Override public void periodic() { @@ -82,6 +104,21 @@ public void periodic() mMotorControllers[i].updateSmartDashboard(); } SmartDashboard.putBoolean("Arcade Drive", mIsArcadeDrive); + mOdometry.update(mGyro.getRotation2d(), mMotorControllers[Constants.kDriveLeftFrontIndex].getEncoder().getPosition(), + mMotorControllers[Constants.kDriveRightFrontIndex].getEncoder().getPosition()); + } + + @Override + public void accept(Double arg0, Double arg1) { + //arg0 is left drivebase velocity in wheel mps, and arg1 is drivebase right velocity in wheel mps + //We do arg divided by the conversionFactorConstant to convert wheel speed mps to motor rpm + mMotorControllers[Constants.kDriveLeftFrontIndex].getPID().setReference(arg0 / Constants.kPositionConversionFactor, ControlType.kVelocity); + mMotorControllers[Constants.kDriveRightFrontIndex].getPID().setReference(arg1 / Constants.kPositionConversionFactor, ControlType.kVelocity); + } + + @Override + public Pose2d get() { + return getPose(); } } diff --git a/src/main/java/frc/robot/utilities/path.java b/src/main/java/frc/robot/utilities/path.java new file mode 100644 index 0000000..b20e7bf --- /dev/null +++ b/src/main/java/frc/robot/utilities/path.java @@ -0,0 +1,38 @@ +package frc.robot.utilities; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj.DriverStation; +import java.io.*; +import java.nio.file.Path; +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import frc.robot.subsystems.DriveBaseSubsystem; + +public class path { + private String mPathName; + private RamseteCommand mPathCommand; + public Trajectory mTrajectory = new Trajectory(); + + private final DriveBaseSubsystem mDriveBaseSubsystem; + + public path(String pathName, DriveBaseSubsystem driveBaseSubsystem) { + mPathName = pathName; + mDriveBaseSubsystem = driveBaseSubsystem; + try { + Path path = Filesystem.getDeployDirectory().toPath().resolve(mPathName); + mTrajectory = TrajectoryUtil.fromPathweaverJson(path); + } catch (IOException ex) { + DriverStation.reportError("Could not find file: " + mPathName, ex.getStackTrace()); + } + mPathCommand = new RamseteCommand(mTrajectory, mDriveBaseSubsystem, + new RamseteController() /* b = 2.0, zeta = 0.7 */, + mDriveBaseSubsystem.getDifferentialDriveKinematics(), mDriveBaseSubsystem, mDriveBaseSubsystem); + + } + + public RamseteCommand getPathCommand() { + return mPathCommand; + } +}