Merge branch 'master' of https://github.com/IF-Robotics/CS-States

MeepMeep/.gitignore

@@ -0,0 +1 @@
+/build

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'
+}

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();
+    }
+}

MeepMeep/src/main/java/com/example/meepmeep/MyClass.java

@@ -0,0 +1,4 @@
+package com.example.meepmeep;
+
+public class MyClass {
+}

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);
     }

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;

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'
     }
 }
 

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

settings.gradle

@@ -1,2 +1,3 @@
 include ':FtcRobotController'
 include ':TeamCode'
+include ':MeepMeep'