diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index 56267d6..443468c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -163,7 +163,9 @@ public class TeleopV2 extends LinearOpMode { robot.turr1.setPower(turretPID); robot.turr2.setPower(-turretPID); + //TODO: make sure changing position works throughout opmode if (!servo.spinEqual(spindexPos)){ + spindexPID = servo.setSpinPos(spindexPos); robot.spin1.setPower(spindexPID); robot.spin2.setPower(-spindexPID); } else{ @@ -320,6 +322,7 @@ public class TeleopV2 extends LinearOpMode { offset -= 360; } + //TODO: test the camera teleop code double pos = turrDefault + (error/8); // adds the overall error to the default TELE.addData("offset", offset); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index 1271584..15a34d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -20,7 +20,7 @@ public class IntakeTest extends LinearOpMode { Servos servo; public static int mode = 0; // 0 for teleop, 1 for auto - public static double manualPow = 0.5; + public static double manualPow = 1.0; double stamp = 0; int ticker = 0; boolean b1 = false; @@ -40,6 +40,7 @@ public class IntakeTest extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()) { + //TODO: test tele intake with new spindexer if (mode == 0) { if (gamepad1.cross) { ticker = 0; @@ -59,6 +60,7 @@ public class IntakeTest extends LinearOpMode { robot.intake.setPower(0); } } + //TODO: test this monstrosity } else if (mode == 1) { if (gamepad1.cross){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 3fe29b3..b990765 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -6,9 +6,7 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.utils.Robot; - -import java.util.List; - +//TODO: fix to get the apriltag that it is reading public class LimelightTest extends LinearOpMode { Robot robot; MultipleTelemetry TELE; @@ -20,7 +18,6 @@ public class LimelightTest extends LinearOpMode { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); robot.limelight.pipelineSwitch(pipeline); - waitForStart(); if (isStopRequested()) return; robot.limelight.start(); 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 a2d82e6..57ee5a2 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 @@ -15,8 +15,9 @@ public class Servos { PIDFController turretPID; //PID constants + // TODO: get PIDF constants public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03; - public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF; + public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0; public static double spin_scalar = 1.011; public static double spin_restPos = 0.0; @@ -34,7 +35,7 @@ public class Servos { public double getSpinPos() { return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); } - + //TODO: PID warp so 0 and 1 are usable positions public double setSpinPos(double pos) { spinPID.setPIDF(spinP, spinI, spinD, spinF);