diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index c1c18ae..dd67303 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -22,7 +22,8 @@ public class Constants { .lateralZeroPowerAcceleration(-72.872) .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) - .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)); + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)) + .centripetalScaling(0.0005); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 1de9ad4..20f8497 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -114,7 +114,7 @@ public class Tuning extends SelectableOpMode { public static void drawCurrent() { try { - Drawing.drawRobot(follower.getPose(), "blue"); + Drawing.drawRobot(follower.getPose(), "green"); Drawing.sendPacket(); } catch (Exception e) { throw new RuntimeException("Drawing failed " + e);