1.0.9

1.0.9

Author: BaronClaps

build.gradle.kts

@@ -79,12 +79,7 @@ dependencies {
 	compileOnly("org.firstinspires.ftc:FtcCommon:10.2.0")
 	compileOnly("org.firstinspires.ftc:RobotServer:10.2.0")
 	compileOnly("org.firstinspires.ftc:OnBotJava:10.2.0")
-
-
-
-	implementation("com.acmerobotics.dashboard:dashboard:0.4.16") {
-		exclude(group = "org.firstinspires.ftc")
-	}
+	compileOnly("com.acmerobotics.dashboard:dashboard:0.4.16")
 
 	implementation("org.apache.commons:commons-math3:3.6.1")
 	dokkaHtmlPlugin("org.jetbrains.dokka:kotlin-as-java-plugin:1.9.20")
@@ -98,7 +93,7 @@ publishing {
 		register<MavenPublication>("release") {
 			groupId = "com.pedropathing"
 			artifactId = "pedro"
-			version = "1.0.8"
+			version = "1.0.9"
 
 			afterEvaluate {
 				from(components["release"])

maven.pedropathing.com

@@ -32,6 +32,9 @@
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.util.Constants;
+import com.pedropathing.util.CustomFilteredPIDFCoefficients;
+import com.pedropathing.util.CustomPIDFCoefficients;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -91,25 +94,25 @@ public class Follower {
 
     private PathChain currentPathChain;
 
-    private final int BEZIER_CURVE_SEARCH_LIMIT = FollowerConstants.BEZIER_CURVE_SEARCH_LIMIT;
-    private final int AVERAGED_VELOCITY_SAMPLE_NUMBER = FollowerConstants.AVERAGED_VELOCITY_SAMPLE_NUMBER;
+    private int BEZIER_CURVE_SEARCH_LIMIT;
+    private int AVERAGED_VELOCITY_SAMPLE_NUMBER;
 
     private int chainIndex;
 
     private long[] pathStartTimes;
 
     private boolean followingPathChain;
     private boolean holdingPosition;
-    private boolean isBusy;
+    private boolean isBusy, isTurning;
     private boolean reachedParametricPathEnd;
     private boolean holdPositionAtEnd;
     private boolean teleopDrive;
 
     private double globalMaxPower = 1;
     private double previousSecondaryTranslationalIntegral;
     private double previousTranslationalIntegral;
-    private double holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling;
-    private double holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling;
+    private double holdPointTranslationalScaling;
+    private double holdPointHeadingScaling;
     public double driveError;
     public double headingError;
 
@@ -134,21 +137,22 @@ public class Follower {
     public Vector centripetalVector;
     public Vector correctiveVector;
 
-    private double centripetalScaling = FollowerConstants.centripetalScaling;
+    private double centripetalScaling;
 
-    private PIDFController secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients);
-    private PIDFController secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral);
-    private PIDFController translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients);
-    private PIDFController translationalIntegral = new PIDFController(FollowerConstants.translationalIntegral);
-    private PIDFController secondaryHeadingPIDF = new PIDFController(FollowerConstants.secondaryHeadingPIDFCoefficients);
-    private PIDFController headingPIDF = new PIDFController(FollowerConstants.headingPIDFCoefficients);
-    private FilteredPIDFController secondaryDrivePIDF = new FilteredPIDFController(FollowerConstants.secondaryDrivePIDFCoefficients);
-    private FilteredPIDFController drivePIDF = new FilteredPIDFController(FollowerConstants.drivePIDFCoefficients);
+    private PIDFController secondaryTranslationalPIDF;
+    private PIDFController secondaryTranslationalIntegral;
+    private PIDFController translationalPIDF;
+    private PIDFController translationalIntegral;
+    private PIDFController secondaryHeadingPIDF;
+    private PIDFController headingPIDF;
+    private FilteredPIDFController secondaryDrivePIDF;
+    private FilteredPIDFController drivePIDF;
 
-    private KalmanFilter driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters);
+    private KalmanFilter driveKalmanFilter;
     private double[] driveErrors;
     private double rawDriveError;
     private double previousRawDriveError;
+    private double turnHeadingErrorThreshold;
 
     public static boolean drawOnDashboard = true;
     public static boolean useTranslational = true;
@@ -175,8 +179,9 @@ public class Follower {
      * This creates a new Follower given a HardwareMap.
      * @param hardwareMap HardwareMap required
      */
-    public Follower(HardwareMap hardwareMap) {
+    public Follower(HardwareMap hardwareMap, Class<?> FConstants, Class<?> LConstants) {
         this.hardwareMap = hardwareMap;
+        setupConstants(FConstants, LConstants);
         initialize();
     }
 
@@ -185,11 +190,36 @@ public Follower(HardwareMap hardwareMap) {
      * @param hardwareMap HardwareMap required
      * @param localizer the localizer you wish to use
      */
-    public Follower(HardwareMap hardwareMap, Localizer localizer) {
+    public Follower(HardwareMap hardwareMap, Localizer localizer, Class<?> FConstants, Class<?> LConstants) {
         this.hardwareMap = hardwareMap;
+        setupConstants(FConstants, LConstants);
         initialize(localizer);
     }
 
+    /**
+     * Setup constants for the Follower.
+     * @param FConstants the constants for the Follower
+     * @param LConstants the constants for the Localizer
+     */
+    public void setupConstants(Class<?> FConstants, Class<?> LConstants) {
+        Constants.setConstants(FConstants, LConstants);
+        BEZIER_CURVE_SEARCH_LIMIT = FollowerConstants.BEZIER_CURVE_SEARCH_LIMIT;
+        AVERAGED_VELOCITY_SAMPLE_NUMBER = FollowerConstants.AVERAGED_VELOCITY_SAMPLE_NUMBER;
+        holdPointTranslationalScaling = FollowerConstants.holdPointTranslationalScaling;
+        holdPointHeadingScaling = FollowerConstants.holdPointHeadingScaling;
+        centripetalScaling = FollowerConstants.centripetalScaling;
+        secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients);
+        secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral);
+        translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients);
+        translationalIntegral = new PIDFController(FollowerConstants.translationalIntegral);
+        secondaryHeadingPIDF = new PIDFController(FollowerConstants.secondaryHeadingPIDFCoefficients);
+        headingPIDF = new PIDFController(FollowerConstants.headingPIDFCoefficients);
+        secondaryDrivePIDF = new FilteredPIDFController(FollowerConstants.secondaryDrivePIDFCoefficients);
+        drivePIDF = new FilteredPIDFController(FollowerConstants.drivePIDFCoefficients);
+        driveKalmanFilter = new KalmanFilter(FollowerConstants.driveKalmanFilterParameters);
+        turnHeadingErrorThreshold = FollowerConstants.turnHeadingErrorThreshold;
+    }
+
     /**
      * This initializes the follower.
      * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are
@@ -601,6 +631,11 @@ public void update() {
                             }
                         }
                     }
+
+                    if(headingError < turnHeadingErrorThreshold && isTurning) {
+                        isTurning = false;
+                        isBusy = false;
+                    }
                 } else {
                     if (isBusy) {
                         closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_SEARCH_LIMIT);
@@ -1277,13 +1312,17 @@ public void refreshVoltage() {
     public void turn(double radians, boolean isLeft) {
         Pose temp = new Pose(getPose().getX(), getPose().getY(), getPose().getHeading() + (isLeft ? radians : -radians));
         holdPoint(temp);
+        isTurning = true;
+        isBusy = true;
     }
 
     /** Turns to a specific heading
      * @param radians the heading in radians to turn to
      */
     public void turnTo(double radians) {
         holdPoint(new Pose(getPose().getX(), getPose().getY(), Math.toRadians(radians)));
+        isTurning = true;
+        isBusy = true;
     }
 
     /** Turns to a specific heading in degrees
@@ -1300,4 +1339,103 @@ public void turnToDegrees(double degrees) {
     public void turnDegrees(double degrees, boolean isLeft) {
         turn(Math.toRadians(degrees), isLeft);
     }
+
+    public boolean isTurning() {
+        return isTurning;
+    }
+
+    /**
+     * This will update the PIDF coefficients for primary Heading PIDF mid run
+     * can be used between paths
+     *
+     * @param set PIDF coefficients you would like to set.
+     */
+    public void setHeadingPIDF(CustomPIDFCoefficients set){
+        headingPIDF.setCoefficients(set);
+    }
+
+    /**
+     * This will update the PIDF coefficients for primary Translational PIDF mid run
+     * can be used between paths
+     *
+     * @param set PIDF coefficients you would like to set.
+     */
+    public void setTranslationalPIDF(CustomPIDFCoefficients set){
+        translationalPIDF.setCoefficients(set);
+    }
+
+    /**
+     * This will update the PIDF coefficients for primary Drive PIDF mid run
+     * can be used between paths
+     *
+     * @param set PIDF coefficients you would like to set.
+     */
+    public void setDrivePIDF(CustomFilteredPIDFCoefficients set){
+        drivePIDF.setCoefficients(set);
+    }
+
+    /**
+     * This will update the PIDF coefficients for secondary Heading PIDF mid run
+     * can be used between paths
+     *
+     * @param set PIDF coefficients you would like to set.
+     */
+    public void setSecondaryHeadingPIDF(CustomPIDFCoefficients set){
+        secondaryHeadingPIDF.setCoefficients(set);
+    }
+
+    /**
+     * This will update the PIDF coefficients for secondary Translational PIDF mid run
+     * can be used between paths
+     *
+     * @param set PIDF coefficients you would like to set.
+     */
+    public void setSecondaryTranslationalPIDF(CustomPIDFCoefficients set){
+        secondaryTranslationalPIDF.setCoefficients(set);
+    }
+
+    /**
+     * This will update the PIDF coefficients for secondary Drive PIDF mid run
+     * can be used between paths
+     *
+     * @param set PIDF coefficients you would like to set.
+     */
+    public void setSecondaryDrivePIDF(CustomFilteredPIDFCoefficients set){
+        secondaryDrivePIDF.setCoefficients(set);
+    }
+
+    /**
+     * Checks if the robot is at a certain point within certain tolerances
+     * @param point Point to compare with the current point
+     * @param xTolerance Tolerance for the x position
+     * @param yTolerance Tolerance for the y position
+     */
+    public boolean atPoint(Point point, double xTolerance, double yTolerance) {
+        return Math.abs(point.getX() - getPose().getX()) < xTolerance && Math.abs(point.getY() - getPose().getY()) < yTolerance;
+    }
+
+    /**
+     * Checks if the robot is at a certain pose within certain tolerances
+     * @param pose Pose to compare with the current pose
+     * @param xTolerance Tolerance for the x position
+     * @param yTolerance Tolerance for the y position
+     * @param headingTolerance Tolerance for the heading
+     */
+    public boolean atPose(Pose pose, double xTolerance, double yTolerance, double headingTolerance) {
+        return Math.abs(pose.getX() - getPose().getX()) < xTolerance && Math.abs(pose.getY() - getPose().getY()) < yTolerance && Math.abs(pose.getHeading() - getPose().getHeading()) < headingTolerance;
+    }
+
+    /**
+     * Checks if the robot is at a certain pose within certain tolerances
+     * @param pose Pose to compare with the current pose
+     * @param xTolerance Tolerance for the x position
+     * @param yTolerance Tolerance for the y position
+     */
+    public boolean atPose(Pose pose, double xTolerance, double yTolerance) {
+        return Math.abs(pose.getX() - getPose().getX()) < xTolerance && Math.abs(pose.getY() - getPose().getY()) < yTolerance;
+    }
+
+    public double getHeadingError() {
+        return headingError;
+    }
 }

src/main/java/com/pedropathing/follower/Follower.java

@@ -345,4 +345,9 @@ public class FollowerConstants {
      *  Will only read voltage if useVoltageCompensation is true.
      *  Default Value: 0.5 */
     public static double cacheInvalidateSeconds = 0.5;
+
+    /** Threshold that the turn and turnTo methods will be considered to be finished
+     *  In Radians
+     *  Default Value: 0.01 */
+    public static double turnHeadingErrorThreshold = 0.01;
 }