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 8a2ddf5..798cf86 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 @@ -105,7 +105,6 @@ public class TeleopV3 extends LinearOpMode { limelightUsed = true; robot.light.setPosition(1); - while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { @@ -265,7 +264,7 @@ public class TeleopV3 extends LinearOpMode { spindexer.prepareShootAllContinous(); //TELE.addLine("preparing to shoot"); // } else if (shooterTicker == 2) { -// //robot.transferServo.setPosition(transferServo_in); +// //servo.setTransferPos(transferServo_in); // spindexer.shootAll(); // TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { 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 9a07420..7cbd38a 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 @@ -12,6 +12,7 @@ public class Flywheel { public double velo1 = 0.0; public double velo2 = 0.0; double targetVelocity = 0.0; + double previousTargetVelocity = 0.0; double powPID = 0.0; boolean steady = false; public Flywheel (HardwareMap hardwareMap) { @@ -55,20 +56,21 @@ public class Flywheel { private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;} public double manageFlywheel(double commandedVelocity) { - // Add code here to set PIDF based on desired RPM - if (targetVelocity != commandedVelocity){ - robot.shooter1.setVelocity(RPM_to_TPS(commandedVelocity)); - robot.shooter2.setVelocity(RPM_to_TPS(commandedVelocity)); + if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { + targetVelocity = commandedVelocity; + // Add code here to set PIDF based on desired RPM + //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + + // Record Current Velocity + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + velo = Math.max(velo1, velo2); + } - - targetVelocity = commandedVelocity; - - // Record Current Velocity - velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); - velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = Math.max(velo1,velo2); - // really should be a running average of the last 5 steady = (Math.abs(targetVelocity - velo) < 200.0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index e150405..c66ff79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -91,4 +91,8 @@ public class Servos { public double setTurrPos(double pos) { return 1.0; } + + public boolean turretEqual(double pos) { + return true; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index a00b76b..30b5af6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -11,6 +11,7 @@ import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; @@ -38,7 +39,7 @@ public class Turret { public static double clampTolerance = 0.03; //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; - public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090; + public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; @@ -56,6 +57,9 @@ public class Turret { private double permanentOffset = 0.0; private PIDController bearingPID; + private double prevTurretPos = 0.0; + private boolean firstTurretPos = true; + public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; this.robot = rob;