Skip to content

Commit 75abd68

Browse files
BaronClapsBaronClaps
authored
1.0.8 Release
1.0.8 Release
2 parents 576d279 + 30a77d4 commit 75abd68

19 files changed

+403
-74
lines changed

‎build.gradle.kts‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -97,8 +97,8 @@ publishing {
9797
publications {
9898
register<MavenPublication>("release") {
9999
groupId = "com.pedropathing"
100-
artifactId = "pedro"
101-
version = "1.0.7"
100+
artifactId = "beta"
101+
version = "1.0.8-beta3"
102102

103103
afterEvaluate {
104104
from(components["release"])

‎maven.pedropathing.com‎

Submodule maven.pedropathing.com updated 70 files

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

Lines changed: 123 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package com.pedropathing.follower;
22

33
import static com.pedropathing.follower.FollowerConstants.automaticHoldEnd;
4+
import static com.pedropathing.follower.FollowerConstants.cacheInvalidateSeconds;
45
import static com.pedropathing.follower.FollowerConstants.drivePIDFFeedForward;
56
import static com.pedropathing.follower.FollowerConstants.drivePIDFSwitch;
67
import static com.pedropathing.follower.FollowerConstants.forwardZeroPowerAcceleration;
@@ -9,7 +10,7 @@
910
import static com.pedropathing.follower.FollowerConstants.lateralZeroPowerAcceleration;
1011
import static com.pedropathing.follower.FollowerConstants.leftFrontMotorName;
1112
import static com.pedropathing.follower.FollowerConstants.leftRearMotorName;
12-
import static com.pedropathing.follower.FollowerConstants.localizers;
13+
import static com.pedropathing.follower.FollowerConstants.nominalVoltage;
1314
import static com.pedropathing.follower.FollowerConstants.rightFrontMotorName;
1415
import static com.pedropathing.follower.FollowerConstants.rightRearMotorName;
1516
import static com.pedropathing.follower.FollowerConstants.leftFrontMotorDirection;
@@ -24,15 +25,17 @@
2425
import static com.pedropathing.follower.FollowerConstants.useSecondaryDrivePID;
2526
import static com.pedropathing.follower.FollowerConstants.useSecondaryHeadingPID;
2627
import static com.pedropathing.follower.FollowerConstants.useSecondaryTranslationalPID;
28+
import static com.pedropathing.follower.FollowerConstants.useVoltageCompensationInAuto;
29+
import static com.pedropathing.follower.FollowerConstants.useVoltageCompensationInTeleOp;
2730

2831
import android.util.Log;
2932

3033
import com.acmerobotics.dashboard.config.Config;
3134
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
3235
import com.qualcomm.robotcore.hardware.DcMotor;
3336
import com.qualcomm.robotcore.hardware.DcMotorEx;
34-
import com.qualcomm.robotcore.hardware.DcMotorSimple;
3537
import com.qualcomm.robotcore.hardware.HardwareMap;
38+
import com.qualcomm.robotcore.hardware.VoltageSensor;
3639
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
3740

3841
import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -88,7 +91,7 @@ public class Follower {
8891

8992
private PathChain currentPathChain;
9093

91-
private final int BEZIER_CURVE_BINARY_STEP_LIMIT = FollowerConstants.BEZIER_CURVE_BINARY_STEP_LIMIT;
94+
private final int BEZIER_CURVE_SEARCH_LIMIT = FollowerConstants.BEZIER_CURVE_SEARCH_LIMIT;
9295
private final int AVERAGED_VELOCITY_SAMPLE_NUMBER = FollowerConstants.AVERAGED_VELOCITY_SAMPLE_NUMBER;
9396

9497
private int chainIndex;
@@ -131,6 +134,8 @@ public class Follower {
131134
public Vector centripetalVector;
132135
public Vector correctiveVector;
133136

137+
private double centripetalScaling = FollowerConstants.centripetalScaling;
138+
134139
private PIDFController secondaryTranslationalPIDF = new PIDFController(FollowerConstants.secondaryTranslationalPIDFCoefficients);
135140
private PIDFController secondaryTranslationalIntegral = new PIDFController(FollowerConstants.secondaryTranslationalIntegral);
136141
private PIDFController translationalPIDF = new PIDFController(FollowerConstants.translationalPIDFCoefficients);
@@ -151,10 +156,20 @@ public class Follower {
151156
public static boolean useHeading = true;
152157
public static boolean useDrive = true;
153158

159+
/*
160+
* Voltage Compensation
161+
* Credit to team 14343 Escape Velocity for the voltage code
162+
* Credit to team 23511 Seattle Solvers for implementing the voltage code into Follower.java
163+
*/
164+
private boolean cached = false;
165+
166+
private VoltageSensor voltageSensor;
167+
public double voltage = 0;
168+
private final ElapsedTime voltageTimer = new ElapsedTime();
169+
154170
private boolean logDebug = true;
155171

156172
private ElapsedTime zeroVelocityDetectedTimer;
157-
private ElapsedTime pinpointRecalibrationTimer;
158173

159174
/**
160175
* This creates a new Follower given a HardwareMap.
@@ -185,6 +200,9 @@ public void initialize() {
185200
poseUpdater = new PoseUpdater(hardwareMap);
186201
driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector);
187202

203+
voltageSensor = hardwareMap.voltageSensor.iterator().next();
204+
voltageTimer.reset();
205+
188206
leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
189207
leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
190208
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
@@ -221,6 +239,9 @@ public void initialize(Localizer localizer) {
221239
poseUpdater = new PoseUpdater(hardwareMap, localizer);
222240
driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector);
223241

242+
voltageSensor = hardwareMap.voltageSensor.iterator().next();
243+
voltageTimer.reset();
244+
224245
leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName);
225246
leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName);
226247
rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
@@ -245,6 +266,10 @@ public void initialize(Localizer localizer) {
245266
breakFollowing();
246267
}
247268

269+
public void setCentripetalScaling(double set) {
270+
centripetalScaling = set;
271+
}
272+
248273
/**
249274
* This sets the motors to the zero power behavior of brake.
250275
*/
@@ -463,7 +488,7 @@ public void followPath(Path path, boolean holdEnd) {
463488
isBusy = true;
464489
followingPathChain = false;
465490
currentPath = path;
466-
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
491+
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_SEARCH_LIMIT);
467492
}
468493

469494
/**
@@ -514,7 +539,8 @@ public void followPath(PathChain pathChain, double maxPower, boolean holdEnd) {
514539
chainIndex = 0;
515540
currentPathChain = pathChain;
516541
currentPath = pathChain.getPath(chainIndex);
517-
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
542+
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_SEARCH_LIMIT);
543+
currentPathChain.resetCallbacks();
518544
}
519545

520546
/**
@@ -524,7 +550,7 @@ public void resumePathFollowing() {
524550
pathStartTimes = new long[currentPathChain.size()];
525551
pathStartTimes[0] = System.currentTimeMillis();
526552
isBusy = true;
527-
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
553+
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_SEARCH_LIMIT);
528554
}
529555

530556
/**
@@ -566,20 +592,32 @@ public void update() {
566592

567593
for (int i = 0; i < motors.size(); i++) {
568594
if (Math.abs(motors.get(i).getPower() - drivePowers[i]) > FollowerConstants.motorCachingThreshold) {
569-
motors.get(i).setPower(drivePowers[i]);
595+
double voltageNormalized = getVoltageNormalized();
596+
597+
if (useVoltageCompensationInAuto) {
598+
motors.get(i).setPower(drivePowers[i] * voltageNormalized);
599+
} else {
600+
motors.get(i).setPower(drivePowers[i]);
601+
}
570602
}
571603
}
572604
} else {
573605
if (isBusy) {
574-
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
606+
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_SEARCH_LIMIT);
575607

576608
if (followingPathChain) updateCallbacks();
577609

578610
drivePowers = driveVectorScaler.getDrivePowers(getCorrectiveVector(), getHeadingVector(), getDriveVector(), poseUpdater.getPose().getHeading());
579611

580612
for (int i = 0; i < motors.size(); i++) {
581613
if (Math.abs(motors.get(i).getPower() - drivePowers[i]) > FollowerConstants.motorCachingThreshold) {
582-
motors.get(i).setPower(drivePowers[i]);
614+
double voltageNormalized = getVoltageNormalized();
615+
616+
if (useVoltageCompensationInAuto) {
617+
motors.get(i).setPower(drivePowers[i] * voltageNormalized);
618+
} else {
619+
motors.get(i).setPower(drivePowers[i]);
620+
}
583621
}
584622
}
585623
}
@@ -609,7 +647,7 @@ public void update() {
609647
followingPathChain = true;
610648
chainIndex++;
611649
currentPath = currentPathChain.getPath(chainIndex);
612-
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_BINARY_STEP_LIMIT);
650+
closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), BEZIER_CURVE_SEARCH_LIMIT);
613651
} else {
614652
// At last path, run some end detection stuff
615653
// set isBusy to false if at end
@@ -656,7 +694,13 @@ public void update() {
656694

657695
for (int i = 0; i < motors.size(); i++) {
658696
if (Math.abs(motors.get(i).getPower() - drivePowers[i]) > FollowerConstants.motorCachingThreshold) {
659-
motors.get(i).setPower(drivePowers[i]);
697+
double voltageNormalized = getVoltageNormalized();
698+
699+
if (useVoltageCompensationInTeleOp) {
700+
motors.get(i).setPower(drivePowers[i] * voltageNormalized);
701+
} else {
702+
motors.get(i).setPower(drivePowers[i]);
703+
}
660704
}
661705
}
662706
}
@@ -861,18 +905,18 @@ public double getDriveVelocityError() {
861905
Vector velocity = new Vector(MathFunctions.dotProduct(getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), currentPath.getClosestPointTangentVector().getTheta());
862906

863907
Vector forwardHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading());
908+
864909
double forwardVelocity = MathFunctions.dotProduct(forwardHeadingVector, velocity);
865910
double forwardDistanceToGoal = MathFunctions.dotProduct(forwardHeadingVector, distanceToGoalVector);
866-
867911
double forwardVelocityGoal = MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * forwardZeroPowerAcceleration * (forwardDistanceToGoal <= 0 ? 1 : -1) * forwardDistanceToGoal));
868-
double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * forwardDistanceToGoal));
912+
double forwardVelocityZeroPowerDecay = forwardVelocity - MathFunctions.getSign(forwardDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(forwardVelocity, 2) + 2 * forwardZeroPowerAcceleration * Math.abs(forwardDistanceToGoal)));
869913

870914
Vector lateralHeadingVector = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2);
871915
double lateralVelocity = MathFunctions.dotProduct(lateralHeadingVector, velocity);
872916
double lateralDistanceToGoal = MathFunctions.dotProduct(lateralHeadingVector, distanceToGoalVector);
873917

874918
double lateralVelocityGoal = MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(-2 * currentPath.getZeroPowerAccelerationMultiplier() * lateralZeroPowerAcceleration * (lateralDistanceToGoal <= 0 ? 1 : -1) * lateralDistanceToGoal));
875-
double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * lateralDistanceToGoal));
919+
double lateralVelocityZeroPowerDecay = lateralVelocity - MathFunctions.getSign(lateralDistanceToGoal) * Math.sqrt(Math.abs(Math.pow(lateralVelocity, 2) + 2 * lateralZeroPowerAcceleration * Math.abs(lateralDistanceToGoal)));
876920

877921
Vector forwardVelocityError = new Vector(forwardVelocityGoal - forwardVelocityZeroPowerDecay - forwardVelocity, forwardHeadingVector.getTheta());
878922
Vector lateralVelocityError = new Vector(lateralVelocityGoal - lateralVelocityZeroPowerDecay - lateralVelocity, lateralHeadingVector.getTheta());
@@ -1021,7 +1065,7 @@ public Vector getCentripetalForceCorrection() {
10211065
curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3));
10221066
}
10231067
if (Double.isNaN(curvature)) return new Vector();
1024-
centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
1068+
centripetalVector = new Vector(MathFunctions.clamp(centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -driveVectorScaler.getMaxPowerScaling(), driveVectorScaler.getMaxPowerScaling()), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta()));
10251069
return centripetalVector;
10261070
}
10271071

@@ -1192,7 +1236,68 @@ public void drawOnDashBoard() {
11921236
}
11931237
}
11941238

1195-
public boolean isPinpointCooked() {
1196-
return poseUpdater.getLocalizer().isPinpointCooked();
1239+
public boolean isLocalizationNAN() {
1240+
return poseUpdater.getLocalizer().isNAN();
1241+
}
1242+
1243+
/**
1244+
* @return The last cached voltage measurement.
1245+
*/
1246+
public double getVoltage() {
1247+
if (voltageTimer.seconds() > cacheInvalidateSeconds && cacheInvalidateSeconds >= 0) {
1248+
cached = false;
1249+
}
1250+
1251+
if (!cached)
1252+
refreshVoltage();
1253+
1254+
return voltage;
1255+
}
1256+
1257+
/**
1258+
* @return A scalar that normalizes power outputs to the nominal voltage from the current voltage.
1259+
*/
1260+
public double getVoltageNormalized() {
1261+
return Math.min(nominalVoltage / getVoltage(), 1);
1262+
}
1263+
1264+
/**
1265+
* Overrides the voltage cooldown.
1266+
*/
1267+
public void refreshVoltage() {
1268+
cached = true;
1269+
voltage = voltageSensor.getVoltage();
1270+
voltageTimer.reset();
1271+
}
1272+
1273+
/** Turns a certain amount of degrees left
1274+
* @param radians the amount of radians to turn
1275+
* @param isLeft true if turning left, false if turning right
1276+
*/
1277+
public void turn(double radians, boolean isLeft) {
1278+
Pose temp = new Pose(getPose().getX(), getPose().getY(), getPose().getHeading() + (isLeft ? radians : -radians));
1279+
holdPoint(temp);
1280+
}
1281+
1282+
/** Turns to a specific heading
1283+
* @param radians the heading in radians to turn to
1284+
*/
1285+
public void turnTo(double radians) {
1286+
holdPoint(new Pose(getPose().getX(), getPose().getY(), Math.toRadians(radians)));
1287+
}
1288+
1289+
/** Turns to a specific heading in degrees
1290+
* @param degrees the heading in degrees to turn to
1291+
*/
1292+
public void turnToDegrees(double degrees) {
1293+
turnTo(Math.toRadians(degrees));
1294+
}
1295+
1296+
/** Turns a certain amount of degrees left
1297+
* @param degrees the amount of degrees to turn
1298+
* @param isLeft true if turning left, false if turning right
1299+
*/
1300+
public void turnDegrees(double degrees, boolean isLeft) {
1301+
turn(Math.toRadians(degrees), isLeft);
11971302
}
11981303
}

‎src/main/java/com/pedropathing/follower/FollowerConstants.java‎

Lines changed: 26 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -207,7 +207,7 @@ public class FollowerConstants {
207207
public static double pathEndTimeoutConstraint = 500;
208208

209209
/** This is how many steps the BezierCurve class uses to approximate the length of a BezierCurve.
210-
* @see #BEZIER_CURVE_BINARY_STEP_LIMIT
210+
* @see #BEZIER_CURVE_SEARCH_LIMIT
211211
* Default Value: 1000 */
212212
public static int APPROXIMATION_STEPS = 1000;
213213

@@ -226,12 +226,11 @@ public class FollowerConstants {
226226
* Default Value: 8 */
227227
public static int AVERAGED_VELOCITY_SAMPLE_NUMBER = 8;
228228

229-
/** This is the number of steps the binary search for closest point uses. More steps is more
230-
* accuracy, and this increases at an exponential rate. However, more steps also does take more
231-
* time.
229+
/** This is the number of steps the search for the closest point uses. More steps lead to bigger
230+
* accuracy. However, more steps also take more time.
232231
* @see #APPROXIMATION_STEPS
233232
* Default Value: 10 */
234-
public static int BEZIER_CURVE_BINARY_STEP_LIMIT = 10;
233+
public static int BEZIER_CURVE_SEARCH_LIMIT = 10;
235234

236235
/** This activates/deactivates the secondary translational PIDF. It takes over at a certain translational error
237236
* @see #translationalPIDFSwitch
@@ -318,11 +317,32 @@ public class FollowerConstants {
318317
* Default Value: 0.01 */
319318
public static double secondaryDrivePIDFFeedForward = 0.01;
320319

321-
/** Use break mode for the drive motors in teleop
320+
/** Use brake mode for the drive motors in teleop
322321
* Default Value: false */
323322
public static boolean useBrakeModeInTeleOp = false;
324323

325324
/** Boolean that determines if holdEnd is automatically (when not defined in the constructor) enabled at the end of a path.
326325
* Default Value: true */
327326
public static boolean automaticHoldEnd = true;
327+
328+
/** Use voltage compensation to linearly scale motor powers in Auto
329+
* Requires fully re-tuning if you set it to true
330+
* Default Value: false */
331+
public static boolean useVoltageCompensationInAuto = false;
332+
333+
/** Use voltage compensation to linearly scale motor powers in TeleOp
334+
* Requires fully re-tuning if you set it to true
335+
* Default Value: false */
336+
public static boolean useVoltageCompensationInTeleOp = false;
337+
338+
/** The voltage to scale to (the voltage that you tuned at)
339+
* If the robot's voltage is at the default value, it will not affect the motor powers.
340+
* Will only read voltage if useVoltageCompensation is true.
341+
* Default Value: 12.0 */
342+
public static double nominalVoltage = 12.0;
343+
344+
/** Time (in seconds) before reading voltage again
345+
* Will only read voltage if useVoltageCompensation is true.
346+
* Default Value: 0.5 */
347+
public static double cacheInvalidateSeconds = 0.5;
328348
}

‎src/main/java/com/pedropathing/localization/Localizer.java‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -103,7 +103,10 @@ public IMU getIMU() {
103103
return null;
104104
}
105105

106-
public boolean isPinpointCooked() {return false;}
107-
108-
public void setPinpointIsCooked(boolean cooked) {}
106+
/**
107+
* This returns whether if any component of robot's position is NaN.
108+
*
109+
* @return returns if any component of the robot's position is NaN
110+
*/
111+
public abstract boolean isNAN();
109112
}

0 commit comments

Comments
 (0)