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 6204681..0a944dc 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,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.665; + public static double spindexer_intakePos1 = 0.34; - public static double spindexer_intakePos3 = 0.29; + public static double spindexer_intakePos3 = 0.5; - public static double spindexer_intakePos2 = 0.99; + public static double spindexer_intakePos2 = 0.66; - public static double spindexer_outtakeBall3 = 0.845; + public static double spindexer_outtakeBall3 = 0.42; - public static double spindexer_outtakeBall2 = 0.48; - public static double spindexer_outtakeBall1 = 0.1; + public static double spindexer_outtakeBall2 = 0.74; + public static double spindexer_outtakeBall1 = 0.58; public static double transferServo_out = 0.15; 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 15a34d8..c023c54 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 @@ -74,7 +74,7 @@ public class IntakeTest extends LinearOpMode { colorDetect(); spindexer(); - if (b1 && steadySpin && getRuntime() - stamp > 0.5){ + if (b1 && steadySpin && getRuntime() - stamp > 1.0){ if (!b2){ if (servo.spinEqual(spindexer_intakePos1)){ spindexerPos = spindexer_intakePos2; @@ -94,8 +94,6 @@ public class IntakeTest extends LinearOpMode { } } - powPID = servo.setSpinPos(spindexerPos); - } else if (mode == 2){ // switch to this mode before switching modes powPID = 0; spindexerPos = spindexer_intakePos1; @@ -150,18 +148,15 @@ public class IntakeTest extends LinearOpMode { public void spindexer(){ if (!servo.spinEqual(spindexerPos)){ + powPID = servo.setSpinPos(spindexerPos); robot.spin1.setPower(powPID); robot.spin2.setPower(-powPID); steadySpin = false; - ticker = 0; + stamp = getRuntime(); } else{ robot.spin1.setPower(0); robot.spin2.setPower(0); steadySpin = true; - if (ticker == 0){ - stamp = getRuntime(); - } - ticker++; } } } 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 b990765..d8d0d23 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 @@ -3,28 +3,28 @@ package org.firstinspires.ftc.teamcode.tests; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.utils.Robot; //TODO: fix to get the apriltag that it is reading public class LimelightTest extends LinearOpMode { - Robot robot; MultipleTelemetry TELE; public static int pipeline = 0; //0 is for test; 1, 2, and 3 are for obelisk; 4 is for blue track; 5 is for red track public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track int obeliskPipe = 1; @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight"); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - robot.limelight.pipelineSwitch(pipeline); + limelight.pipelineSwitch(pipeline); waitForStart(); if (isStopRequested()) return; - robot.limelight.start(); + limelight.start(); while (opModeIsActive()){ if (mode == 0){ - robot.limelight.pipelineSwitch(pipeline); - LLResult result = robot.limelight.getLatestResult(); + limelight.pipelineSwitch(pipeline); + LLResult result = limelight.getLatestResult(); if (result != null) { if (result.isValid()) { TELE.addData("tx", result.getTx()); @@ -33,8 +33,8 @@ public class LimelightTest extends LinearOpMode { } } } else if (mode == 1){ - robot.limelight.pipelineSwitch(obeliskPipe); - LLResult result = robot.limelight.getLatestResult(); + limelight.pipelineSwitch(obeliskPipe); + LLResult result = limelight.getLatestResult(); if (result != null && result.isValid()){ TELE.addData("ID", obeliskPipe+20); TELE.update(); @@ -46,8 +46,8 @@ public class LimelightTest extends LinearOpMode { } } } else if (mode == 2){ - robot.limelight.pipelineSwitch(4); - LLResult result = robot.limelight.getLatestResult(); + limelight.pipelineSwitch(4); + LLResult result = limelight.getLatestResult(); if (result != null) { if (result.isValid()) { TELE.addData("tx", result.getTx()); @@ -56,8 +56,8 @@ public class LimelightTest extends LinearOpMode { } } } else if (mode == 3){ - robot.limelight.pipelineSwitch(5); - LLResult result = robot.limelight.getLatestResult(); + limelight.pipelineSwitch(5); + LLResult result = limelight.getLatestResult(); if (result != null) { if (result.isValid()) { TELE.addData("tx", result.getTx()); @@ -66,7 +66,7 @@ public class LimelightTest extends LinearOpMode { } } } else { - robot.limelight.pipelineSwitch(0); + limelight.pipelineSwitch(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 821597d..e013612 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 @@ -133,6 +133,9 @@ public class Robot { spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); + spin1.setDirection(DcMotorSimple.Direction.REVERSE); + spin2.setDirection(DcMotorSimple.Direction.REVERSE); + pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); @@ -166,7 +169,5 @@ public class Robot { color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); - - limelight = hardwareMap.get(Limelight3A.class,"Limelight"); } } 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 57ee5a2..ff6323a 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 @@ -16,10 +16,10 @@ public class Servos { //PID constants // TODO: get PIDF constants - public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03; + public static double spinP = 1.8, spinI = 0, spinD = 0.03, spinF = 0.03; 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_scalar = 1.0086; public static double spin_restPos = 0.0; public static double turret_scalar = 1.009; public static double turret_restPos = 0.0;