diff --git a/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java b/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java index 0cbbb1b..e60f218 100644 --- a/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java +++ b/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java @@ -187,19 +187,19 @@ public void buildPaths() { pushSamples = follower.pathBuilder() .addPath(new BezierCurve(new Point(preloadPose), new Point(16.088, 22.000, Point.CARTESIAN), new Point(57.345, 50.496, Point.CARTESIAN), new Point(56.000, 26.000, Point.CARTESIAN))) - .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(90)) .addPath(new BezierLine(new Point(56.000, 26.000, Point.CARTESIAN), new Point(24, 26.000, Point.CARTESIAN))) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(90)) .addPath(new BezierCurve(new Point(24, 26.000, Point.CARTESIAN), new Point(56.000, 26.000, Point.CARTESIAN), new Point(56.000, 16.000, Point.CARTESIAN))) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(90)) .addPath(new BezierLine(new Point(56.000, 16.000, Point.CARTESIAN),new Point(24, 16.000, Point.CARTESIAN))) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(90)) // .addPath(new BezierCurve(new Point(24, 16.000, Point.CARTESIAN), new Point(56.000, 16.000, Point.CARTESIAN), new Point(56.000, 9.50, Point.CARTESIAN))) // .setConstantHeadingInterpolation(Math.toRadians(0)) // .addPath(new BezierLine(new Point(56.000, 9.50, Point.CARTESIAN), new Point(24, 9.5, Point.CARTESIAN))) // .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath(new BezierLine(new Point(24, 16, Point.CARTESIAN), new Point(blueObservationSpecimenSetPose))) - .setLinearHeadingInterpolation(Math.toRadians(0), blueObservationSpecimenSetPose.getHeading()) + .setLinearHeadingInterpolation(Math.toRadians(90), blueObservationSpecimenSetPose.getHeading()) .build(); grab1 = follower.pathBuilder() diff --git a/TeamCode/src/main/java/indubitables/config/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/indubitables/config/subsystem/LiftSubsystem.java index cb600f9..d9f370c 100644 --- a/TeamCode/src/main/java/indubitables/config/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/indubitables/config/subsystem/LiftSubsystem.java @@ -37,8 +37,7 @@ public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { leftLift.setDirection(DcMotor.Direction.REVERSE); rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + liftPID = new PIDController(p, i, d); @@ -52,6 +51,9 @@ public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { public void updatePIDF(){ if (!manual) { + rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + double pid = liftPID.calculate(getPos(), target); double ticks_in_degrees = 537.7 / 360.0; double ff = Math.cos(Math.toRadians(target / ticks_in_degrees)) * f; @@ -66,7 +68,8 @@ public void updatePIDF(){ } public void manual(double n){ - manual = true; + rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightLift.setPower(n); leftLift.setPower(n); diff --git a/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java b/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java index 2f8593d..10296c5 100644 --- a/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java +++ b/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java @@ -41,8 +41,8 @@ public enum RobotStart { public static final Pose blueObservationElement3Pose = new Pose(20, 96, Math.toRadians(180)); public static final Pose blueObservationElement3ControlPose = new Pose(20, 110); public static final Pose blueObservationSpecimenSetPose = new Pose(21, 25.5, Math.toRadians(0)); - public static final Pose blueObservationSpecimenPickupPose = new Pose(16.5, 25.75, Math.toRadians(0)); - public static final Pose blueObservationSpecimenPickup2Pose = new Pose(16.75, 25.5, Math.toRadians(0)); + public static final Pose blueObservationSpecimenPickupPose = new Pose(17.25, 25.5, Math.toRadians(0)); + public static final Pose blueObservationSpecimenPickup2Pose = new Pose(17.25, 25.5, Math.toRadians(0)); public static final Pose blueObservationSpecimen1Pose = new Pose(28, 68.625, Math.toRadians(180)); public static final Pose blueObservationSpecimen2Pose = new Pose(28.25, 71.625, Math.toRadians(180)); public static final Pose blueObservationSpecimen3Pose = new Pose(30.25, 76.625, Math.toRadians(180)); diff --git a/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java b/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java index a3d4702..33f6ef7 100644 --- a/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java +++ b/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java @@ -128,7 +128,7 @@ public void pathUpdate() { if(pathTimer.getElapsedTimeSeconds() > 0.5) { auto.init(); auto.liftPIDF = false; - auto.liftManual = -0.25; + auto.liftManual = -0.15; setPathState(13); } break; case 13: //Drives to chamber once action finishes diff --git a/TeamCode/src/main/java/indubitables/opmode/pidTest.java b/TeamCode/src/main/java/indubitables/opmode/pidTest.java index d98ae05..5497955 100644 --- a/TeamCode/src/main/java/indubitables/opmode/pidTest.java +++ b/TeamCode/src/main/java/indubitables/opmode/pidTest.java @@ -61,8 +61,6 @@ public void init() { rightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); bottom = getPos(); @@ -90,11 +88,17 @@ public void loop() { telemetry.addData("pos", getPos()); telemetry.addData("baron", baron); telemetry.addData("target", target); + telemetry.addData("P:", p); + telemetry.addData("I:", i); + telemetry.addData("D:", d); + telemetry.addData("F:", f); telemetry.update(); } public void updatePIDF(){ if (!manual) { + rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); double pid = liftPID.calculate(getPos(), target); double ticks_in_degrees = 537.7 / 360.0; double ff = Math.cos(Math.toRadians(target / ticks_in_degrees)) * f;