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 50cc8cc..7aa9159 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,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double rapidFireBlocker_Closed = 0.3; + public static double rapidFireBlocker_Closed = 0.35; public static double rapidFireBlocker_Open = 0.5; public static double spindexBlocker_Closed = 0.31; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index b86eb4c..a061c24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -25,6 +25,7 @@ public class TeleopV4 extends LinearOpMode { MultipleTelemetry TELE; Follower follower; SpindexerTransferIntake spindexerTransferIntake; + @Override public void runOpMode() throws InterruptedException { @@ -49,7 +50,7 @@ public class TeleopV4 extends LinearOpMode { if (isStopRequested()) return; - while (opModeIsActive()){ + while (opModeIsActive()) { //Drivetrain @@ -93,9 +94,6 @@ public class TeleopV4 extends LinearOpMode { } - - - TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java new file mode 100644 index 0000000..fafc88c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode.tests; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.constants.ServoPositions; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Shooter; + +@Config +@TeleOp +public class NewShooterTest extends LinearOpMode { + + Robot robot; + + public static boolean intake = true; + public static boolean shoot = false; + public static double intakePower = 1.0; + public static double transferShootPower = -1; + public static double transferIntakePower = -1.0; + public static double turretPos = 0.51; + public static double hoodPos = 0.51; + public static double flywheel = 0; + + private enum ShootState { + IDLE, + WAIT_GATE, + WAIT_PUSH + } + + private ShootState shootState = ShootState.IDLE; + private long timestamp = 0; + + @Override + public void runOpMode() throws InterruptedException { + + Robot.resetInstance(); + + robot = Robot.getInstance(hardwareMap); + + Shooter shooter = new Shooter( + robot, + new MultipleTelemetry( + telemetry, + FtcDashboard.getInstance().getTelemetry() + ), + Constants.createFollower(hardwareMap), + true + ); + + shooter.setState(Shooter.ShooterState.MANUAL); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + + robot.setHoodPos(hoodPos); + shooter.setTurretPosition(turretPos); + shooter.setFlywheelVelocity(flywheel); + robot.setSpinPos(ServoPositions.spindexer_A2); + + if (intake && !shoot) { + + shootState = ShootState.IDLE; + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Closed); + + robot.setTransferPower(transferIntakePower); + robot.setIntakePower(intakePower); + robot.setTransferServoPos( + ServoPositions.transferServo_out); + } + + else if (shoot) { + robot.setIntakePower(intakePower); + + + switch (shootState) { + + case IDLE: + + robot.setTransferPower(transferShootPower); + + timestamp = System.currentTimeMillis(); + shootState = ShootState.WAIT_GATE; + + break; + + case WAIT_GATE: + + if (System.currentTimeMillis() - timestamp >= 300) { + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open); + + timestamp = System.currentTimeMillis(); + shootState = ShootState.WAIT_PUSH; + } + + break; + + case WAIT_PUSH: + + if (System.currentTimeMillis() - timestamp >= 100) { + + robot.setTransferServoPos( + ServoPositions.transferServo_in); + + shootState = ShootState.IDLE; + } + + break; + } + } + + shooter.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 58a54d4..224cdca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -125,7 +125,7 @@ public class Robot { spin2 = hardwareMap.get(Servo.class, "spin2"); transfer = hardwareMap.get(DcMotorEx.class, "transfer"); - transfer.setDirection(DcMotorSimple.Direction.REVERSE); + transferServo = hardwareMap.get(Servo.class, "transferServo");