diff --git a/MeepMeep/.gitignore b/MeepMeep/.gitignore new file mode 100644 index 0000000000..42afabfd2a --- /dev/null +++ b/MeepMeep/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/MeepMeep/build.gradle b/MeepMeep/build.gradle new file mode 100644 index 0000000000..49feefc4fe --- /dev/null +++ b/MeepMeep/build.gradle @@ -0,0 +1,17 @@ +plugins { + id 'java-library' +} + +java { + sourceCompatibility = JavaVersion.VERSION_1_8 + targetCompatibility = JavaVersion.VERSION_1_8 +} + +repositories { + maven { url = 'https://jitpack.io' } + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'com.github.NoahBres:MeepMeep:2.0.3' +} \ No newline at end of file diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepMeepTesting.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepMeepTesting.java new file mode 100644 index 0000000000..12be4350c2 --- /dev/null +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepMeepTesting.java @@ -0,0 +1,227 @@ +package com.example.meepmeep; + +import static java.lang.Thread.sleep; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.noahbres.meepmeep.MeepMeep; +import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder; +import com.noahbres.meepmeep.roadrunner.SampleMecanumDrive; +import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity; +import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequence; + +public class MeepMeepTesting { + public static void main(String[] args) { + MeepMeep meepMeep = new MeepMeep(800); + RoadRunnerBotEntity bot = new DefaultBotBuilder(meepMeep).build(); + //SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + Pose2d blueAud = new Pose2d(-40.25, 64.25, Math.toRadians(-90)); + Pose2d blueBack = new Pose2d(7.75, 64.25, Math.toRadians(-90)); + Pose2d redBack = new Pose2d(7.75, -64.25, Math.toRadians(90)); + Pose2d redAud = new Pose2d(-40.25, -64.25, Math.toRadians(90)); + int teamProp = 1; + + + + RoadRunnerBotEntity blueAudL = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(blueAud) + .lineToSplineHeading(new Pose2d(-40.6, 56, Math.toRadians(-67))) + //purple + .addDisplacementMarker(() ->{}) + .splineToSplineHeading(new Pose2d(-20, 10), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(50,10, Math.toRadians(0))) + .build() + ); + + RoadRunnerBotEntity blueAudC = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(blueAud) + //purple + .lineToLinearHeading(new Pose2d(-40.25,62, Math.toRadians(-90))) + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,59, Math.toRadians(-90))) + .lineToSplineHeading(new Pose2d(50,59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity blueAudR = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(blueAud) + //purple + .lineToLinearHeading(new Pose2d(-45,62, Math.toRadians(-90))) + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,59, Math.toRadians(-90))) + .lineToSplineHeading(new Pose2d(50,59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity blueBackL = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(blueBack) + //purple + .lineToLinearHeading(new Pose2d(20,59, Math.toRadians(-90))) + .addDisplacementMarker(() ->{}) + .lineToSplineHeading(new Pose2d(50,59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity blueBackC = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(blueBack) + //purple + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,59, Math.toRadians(-90))) + + .lineToSplineHeading(new Pose2d(50,59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity blueBackR = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(blueBack) + //purple + .lineToLinearHeading(new Pose2d(20,59, Math.toRadians(-120))) + .addDisplacementMarker(() ->{ + + }) + .lineToSplineHeading(new Pose2d(50,59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity redAudL = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redAud) + //purple + .lineToLinearHeading(new Pose2d(-50,-59, Math.toRadians(90))) + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,-59, Math.toRadians(90))) + + .lineToSplineHeading(new Pose2d(50,-59, Math.toRadians(-180))) + + .build() + ); + + + RoadRunnerBotEntity redAudC = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redAud) + //purple + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,-59, Math.toRadians(90))) + + .lineToSplineHeading(new Pose2d(50,-59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity redAudR = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redAud) + //purple + .lineToLinearHeading(new Pose2d(-50,-59, Math.toRadians(60))) + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,-59, Math.toRadians(90))) + + .lineToSplineHeading(new Pose2d(50,-59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity redAudRC = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redAud) + //purple + .lineToLinearHeading(new Pose2d(-50,-59, Math.toRadians(60))) + .addDisplacementMarker(() ->{}) + .splineToSplineHeading(new Pose2d(-20, -10), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(50,-10, Math.toRadians(0))) + + .build() + ); + + RoadRunnerBotEntity redBackL = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redBack) + //purple + .lineToLinearHeading(new Pose2d(20,-59, Math.toRadians(120))) + .addDisplacementMarker(() ->{}) + .lineToSplineHeading(new Pose2d(50,-59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity redBackC = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redBack) + //purple + .addDisplacementMarker(() ->{}) + .lineToLinearHeading(new Pose2d(20,-59, Math.toRadians(90))) + + .lineToSplineHeading(new Pose2d(50,-59, Math.toRadians(-180))) + + .build() + ); + + RoadRunnerBotEntity redBackR = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(redBack) + //purple + .lineToLinearHeading(new Pose2d(20,-59, Math.toRadians(90))) + .addDisplacementMarker(() ->{}) + .lineToSplineHeading(new Pose2d(50,-59, Math.toRadians(-180))) + + .build() + ); + + meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(blueAudL) + .addEntity(blueAudC) + .addEntity(blueAudR) + .addEntity(blueBackL) + .addEntity(blueBackC) + .addEntity(blueBackR) + .addEntity(redAudL) + .addEntity(redAudC) + .addEntity(redAudR) + .addEntity(redAudRC) + .addEntity(redBackL) + .addEntity(redBackC) + .addEntity(redBackR) + .start(); + } +} diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MyClass.java b/MeepMeep/src/main/java/com/example/meepmeep/MyClass.java new file mode 100644 index 0000000000..c4595f4e8e --- /dev/null +++ b/MeepMeep/src/main/java/com/example/meepmeep/MyClass.java @@ -0,0 +1,4 @@ +package com.example.meepmeep; + +public class MyClass { +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 87e3884471..309a214814 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -57,19 +57,19 @@ public static class Params { // TODO: fill in these values based on // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = - RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.LogoFacingDirection.DOWN; public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; // drive model parameters - public double inPerTick = 1; - public double lateralInPerTick = inPerTick; - public double trackWidthTicks = 0; + public double inPerTick = 0.00105377300784073847522660290274; + public double lateralInPerTick = 0.0007101582756186512; + public double trackWidthTicks = 12305.806378870477; // feedforward parameters (in tick units) - public double kS = 0; - public double kV = 0; - public double kA = 0; + public double kS = 1.033044880251241; + public double kV = 0.000232239131311119; + public double kA = 0.00005; // path profile parameters (in inches) public double maxWheelVel = 50; @@ -81,9 +81,9 @@ public static class Params { public double maxAngAccel = Math.PI; // path controller gains - public double axialGain = 0.0; - public double lateralGain = 0.0; - public double headingGain = 0.0; // shared with turn + public double axialGain = 10; + public double lateralGain = 12; + public double headingGain = 18; // shared with turn public double axialVelGain = 0.0; public double lateralVelGain = 0.0; @@ -134,7 +134,7 @@ public DriveLocalizer() { rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); // TODO: reverse encoders if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + lastLeftFrontPos = leftFront.getPositionAndVelocity().position; lastLeftBackPos = leftBack.getPositionAndVelocity().position; @@ -201,10 +201,10 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { // TODO: make sure your config has motors with these names (or change them) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); - rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, "FL"); + leftBack = hardwareMap.get(DcMotorEx.class, "BL"); + rightBack = hardwareMap.get(DcMotorEx.class, "BR"); + rightFront = hardwareMap.get(DcMotorEx.class, "FR"); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -213,6 +213,10 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { // TODO: reverse motor directions if needed // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + rightBack.setDirection(DcMotorSimple.Direction.FORWARD); + rightFront.setDirection(DcMotorSimple.Direction.FORWARD); + leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); // TODO: make sure your config has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html @@ -223,7 +227,7 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { voltageSensor = hardwareMap.voltageSensor.iterator().next(); - localizer = new DriveLocalizer(); + localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick); FlightRecorder.write("MECANUM_PARAMS", PARAMS); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java index b68efcc00c..cd0d83eb3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java @@ -17,9 +17,9 @@ @Config public final class ThreeDeadWheelLocalizer implements Localizer { public static class Params { - public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units) - public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units) - public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units) + public double par0YTicks = 5386.816506638707; // y position of the first parallel encoder (in tick units) + public double par1YTicks = -5976.810420289613; // y position of the second parallel encoder (in tick units) + public double perpXTicks = -1541.838861326853; // x position of the perpendicular encoder (in tick units) } public static Params PARAMS = new Params(); @@ -34,12 +34,14 @@ public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) { // TODO: make sure your config has **motors** with these names (or change them) // the encoders should be plugged into the slot matching the named motor // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html - par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0"))); - par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1"))); - perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"))); + par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "BR"))); + par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "FL"))); + perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "FR"))); // TODO: reverse encoder directions if needed // par0.setDirection(DcMotorSimple.Direction.REVERSE); + perp.setDirection(DcMotorSimple.Direction.REVERSE); + par0.setDirection((DcMotorSimple.Direction.REVERSE)); lastPar0Pos = par0.getPositionAndVelocity().position; lastPar1Pos = par1.getPositionAndVelocity().position; diff --git a/build.gradle b/build.gradle index 8969a41590..b26ca8e63a 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:7.4.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fceae..8049c684f0 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.5-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/settings.gradle b/settings.gradle index 9e2cfb3b4b..a56e34042d 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,2 +1,3 @@ include ':FtcRobotController' include ':TeamCode' +include ':MeepMeep'