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 ff30df7..3e87944 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 @@ -16,7 +16,6 @@ public class ServoPositions { public static double transferServo_out = 0.13; public static double transferServo_in = 0.4; - public static double transferServoPos = 0.5; public static double hoodDefault = 0.35; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index 602e3ef..72120f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -244,12 +244,5 @@ public class Shooter implements Subsystem { } - if (Objects.equals(turretMode, "MANUAL")) { - hoodServo.setPosition(hoodPos); - - moveTurret(turretPos); - - } - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 5f1c19c..0a55e85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -210,7 +210,6 @@ public class TeleopV1 extends LinearOpMode { TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); TELE.addData("off", offset); - robot.transferServo.setPosition(transferServoPos); robot.hood.setPosition(pos); 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 e35a6d3..934106f 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,7 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -44,6 +46,8 @@ public class ShooterTest extends LinearOpMode { public static int tolerance = 300; + public static boolean shoot = false; + MultipleTelemetry TELE; @Override @@ -77,14 +81,10 @@ public class ShooterTest extends LinearOpMode { shooter.setManualPower(pow); - if (hoodPos != 0.501) { - robot.hood.setPosition(hoodPos); - } + robot.hood.setPosition(hoodPos); + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(turretPos); - if (turretPos != 0.501) { - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(turretPos); - } velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); stamp1 = getRuntime(); initPos1 = shooter.getECPRPosition(); @@ -96,6 +96,13 @@ public class ShooterTest extends LinearOpMode { powPID = powPID - 0.001; } shooter.setVelocity(powPID); + robot.transfer.setPower((powPID / 2) + 0.5); + + if (shoot){ + robot.transferServo.setPosition(transferServo_in); + } else { + robot.transferServo.setPosition(transferServo_out); + } shooter.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index 53d7060..e6edc8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java @@ -9,11 +9,11 @@ import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; @TeleOp public class ConfigureColorRangefinder extends LinearOpMode { - public static int LED_Brightness = 35; //1 - 35, 2 - 50, + public static int LED_Brightness = 50; - public static int lowerGreen = 90; + public static int lowerGreen = 100; - public static int higherGreen = 160; + public static int higherGreen = 150; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java new file mode 100644 index 0000000..019ecce --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +@Config +public class PositionalServoProgrammer extends LinearOpMode { + Robot robot; + public static double spindexPos = 0.501; + public static double turretPos = 0.501; + public static double transferPos = 0.501; + public static double hoodPos = 0.501; + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + waitForStart(); + if (isStopRequested()) return; + while (opModeIsActive()){ + if (spindexPos != 0.501){ + robot.spin1.setPosition(spindexPos); + robot.spin2.setPosition(1-spindexPos); + } + if (turretPos != 0.501){ + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(1-turretPos); + } + if (transferPos != 0.501){ + robot.transferServo.setPosition(transferPos); + } + if (hoodPos != 0.501){ + robot.hood.setPosition(hoodPos); + } + } + } +}