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 14e2371..a755589 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.19; + public static double spindexer_intakePos1 = 0.18; - public static double spindexer_intakePos2 = 0.35;//0.5; + public static double spindexer_intakePos2 = 0.36;//0.5; - public static double spindexer_intakePos3 = 0.51;//0.66; + public static double spindexer_intakePos3 = 0.54;//0.66; public static double spindexer_outtakeBall3 = 0.47; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 789f60e..f188686 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; @@ -12,12 +13,13 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Spindexer; @Config @TeleOp public class ShooterTest extends LinearOpMode { - public static int mode = 0; + public static int mode = 1; public static double parameter = 0.0; // --- CONSTANTS YOU TUNE --- @@ -31,9 +33,18 @@ public class ShooterTest extends LinearOpMode { public static double hoodPos = 0.501; public static double turretPos = 0.501; public static boolean shoot = false; + + public static boolean intake = false; Robot robot; Flywheel flywheel; + double shootStamp = 0.0; + boolean shootAll = false; + + public double spinPow = 0.09; + + Spindexer spindexer ; + @Override public void runOpMode() throws InterruptedException { @@ -41,6 +52,7 @@ public class ShooterTest extends LinearOpMode { DcMotorEx leftShooter = robot.shooter1; DcMotorEx rightShooter = robot.shooter2; flywheel = new Flywheel(hardwareMap); + spindexer = new Spindexer(hardwareMap); MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() @@ -64,12 +76,53 @@ public class ShooterTest extends LinearOpMode { robot.hood.setPosition(hoodPos); } - robot.transfer.setPower(transferPower); - if (shoot) { - robot.transferServo.setPosition(transferServo_in); + if (intake) { + robot.intake.setPower(1); + } else { - robot.transferServo.setPosition(transferServo_out); + robot.intake.setPower(0); } + + + if (shoot) { + shootStamp = getRuntime(); + shootAll = true; + shoot = false; + robot.transfer.setPower(transferPower); + } + if (shootAll) { + + //intake = false; + //reject = false; + + // TODO: Change starting position based on desired order to shoot green ball + //spindexPos = spindexer_intakePos1; + + if (getRuntime() - shootStamp < 3.5) { + + robot.transferServo.setPosition(transferServo_in); + + robot.spin1.setPower(-spinPow); + robot.spin2.setPower(spinPow); + + } else { + robot.transferServo.setPosition(transferServo_out); + //spindexPos = spindexer_intakePos1; + + shootAll = false; + + robot.transferServo.setPosition(transferServo_out); + robot.transfer.setPower(0); + robot.spin1.setPower(0); + robot.spin2.setPower(0); + + spindexer.resetSpindexer(); + spindexer.processIntake(); + } + } else { + spindexer.processIntake(); + } + TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Velocity 1", flywheel.getVelo1()); TELE.addData("Velocity 2", flywheel.getVelo2()); 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 8a85cac..d3af293 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,10 +38,14 @@ public class Flywheel { // Set the robot PIDF for the next cycle. public void setPIDF(double p, double i, double d, double f) { - robot.shooterPIDF.p = p; - robot.shooterPIDF.i = i; - robot.shooterPIDF.d = d; - robot.shooterPIDF.f = f; + shooterPIDF1.p = p; + shooterPIDF1.i = i; + shooterPIDF1.d = d; + shooterPIDF1.f = f; + shooterPIDF2.p = p; + shooterPIDF2.i = i; + shooterPIDF2.d = d; + shooterPIDF2.f = f; } // Convert from RPM to Ticks per Second 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 9dcc104..347b850 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 @@ -21,7 +21,7 @@ public class Robot { //Initialize Public Components public static boolean usingLimelight = true; - public static boolean usingCamera = true; + public static boolean usingCamera = false; public DcMotorEx frontLeft; public DcMotorEx frontRight; public DcMotorEx backLeft; 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 f145927..f185ac0 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 @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Servos { //PID constants // TODO: get PIDF constants - public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02; + public static double spinP = 2.0, spinI = 0, spinD = 0.3, spinF = 0.02; public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0; public static double spin_scalar = 1.0086; public static double spin_restPos = 0.0; @@ -40,7 +40,7 @@ public class Servos { } public boolean spinEqual(double pos) { - return Math.abs(pos - this.getSpinPos()) < 0.02; + return Math.abs(pos - this.getSpinPos()) < 0.03; } public double getTurrPos() { 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 60616b5..9ffa9f7 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 @@ -277,7 +277,6 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.UNKNOWN_DETECT; stopSpindexer(); - detectBalls(true, true); unknownColorDetect = 0; } else { // Keep moving the spindexer @@ -288,7 +287,7 @@ public class Spindexer { if (unknownColorDetect >5) { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } else { - detectBalls(true, true); + //detectBalls(true, true); unknownColorDetect++; } break; @@ -343,7 +342,7 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); - detectBalls(false, false); + //detectBalls(false, false); } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]); @@ -374,7 +373,7 @@ public class Spindexer { case SHOOT_ALL_READY: // Double Check Colors - detectBalls(false, false); // Minimize hardware calls + //detectBalls(false, false); // Minimize hardware calls if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { // All ball shot move to intake state currentIntakeState = Spindexer.IntakeState.FINDNEXT; @@ -422,7 +421,7 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); - detectBalls(true, false); + //detectBalls(true, false); } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]);