Pedro pathing tuning

TeamCode/build.gradle

@@ -8,12 +8,23 @@
 // please think carefully as to whether such customizations are really necessary
 // before doing so.
 
+buildscript {
+    repositories {
+        mavenCentral()
+//        maven { url "https://repo.dairy.foundation/releases" }
+    }
+    dependencies {
+//        classpath "dev.frozenmilk:Load:0.2.4"
+    }
+}
 
 // Custom definitions may go here
 
 // Include common definitions from above.
 apply from: '../build.common.gradle'
 apply from: '../build.dependencies.gradle'
+//apply plugin: 'dev.frozenmilk.sinister.sloth.load'
+
 
 android {
     namespace = 'org.firstinspires.ftc.teamcode'
@@ -23,6 +34,14 @@ android {
     }
 }
 
+repositories {
+//    maven { url = "https://repo.dairy.foundation/releases" }
+//    maven { url = "https://repo.dairy.foundation/snapshots" }
+}
+
 dependencies {
     implementation project(':FtcRobotController')
-}
+//    implementation("dev.frozenmilk.sinister:Sloth:0.2.4")
+//    implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12")
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SampleAutoPathing.java

@@ -0,0 +1,149 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.TelemetryManager;
+import com.bylazar.telemetry.PanelsTelemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.paths.PathChain;
+import com.pedropathing.geometry.Pose;
+
+
+
+@Autonomous(name = "Pedro Pathing Autonomous", group = "Autonomous")
+@Configurable // Panels
+public class SampleAutoPathing extends OpMode {
+    private TelemetryManager panelsTelemetry; // Panels Telemetry instance
+    public Follower follower; // Pedro Pathing follower instance
+    private int pathState; // Current autonomous path state (state machine)
+    private Paths paths; // Paths defined in the Paths class
+
+
+
+    @Override
+    public void init() {
+        panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry();
+
+
+
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(72, 8, Math.toRadians(90)));
+
+
+
+        paths = new Paths(follower); // Build paths
+
+
+
+        panelsTelemetry.debug("Status", "Initialized");
+        panelsTelemetry.update(telemetry);
+    }
+
+
+
+    @Override
+    public void loop() {
+        follower.update(); // Update Pedro Pathing
+        pathState = autonomousPathUpdate(); // Update autonomous state machine
+
+
+
+// Log values to Panels and Driver Station
+        panelsTelemetry.debug("Path State", pathState);
+        panelsTelemetry.debug("X", follower.getPose().getX());
+        panelsTelemetry.debug("Y", follower.getPose().getY());
+        panelsTelemetry.debug("Heading", follower.getPose().getHeading());
+        panelsTelemetry.update(telemetry);
+    }
+
+
+
+    public static class Paths {
+        public PathChain MainChain;
+
+
+
+        public Paths(Follower follower) {
+            MainChain = follower.pathBuilder()
+                    .addPath(
+                            new BezierLine(
+                                    new Pose(20.509, 120.639),
+                                    new Pose(56.832, 83.276)
+                            )
+                    )
+                    .setLinearHeadingInterpolation(Math.toRadians(142), Math.toRadians(142))
+                    .addPath(
+                            new BezierCurve(
+                                    new Pose(12.062, 81.884),
+                                    new Pose(33.628, 83.185),
+                                    new Pose(63.194, 82.485)
+                            )
+                    )
+                    .setTangentHeadingInterpolation()
+                    .setReversed()
+                    .addPath(
+                            new BezierCurve(
+                                    new Pose(63.194, 82.485),
+                                    new Pose(58.818, 52.194),
+                                    new Pose(10.863, 58.585)
+                            )
+                    )
+                    .setTangentHeadingInterpolation()
+                    .addPath(
+                            new BezierLine(
+                                    new Pose(10.863, 58.585),
+                                    new Pose(60.478, 84.228)
+                            )
+                    )
+                    .setTangentHeadingInterpolation()
+                    .setReversed()
+                    .addPath(
+                            new BezierCurve(
+                                    new Pose(60.478, 84.228),
+                                    new Pose(77.501, 27.500),
+                                    new Pose(10.902, 34.795)
+                            )
+                    )
+                    .setTangentHeadingInterpolation()
+                    .addPath(
+                            new BezierLine(
+                                    new Pose(10.902, 34.795),
+                                    new Pose(59.650, 84.040)
+                            )
+                    )
+                    .setTangentHeadingInterpolation()
+                    .setReversed()
+                    .addPath(
+                            new BezierLine(
+                                    new Pose(59.650, 84.040),
+                                    new Pose(59.830, 115.365)
+                            )
+                    )
+                    .setTangentHeadingInterpolation()
+                    .build();
+        }
+    }
+
+
+
+    public int autonomousPathUpdate() {
+
+        switch (pathState) {
+
+            case 0:
+                follower.followPath(paths.MainChain);
+                return 1;
+
+            case 1:
+                if (!follower.isBusy()) {
+                    return 2;
+                }
+                break;
+        }
+
+        return pathState;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/nineBallBlue.java

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.TelemetryManager;
+import com.bylazar.telemetry.PanelsTelemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.paths.PathChain;
+import com.pedropathing.geometry.Pose;
+
+@Autonomous(name = "Pedro Pathing Autonomous", group = "Autonomous")
+@Configurable // Panels
+public class nineBallBlue extends OpMode {
+    private TelemetryManager panelsTelemetry; // Panels Telemetry instance
+    public Follower follower; // Pedro Pathing follower instance
+    private int pathState; // Current autonomous path state (state machine)
+    private Paths paths; // Paths defined in the Paths class
+
+    @Override
+    public void init() {
+        panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry();
+
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(72, 8, Math.toRadians(90)));
+
+        paths = new Paths(follower); // Build paths
+
+        panelsTelemetry.debug("Status", "Initialized");
+        panelsTelemetry.update(telemetry);
+    }
+
+    @Override
+    public void loop() {
+        follower.update(); // Update Pedro Pathing
+        pathState = autonomousPathUpdate(); // Update autonomous state machine
+
+        // Log values to Panels and Driver Station
+        panelsTelemetry.debug("Path State", pathState);
+        panelsTelemetry.debug("X", follower.getPose().getX());
+        panelsTelemetry.debug("Y", follower.getPose().getY());
+        panelsTelemetry.debug("Heading", follower.getPose().getHeading());
+        panelsTelemetry.update(telemetry);
+    }
+
+    public static class Paths {
+        public PathChain MainChain;
+
+        public Paths(Follower follower) {
+            MainChain = follower.pathBuilder()
+                    .addPath(
+                            new BezierLine(
+                                    new Pose(19.768, 121.211),
+                                    new Pose(57.561, 83.080)
+                            )
+                    )
+                    .setLinearHeadingInterpolation(Math.toRadians(142), Math.toRadians(142))
+                    .build();
+        }
+    }
+
+    public int autonomousPathUpdate() {
+        // Add your state machine Here
+        // Access paths with paths.pathName
+        // Refer to the Pedro Pathing Docs (Auto Example) for an example state machine
+        return 0;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java

@@ -1,19 +1,57 @@
 package org.firstinspires.ftc.teamcode.pedroPathing;
 
+import com.pedropathing.control.PIDFCoefficients;
+import com.pedropathing.control.PredictiveBrakingCoefficients;
 import com.pedropathing.follower.Follower;
 import com.pedropathing.follower.FollowerConstants;
 import com.pedropathing.ftc.FollowerBuilder;
+import com.pedropathing.ftc.drivetrains.MecanumConstants;
+import com.pedropathing.ftc.localization.constants.PinpointConstants;
 import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 
 public class Constants {
-    public static FollowerConstants followerConstants = new FollowerConstants();
+
+    public static FollowerConstants followerConstants = new FollowerConstants()
+            .mass(10)
+            .predictiveBrakingCoefficients(new PredictiveBrakingCoefficients(0.2, 0.04570170496436661, 0.001962488318085352))
+            .forwardZeroPowerAcceleration(-37.6795)
+            .lateralZeroPowerAcceleration(-68.28657)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.025, 0, 0.0001, 0.04))
+            .headingPIDFCoefficients(new PIDFCoefficients(0.87, 0, 0.000001, 0.04));
+
+    public static MecanumConstants driveConstants = new MecanumConstants()
+            .maxPower(1)
+            .rightFrontMotorName("rf")
+            .rightRearMotorName("rr")
+            .leftRearMotorName("lr")
+            .leftFrontMotorName("lf")
+            .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE)
+            .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
+            .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
+            .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
+            .xVelocity(77.4087)
+            .yVelocity(63.1328);
+
+    public static PinpointConstants localizerConstants = new PinpointConstants()
+            .forwardPodY(4.492)
+            .strafePodX(-1.781)   //inches
+            .distanceUnit(DistanceUnit.INCH)
+            .hardwareMapName("pinpoint")
+            .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
+            .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED)
+            .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED);
 
     public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
 
     public static Follower createFollower(HardwareMap hardwareMap) {
         return new FollowerBuilder(followerConstants, hardwareMap)
                 .pathConstraints(pathConstraints)
+                .mecanumDrivetrain(driveConstants)
+                .pinpointLocalizer(localizerConstants)
                 .build();
     }
-}
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java

@@ -0,0 +1,69 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+
+public class Drivetrain {
+
+    private final Follower follower;
+
+    public Drivetrain(HardwareMap hardwareMap) {
+        follower = Constants.createFollower(hardwareMap);
+    }
+
+    /**
+     * Start Pedro's teleop drive mode.
+     */
+    public void start() {
+        follower.startTeleopDrive();
+    }
+
+    /**
+     * Drive the robot using field-centric controls.
+     *
+     * @param forward Forward/backward joystick input
+     * @param strafe  Left/right joystick input
+     * @param turn    Rotation joystick input
+     */
+    public void drive(double forward, double strafe, double turn) {
+
+        follower.setTeleOpDrive(
+                forward,
+                strafe,
+                turn,
+                true
+        );
+    }
+
+    /**
+     * Update Pedro Pathing.
+     * This should be called once every loop.
+     */
+    public void update() {
+        follower.update();
+    }
+
+    /**
+     * Get the robot's current field position.
+     */
+    public Pose getPose() {
+        return follower.getPose();
+    }
+
+    /**
+     * Set the robot's starting position.
+     */
+    public void setStartingPose(Pose pose) {
+        follower.setStartingPose(pose);
+    }
+
+    /**
+     * Stop the drivetrain.
+     */
+    public void stop() {
+        follower.breakFollowing();
+    }
+}