diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index d1566e7..ec4f470 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -2,6 +2,12 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; @@ -35,22 +41,10 @@ public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; public static double spindexerSpeedIncrease = 0.014; - // These values are ADDED to turrDefault - public static double redObeliskTurrPos1 = 0.12; - public static double redObeliskTurrPos2 = 0.13; - public static double redObeliskTurrPos3 = 0.14; - public static double blueObeliskTurrPos1 = -0.12; - public static double blueObeliskTurrPos2 = -0.13; - public static double blueObeliskTurrPos3 = -0.14; - public static double redTurretShootPos = 0.1; - public static double blueTurretShootPos = -0.14; - double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; - double turretShootPos = 0.0; - public static double shootAllTime = 2; public static double intake1Time = 3.3; public static double intake2Time = 3.8; @@ -97,8 +91,6 @@ public class Auto_LT_Close extends LinearOpMode { double xPrep, yPrep, hPrep; double xLeave, yLeave, hLeave; - private double shoot1Tangent; - int ballCycles = 3; int prevMotif = 0; @@ -218,8 +210,6 @@ public class Auto_LT_Close extends LinearOpMode { obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; - turretShootPos = turrDefault + redTurretShootPos; - } else { robot.light.setPosition(0.6); @@ -261,8 +251,6 @@ public class Auto_LT_Close extends LinearOpMode { obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; - turretShootPos = turrDefault + blueTurretShootPos; - } shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) 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 e0a2841..e5b964c 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 @@ -3,6 +3,14 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueTurretShootPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; @@ -34,8 +42,6 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Far extends LinearOpMode { public static double shoot0Vel = 3300, shoot0Hood = 0.48; - public static double redTurretShootPos = 0.05; - public static double blueTurretShootPos = -0.05; double xLeave, yLeave, hLeave; public int motif = 0; double turretShootPos = 0.0; @@ -70,12 +76,6 @@ public class Auto_LT_Far extends LinearOpMode { public static double colorSenseTime = 1; public static double intakeStackTime = 2.5; public static double intakeGateTime = 2; - public static double redObeliskTurrPos1 = 0.12; - public static double redObeliskTurrPos2 = 0.13; - public static double redObeliskTurrPos3 = 0.14; - public static double blueObeliskTurrPos1 = -0.12; - public static double blueObeliskTurrPos2 = -0.13; - public static double blueObeliskTurrPos3 = -0.14; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 7998cdc..8f22525 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.autonomous.actions; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -10,7 +9,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import androidx.annotation.NonNull; @@ -51,10 +49,8 @@ public class AutoActions{ private int mostGreenSlot = 0; private double firstSpindexShootPos = spindexer_outtakeBall1; private boolean shootForward = true; - public static double firstShootTime = 0.3; public int motif = 0; double spinEndPos = ServoPositions.spinEndPos; - int waitFirstBallTicks = 4; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){ this.robot = rob; @@ -248,7 +244,7 @@ public class AutoActions{ end = prevSpinPos < spinEndPos; } - if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < waitFirstBallTicks+1)) { + if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) { if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) { servos.setTransferPos(transferServo_out); @@ -256,10 +252,10 @@ public class AutoActions{ } else { servos.setTransferPos(transferServo_in); shooterTicker++; - - if (shootForward && shooterTicker > waitFirstBallTicks) { + Spindexer.whileShooting = true; + if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) { servos.setSpinPos(prevSpinPos + spindexSpeed); - } else if (shooterTicker > waitFirstBallTicks){ + } else if (shooterTicker > Spindexer.waitFirstBallTicks){ servos.setSpinPos(prevSpinPos - spindexSpeed); } @@ -269,7 +265,7 @@ public class AutoActions{ } else { servos.setTransferPos(transferServo_out); - + Spindexer.whileShooting = false; spindexer.resetSpindexer(); spindexer.processIntake(); @@ -416,7 +412,7 @@ public class AutoActions{ deltaPose = new Pose2d(robotX, robotY, robotHeading); } - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); +// double distanceToGoal = Math.sqrt(dx * dx + dy * dy); targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 1b48cdb..6dd1be9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; @Config public class Back_Poses { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 1b422ee..a6892d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.07; //0.13; + public static double spindexer_intakePos1 = 0.06; //0.13; - public static double spindexer_intakePos2 = 0.27; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.25; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.46; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24; public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24; @@ -39,7 +39,7 @@ public class ServoPositions { public static double blueObeliskTurrPos1 = -0.12; public static double blueObeliskTurrPos2 = -0.13; public static double blueObeliskTurrPos3 = -0.14; - public static double redTurretShootPos = 0.1; - public static double blueTurretShootPos = -0.14; + public static double redTurretShootPos = 0.05; + public static double blueTurretShootPos = -0.05; } 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 59779be..8bd6b8f 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 @@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; @@ -46,9 +45,9 @@ public class TeleopV3 extends LinearOpMode { public double vel = 3000; public boolean autoVel = true; public boolean targetingHood = true; - public boolean autoHood = true; +// public boolean autoHood = true; public double shootStamp = 0.0; - boolean fixedTurret = false; +// boolean fixedTurret = false; Robot robot; MultipleTelemetry TELE; Light light; @@ -66,13 +65,13 @@ public class TeleopV3 extends LinearOpMode { boolean reject = false; double xOffset = 0.0; double yOffset = 0.0; - double headingOffset = 0.0; +// double headingOffset = 0.0; int ticker = 0; - boolean autoSpintake = false; +// boolean autoSpintake = false; boolean enableSpindexerManager = true; - boolean overrideTurr = false; +// boolean overrideTurr = false; int intakeTicker = 0; private boolean shootAll = false; @@ -178,7 +177,7 @@ public class TeleopV3 extends LinearOpMode { double dy = robotY - goalY; // delta y from robot to goal Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); +// double distanceToGoal = Math.sqrt(dx * dx + dy * dy); targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, turretInterpolate); @@ -262,7 +261,6 @@ public class TeleopV3 extends LinearOpMode { spindexer.setIntakePower(1); } else if (gamepad1.cross) { spindexer.setIntakePower(-1); - } else { spindexer.setIntakePower(0); } @@ -288,11 +286,7 @@ public class TeleopV3 extends LinearOpMode { // //servo.setTransferPos(transferServo_in); // spindexer.shootAll(); // TELE.addLine("starting to shoot"); - } else if (!spindexer.shootAllComplete()) { - servo.setTransferPos(transferServo_in); - //TELE.addLine("shoot"); - } else { - servo.setTransferPos(transferServo_out); + } else if (spindexer.shootAllComplete()) { //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); 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 0af4300..b506329 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,15 +12,13 @@ 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) { robot = new Robot(hardwareMap); shooterPIDF1 = new PIDFCoefficients - (robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F); + (Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); shooterPIDF2 = new PIDFCoefficients - (robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F); + (Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); } public double getVelo () { @@ -52,6 +50,7 @@ public class Flywheel { if (Math.abs(prevF - f) > voltagePIDFDifference){ robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + prevF = f; } } @@ -61,7 +60,7 @@ public class Flywheel { // Convert from Ticks per Second to RPM private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;} - public double manageFlywheel(double commandedVelocity) { + public void manageFlywheel(double commandedVelocity) { if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { targetVelocity = commandedVelocity; @@ -78,7 +77,6 @@ public class Flywheel { // really should be a running average of the last 5 steady = (Math.abs(commandedVelocity - velo) < 50); - return powPID; } public void update() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java index d7d0d1e..b55806d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java @@ -7,10 +7,10 @@ public class MeasuringLoopTimes { private double minLoopTime = 999999999999.0; private double maxLoopTime = 0.0; - private double mainLoopTime = 0.0; + double mainLoopTime = 0.0; private double MeasurementStart = 0.0; - private double currentTime = 0.0; + double currentTime = 0.0; private double avgLoopTime = 0.0; private int avgLoopTimeTicker = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 361d03f..98ca41c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -1,11 +1,9 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.config.Config; -import com.arcrobotics.ftclib.hardware.ServoEx; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -35,7 +33,7 @@ public class Robot { public static double shooterPIDF_I = 0.0; public static double shooterPIDF_D = 0.0; public static double shooterPIDF_F = 75; - public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; +// public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; public Servo hood; 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 2aed644..8f6d885 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 @@ -14,8 +14,6 @@ public class Servos { public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0; public static double spin_scalar = 1.112; public static double spin_restPos = 0.155; - public static double turret_scalar = 1.009; - public static double turret_restPos = 0.0; Robot robot; PIDFController spinPID; PIDFController turretPID; @@ -51,14 +49,13 @@ public class Servos { return (Math.abs(pos1 - pos2) < 0.005); } - public double setTransferPos(double pos) { + public void setTransferPos(double pos) { if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) { robot.transferServo.setPosition(pos); firstTransferPos = false; } prevTransferPos = pos; - return pos; } public double setSpinPos(double pos) { @@ -72,29 +69,16 @@ public class Servos { return pos; } - public double setHoodPos(double pos){ + public void setHoodPos(double pos){ if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) { robot.hood.setPosition(pos + hoodOffset); firstHoodPos = false; } prevHoodPos = pos; - return pos; } public boolean spinEqual(double pos) { return Math.abs(pos - this.getSpinPos()) < 0.03; } - - public double getTurrPos() { - return 1.0; - } - - 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/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 5edc576..f8f0d5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -16,6 +16,8 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; import static org.firstinspires.ftc.teamcode.utils.Servos.spinF; import static org.firstinspires.ftc.teamcode.utils.Servos.spinI; @@ -52,6 +54,9 @@ public class Spindexer { private double prevPos = 0.0; public double spindexerPosOffset = 0.00; public static int shootWaitMax = 4; + public static boolean whileShooting = false; + public static int waitFirstBallTicks = 4; + private int shootTicks = 0; public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE; // For Use enum RotatedBallPositionNames { @@ -530,18 +535,26 @@ public class Spindexer { break; case SHOOT_PREP_CONTINOUS: - if (servos.spinEqual(spinStartPos)){ + if (shootTicks > waitFirstBallTicks){ currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; + shootTicks++; + } else if (servos.spinEqual(spinStartPos)){ + shootTicks++; + servos.setTransferPos(transferServo_in); } else { servos.setSpinPos(spinStartPos); } break; case SHOOT_CONTINOUS: + whileShooting = true; ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; if (servos.getSpinPos() > spinEndPos){ + whileShooting = false; + servos.setTransferPos(transferServo_out); + shootTicks = 0; currentIntakeState = IntakeState.FINDNEXT; } else { double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; 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 909ecd6..0b5a945 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 @@ -10,11 +10,9 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; -import org.firstinspires.ftc.teamcode.constants.StateEnums; import java.util.List; @@ -32,13 +30,11 @@ public class Turret { public static double manualOffset = 0.0; - public static double visionCorrectionGain = 0.08; // Single tunable gain - public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle - public static double cameraBearingEqual = 0.5; // Deadband +// public static double visionCorrectionGain = 0.08; // Single tunable gain +// public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle +// public static double cameraBearingEqual = 0.5; // Deadband - // TODO: tune these values for limelight - - public static double clampTolerance = 0.03; +// 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.08, B_PID_I = 0.0, B_PID_D = 0.007; Robot robot; @@ -49,7 +45,8 @@ public class Turret { double limelightPosX = 0.0; double limelightPosY = 0.0; LLResult result; - + public static double TARGET_POSITION_TOLERANCE = 0.5; + public static double COLOR_OK_TOLERANCE = 2; boolean bearingAligned = false; private boolean lockOffset = false; private int obeliskID = 0; @@ -57,12 +54,9 @@ public class Turret { private double currentTrackOffset = 0.0; private double lightColor = Color.LightRed; private int currentTrackCount = 0; - private double permanentOffset = 0.0; + double permanentOffset = 0.0; private int prevPipeline = -1; - private PIDController bearingPID; - - private double prevTurretPos = 0.0; - private boolean firstTurretPos = true; + PIDController bearingPID; public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; @@ -168,16 +162,16 @@ public class Turret { /* Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading */ - + private double targetTx = 0; + public static double alphaTX = 0.5; private double bearingAlign(LLResult llResult) { double bearingOffset = 0.0; - double targetTx = llResult.getTx(); // How far left or right the target is (degrees) - final double MIN_OFFSET_POWER = 0.15; - final double TARGET_POSITION_TOLERANCE = 1.0; - // LL has 54.5 degree total Horizontal FOV; very edges are not useful. - final double HORIZONTAL_FOV_RANGE = 26.0; // Total usable horizontal degrees from center +/- - final double DRIVE_POWER_REDUCTION = 2.0; - final double COLOR_OK_TOLERANCE = 2.5; + double tx = llResult.getTx(); // How far left or right the target is (degrees) + targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX)); +// final double MIN_OFFSET_POWER = 0.15; +// // LL has 54.5 degree total Horizontal FOV; very edges are not useful. +// final double HORIZONTAL_FOV_RANGE = 26.0; // Total usable horizontal degrees from center +/- +// final double DRIVE_POWER_REDUCTION = 2.0; if (abs(targetTx) < TARGET_POSITION_TOLERANCE) { bearingAligned = true; @@ -293,7 +287,7 @@ public class Turret { targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax)); // Interpolate towards target position - double currentPos = getTurrPos(); +// double currentPos = getTurrPos(); double turretPos = targetTurretPos; if (targetTurretPos == turrMin) { @@ -303,7 +297,9 @@ public class Turret { } // Set servo positions - setTurret(turretPos + manualOffset); + if (!Spindexer.whileShooting || abs(targetTx) > COLOR_OK_TOLERANCE){ + setTurret(turretPos + manualOffset); + } /* ---------------- TELEMETRY ---------------- */