diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 826a7dc..c6eebf2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -821,7 +821,7 @@ public class Auto_LT_Far extends LinearOpMode { startAuto(); if (stack3){ - cycleStackFar(); + //cycleStackFar(); } if (gatePickup || stack3){ @@ -878,61 +878,61 @@ public class Auto_LT_Far extends LinearOpMode { Actions.runBlocking(leaveFromShoot.build()); } - void cycleStackFar(){ - Actions.runBlocking( - new ParallelAction( - pickup3.build(), - manageShooterAuto( - intake3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - intake(intake3Time), - detectObelisk( - intake3Time, - 0.501, - 0.501, - 0.501, - 0.501, - obeliskTurrPos3 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot3.build(), - prepareShootAll(colorSenseTime, shoot3Time, motif) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - finalShootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) - ) - - ); - } +// void cycleStackFar(){ +// Actions.runBlocking( +// new ParallelAction( +// pickup3.build(), +// manageShooterAuto( +// intake3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// intake(intake3Time), +// detectObelisk( +// intake3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501, +// obeliskTurrPos3 +// ) +// +// ) +// ); +// +// motif = turret.getObeliskID(); +// +// if (motif == 0) motif = prevMotif; +// prevMotif = motif; +// +// Actions.runBlocking( +// new ParallelAction( +// manageFlywheelAuto( +// shoot3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shoot3.build(), +// prepareShootAll(colorSenseTime, shoot3Time, motif) +// ) +// ); +// +// Actions.runBlocking( +// new ParallelAction( +// manageShooterAuto( +// finalShootAllTime, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) +// ) +// +// ); +// } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 798cf86..15770f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -118,6 +118,7 @@ public class TeleopV3 extends LinearOpMode { if (isStopRequested()) return; servo.setTransferPos(transferServo_out); + robot.transfer.setPower(1); while (opModeIsActive()) { @@ -141,8 +142,6 @@ public class TeleopV3 extends LinearOpMode { } - robot.transfer.setPower(1); - //TURRET TRACKING double robX = drive.localizer.getPose().position.x; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index 7cbd38a..b5e017f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -38,6 +38,8 @@ public class Flywheel { } // Set the robot PIDF for the next cycle. + private double prevF = 0.501; + public static int voltagePIDFDifference = 5; public void setPIDF(double p, double i, double d, double f) { shooterPIDF1.p = p; shooterPIDF1.i = i; @@ -47,6 +49,10 @@ public class Flywheel { shooterPIDF2.i = i; shooterPIDF2.d = d; shooterPIDF2.f = f; + if (Math.abs(prevF - f) > voltagePIDFDifference){ + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + } } // Convert from RPM to Ticks per Second