KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

implemented loop time efficiencies and added turret tracking to shooter test

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-07 19:02:41 -06:00
parent 50060d3812
commit ef1c7b0e6b
11 changed files with 274 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -213,8 +213,7 @@ public class Auto_LT_Close extends LinearOpMode {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
} }
ticker++; ticker++;
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
@@ -246,8 +245,7 @@ public class Auto_LT_Close extends LinearOpMode {
spindexerWiggle *= -1.0; spindexerWiggle *= -1.0;
robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle);
spindexer.detectBalls(true, true); spindexer.detectBalls(true, true);
@@ -323,8 +321,7 @@ public class Auto_LT_Close extends LinearOpMode {
// TELE.update(); // TELE.update();
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
robot.spin1.setPosition(firstSpindexShootPos); servos.setSpinPos(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
return true; return true;
} else { } else {
@@ -390,20 +387,18 @@ public class Auto_LT_Close extends LinearOpMode {
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos); servos.setSpinPos(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
} else { } else {
robot.transferServo.setPosition(transferServo_in); servos.setTransferPos(transferServo_in);
shooterTicker++; shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); double prevSpinPos = servos.getSpinCmdPos();
robot.spin1.setPosition(prevSpinPos + spindexSpeed); servos.setSpinPos(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
} }
return true; return true;
} else { } else {
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
@@ -471,27 +466,24 @@ public class Auto_LT_Close extends LinearOpMode {
if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) { if (getRuntime() - stamp < firstShootTime) {
robot.transferServo.setPosition(transferServo_in); servos.setTransferPos(transferServo_out);
robot.spin1.setPosition(firstSpindexShootPos); servos.setSpinPos(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
} else { } else {
robot.transferServo.setPosition(transferServo_in); servos.setTransferPos(transferServo_in);
shooterTicker++; shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) { if (shootForward) {
robot.spin1.setPosition(prevSpinPos + spindexSpeed); servos.setSpinPos(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
} else { } else {
robot.spin1.setPosition(prevSpinPos - spindexSpeed); servos.setSpinPos(prevSpinPos - spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed);
} }
} }
return true; return true;
} else { } else {
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
@@ -563,8 +555,7 @@ public class Auto_LT_Close extends LinearOpMode {
ticker++; ticker++;
motif = turret.detectObelisk(); motif = turret.detectObelisk();
robot.turr1.setPosition(turrPos); turret.setTurret(turrPos);
robot.turr2.setPosition(1 - turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -627,7 +618,7 @@ public class Auto_LT_Close extends LinearOpMode {
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel); flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos); servos.setHoodPos(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -695,7 +686,7 @@ public class Auto_LT_Close extends LinearOpMode {
turret.trackGoal(deltaPose); turret.trackGoal(deltaPose);
robot.hood.setPosition(targetingSettings.hoodAngle); servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
@@ -766,7 +757,7 @@ public class Auto_LT_Close extends LinearOpMode {
targetingSettings = targeting.calculateSettings targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false); (robotX, robotY, robotHeading, 0.0, false);
robot.hood.setPosition(targetingSettings.hoodAngle); servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
@@ -819,14 +810,13 @@ public class Auto_LT_Close extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight); turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(turrDefault); turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
robot.spin1.setPosition(autoSpinStartPos); servos.setSpinPos(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
limelightUsed = false; limelightUsed = false;
@@ -834,8 +824,8 @@ public class Auto_LT_Close extends LinearOpMode {
while (opModeInInit()) { while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood); servos.setHoodPos(shoot0Hood);
turret.manualSetTurret(turrDefault); turret.setTurret(turrDefault);
if (gamepad2.crossWasPressed()) { if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance; redAlliance = !redAlliance;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java
View File

@@ -169,7 +169,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos); turret.setTurret(turretShootPos);
robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
drive.updatePoseEstimate(); drive.updatePoseEstimate();
@@ -611,7 +611,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight); turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4); turret.setTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java
View File

@@ -207,7 +207,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
ticker++; ticker++;
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
turret.manualSetTurret(turretShootPos); turret.setTurret(turretShootPos);
drive.updatePoseEstimate(); drive.updatePoseEstimate();
@@ -752,7 +752,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight); turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(0.4); turret.setTurret(0.4);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -97,8 +97,7 @@ public class Auto_LT_Far extends LinearOpMode {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
} }
ticker++; ticker++;
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
drive.updatePoseEstimate(); drive.updatePoseEstimate();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
@@ -130,8 +129,7 @@ public class Auto_LT_Far extends LinearOpMode {
spindexerWiggle *= -1.0; spindexerWiggle *= -1.0;
robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle);
spindexer.detectBalls(true, true); spindexer.detectBalls(true, true);
@@ -207,8 +205,7 @@ public class Auto_LT_Far extends LinearOpMode {
// TELE.update(); // TELE.update();
robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
robot.spin1.setPosition(firstSpindexShootPos); servos.setSpinPos(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
return true; return true;
} else { } else {
@@ -274,20 +271,18 @@ public class Auto_LT_Far extends LinearOpMode {
if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
robot.spin1.setPosition(autoSpinStartPos); servos.setSpinPos(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
} else { } else {
robot.transferServo.setPosition(transferServo_in); servos.setTransferPos(transferServo_in);
shooterTicker++; shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); double prevSpinPos = servos.getSpinCmdPos();
robot.spin1.setPosition(prevSpinPos + spindexSpeed); servos.setSpinPos(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
} }
return true; return true;
} else { } else {
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
@@ -355,27 +350,24 @@ public class Auto_LT_Far extends LinearOpMode {
if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < shootTime) {
if (getRuntime() - stamp < firstShootTime) { if (getRuntime() - stamp < firstShootTime) {
robot.transferServo.setPosition(transferServo_in); servos.setTransferPos(transferServo_out);
robot.spin1.setPosition(firstSpindexShootPos); servos.setSpinPos(firstSpindexShootPos);
robot.spin2.setPosition(1 - firstSpindexShootPos);
} else { } else {
robot.transferServo.setPosition(transferServo_in); servos.setTransferPos(transferServo_in);
shooterTicker++; shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); double prevSpinPos = servos.getSpinCmdPos();
if (shootForward) { if (shootForward) {
robot.spin1.setPosition(prevSpinPos + spindexSpeed); servos.setSpinPos(prevSpinPos + spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
} else { } else {
robot.spin1.setPosition(prevSpinPos - spindexSpeed); servos.setSpinPos(prevSpinPos - spindexSpeed);
robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed);
} }
} }
return true; return true;
} else { } else {
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); spindexer.processIntake();
@@ -447,8 +439,7 @@ public class Auto_LT_Far extends LinearOpMode {
ticker++; ticker++;
motif = turret.detectObelisk(); motif = turret.detectObelisk();
robot.turr1.setPosition(turrPos); turret.setTurret(turrPos);
robot.turr2.setPosition(1 - turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -511,7 +502,7 @@ public class Auto_LT_Far extends LinearOpMode {
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel); flywheel.manageFlywheel(vel);
robot.hood.setPosition(hoodPos); servos.setHoodPos(hoodPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
@@ -579,7 +570,7 @@ public class Auto_LT_Far extends LinearOpMode {
turret.trackGoal(deltaPose); turret.trackGoal(deltaPose);
robot.hood.setPosition(targetingSettings.hoodAngle); servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
@@ -650,7 +641,7 @@ public class Auto_LT_Far extends LinearOpMode {
targetingSettings = targeting.calculateSettings targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, false); (robotX, robotY, robotHeading, 0.0, false);
robot.hood.setPosition(targetingSettings.hoodAngle); servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
@@ -701,14 +692,13 @@ public class Auto_LT_Far extends LinearOpMode {
turret = new Turret(robot, TELE, robot.limelight); turret = new Turret(robot, TELE, robot.limelight);
turret.manualSetTurret(turrDefault); turret.setTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
robot.spin1.setPosition(autoSpinStartPos); servos.setSpinPos(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos);
robot.transferServo.setPosition(transferServo_out); servos.setTransferPos(transferServo_out);
robot.light.setPosition(1); robot.light.setPosition(1);
@@ -721,7 +711,7 @@ public class Auto_LT_Far extends LinearOpMode {
gamepad2.rumble(1000); gamepad2.rumble(1000);
} }
turret.manualSetTurret(turretShootPos); turret.setTurret(turretShootPos);
robot.hood.setPosition(shoot0Hood); robot.hood.setPosition(shoot0Hood);

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -54,6 +55,7 @@ public class TeleopV3 extends LinearOpMode {
Targeting targeting; Targeting targeting;
Targeting.Settings targetingSettings; Targeting.Settings targetingSettings;
Drivetrain drivetrain; Drivetrain drivetrain;
MeasuringLoopTimes loopTimes;
double autoHoodOffset = 0.0; double autoHoodOffset = 0.0;
int shooterTicker = 0; int shooterTicker = 0;
boolean intake = false; boolean intake = false;
@@ -92,6 +94,9 @@ public class TeleopV3 extends LinearOpMode {
drivetrain = new Drivetrain(robot, drive); drivetrain = new Drivetrain(robot, drive);
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
PIDFController tController = new PIDFController(tp, ti, td, tf); PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001); tController.setTolerance(0.001);
@@ -113,7 +118,7 @@ public class TeleopV3 extends LinearOpMode {
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
robot.transferServo.setPosition(transferServo_out); servo.setTransferPos(transferServo_out);
while (opModeIsActive()) { while (opModeIsActive()) {
@@ -133,7 +138,7 @@ public class TeleopV3 extends LinearOpMode {
if (gamepad1.right_bumper) { if (gamepad1.right_bumper) {
shootAll = false; shootAll = false;
robot.transferServo.setPosition(transferServo_out); servo.setTransferPos(transferServo_out);
} }
@@ -193,9 +198,9 @@ public class TeleopV3 extends LinearOpMode {
//HOOD: //HOOD:
if (targetingHood) { if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset);
} else { } else {
robot.hood.setPosition(hoodDefaultPos); servo.setHoodPos(hoodDefaultPos);
} }
if (gamepad2.dpadUpWasPressed()) { if (gamepad2.dpadUpWasPressed()) {
@@ -265,10 +270,10 @@ public class TeleopV3 extends LinearOpMode {
// spindexer.shootAll(); // spindexer.shootAll();
// TELE.addLine("starting to shoot"); // TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) { } else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in); servo.setTransferPos(transferServo_in);
//TELE.addLine("shoot"); //TELE.addLine("shoot");
} else { } else {
robot.transferServo.setPosition(transferServo_out); servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
shootAll = false; shootAll = false;
spindexer.resetSpindexer(); spindexer.resetSpindexer();
@@ -280,7 +285,7 @@ public class TeleopV3 extends LinearOpMode {
} }
if (gamepad1.left_stick_button) { if (gamepad1.left_stick_button) {
robot.transferServo.setPosition(transferServo_out); servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
shootAll = false; shootAll = false;
spindexer.resetSpindexer(); spindexer.resetSpindexer();
@@ -293,6 +298,7 @@ public class TeleopV3 extends LinearOpMode {
for (LynxModule hub : allHubs) { for (LynxModule hub : allHubs) {
hub.clearBulkCache(); hub.clearBulkCache();
} }
loopTimes.loop();
// //
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -328,7 +334,10 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.addData("Voltage", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub) TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.update(); TELE.update();

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java
View File

@@ -60,7 +60,7 @@ public class LimelightTest extends LinearOpMode {
if (turretMode){ if (turretMode){
if (turretPos != 0.501){ if (turretPos != 0.501){
turret.manualSetTurret(turretPos); turret.setTurret(turretPos);
} }
} }

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -1,23 +1,30 @@
package org.firstinspires.ftc.teamcode.tests; package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
@Config @Config
@TeleOp @TeleOp
@@ -37,10 +44,12 @@ public class ShooterTest extends LinearOpMode {
public static boolean shoot = false; public static boolean shoot = false;
public static boolean intake = false; public static boolean intake = false;
public static boolean turretTrack = true;
Robot robot; Robot robot;
Flywheel flywheel; Flywheel flywheel;
Servos servo; Servos servo;
MecanumDrive drive;
Turret turret;
double shootStamp = 0.0; double shootStamp = 0.0;
boolean shootAll = false; boolean shootAll = false;
@@ -50,6 +59,7 @@ public class ShooterTest extends LinearOpMode {
public double hoodAdjust = 0.0; public double hoodAdjust = 0.0;
public static double hoodAdjustFactor = 1.0; public static double hoodAdjustFactor = 1.0;
private int shooterTicker = 0; private int shooterTicker = 0;
public static double spinSpeed = 0.02;
Spindexer spindexer ; Spindexer spindexer ;
@Override @Override
@@ -61,17 +71,51 @@ public class ShooterTest extends LinearOpMode {
flywheel = new Flywheel(hardwareMap); flywheel = new Flywheel(hardwareMap);
spindexer = new Spindexer(hardwareMap); spindexer = new Spindexer(hardwareMap);
servo = new Servos(hardwareMap); servo = new Servos(hardwareMap);
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
MultipleTelemetry TELE = new MultipleTelemetry( MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry() telemetry, FtcDashboard.getInstance().getTelemetry()
); );
turret = new Turret(robot, TELE, robot.limelight);
Turret.limelightUsed = true;
waitForStart(); waitForStart();
robot.limelight.start();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()) { while (opModeIsActive()) {
if (redAlliance){
robot.limelight.pipelineSwitch(4);
} else {
robot.limelight.pipelineSwitch(2);
}
//TURRET TRACKING
drive.updatePoseEstimate();
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
double goalY = 0;
double dx = robX - goalX; // delta x from robot to goal
double dy = robY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
if (turretTrack){
turret.trackGoal(deltaPose);
} else if (turretPos != 0.501){
turret.setTurret(turretPos);
}
double voltage = robot.voltage.getVoltage(); double voltage = robot.voltage.getVoltage();
if (mode == 0) { if (mode == 0) {
@@ -120,8 +164,11 @@ public class ShooterTest extends LinearOpMode {
robot.transferServo.setPosition(transferServo_in); robot.transferServo.setPosition(transferServo_in);
shooterTicker++; shooterTicker++;
double prevSpinPos = robot.spin1.getPosition(); double prevSpinPos = robot.spin1.getPosition();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); if (prevSpinPos < 0.9){
robot.spin1.setPosition(prevSpinPos + spinSpeed);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeed);
}
} }

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java Normal file
View File

@@ -0,0 +1,66 @@
package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.robotcore.util.ElapsedTime;
public class MeasuringLoopTimes {
private ElapsedTime elapsedtime;
private double minLoopTime = 999999999999.0;
private double maxLoopTime = 0.0;
private double mainLoopTime = 0.0;
private double MeasurementStart = 0.0;
private double currentTime = 0.0;
private double avgLoopTime = 0.0;
private int avgLoopTimeTicker = 0;
private double avgLoopTimeSum = 0;
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
public void init() {
elapsedtime = new ElapsedTime();
elapsedtime.reset();
MeasurementStart = getTimeSeconds();
}
public double getAvgLoopTime() {
return avgLoopTime;
}
public double getMaxLoopTimeOneMin() {
return maxLoopTime;
}
public double getMinLoopTimeOneMin() {
return minLoopTime;
}
public void loop() {
currentTime = getTimeSeconds();
if ((MeasurementStart + 15.0) < currentTime)
{
minLoopTime = 9999999.0;
maxLoopTime = 0.0;
MeasurementStart = currentTime;
avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
avgLoopTimeSum = 0.0;
avgLoopTimeTicker = 0;
}
mainLoopTime = elapsedtime.milliseconds();
elapsedtime.reset();
avgLoopTimeSum += mainLoopTime;
avgLoopTimeTicker++;
minLoopTime = Math.min(minLoopTime,mainLoopTime);
maxLoopTime = Math.max(maxLoopTime,mainLoopTime);
}
}

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -18,6 +18,15 @@ public class Servos {
PIDFController spinPID; PIDFController spinPID;
PIDFController turretPID; PIDFController turretPID;
private double prevSpinPos = 0.0;
private boolean firstSpinPos = true;
private double prevTransferPos = 0.0;
private boolean firstTransferPos = true;
private double prevHoodPos = 0.0;
private boolean firstHoodPos = true;
public Servos(HardwareMap hardwareMap) { public Servos(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
spinPID = new PIDFController(spinP, spinI, spinD, spinF); spinPID = new PIDFController(spinP, spinI, spinD, spinF);
@@ -32,8 +41,42 @@ public class Servos {
return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
} }
public double setSpinPos(double pos) { public double getSpinCmdPos() {
return prevSpinPos;
}
public boolean servoPosEqual(double pos1, double pos2) {
return (Math.abs(pos1 - pos2) < 0.005);
}
public double setTransferPos(double pos) {
if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) {
robot.transferServo.setPosition(pos);
firstTransferPos = false;
}
prevTransferPos = pos;
return pos;
}
public double setSpinPos(double pos) {
if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
firstSpinPos = false;
}
prevSpinPos = pos;
return pos;
}
public double setHoodPos(double pos){
if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
robot.hood.setPosition(pos);
firstHoodPos = false;
}
prevHoodPos = pos;
return pos; return pos;
} }
@@ -48,8 +91,4 @@ public class Servos {
public double setTurrPos(double pos) { public double setTurrPos(double pos) {
return 1.0; return 1.0;
} }
public boolean turretEqual(double pos) {
return true;
}
} }

72
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController; import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.Color.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
@@ -184,11 +186,9 @@ public class Spindexer {
if (detectRearColor) { if (detectRearColor) {
// Detect which color // Detect which color
double green = robot.color1.getNormalizedColors().green; NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue); double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
// FIXIT - Add filtering to improve accuracy. // FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) { if (gP >= 0.38) {
@@ -196,7 +196,7 @@ public class Spindexer {
} else { } else {
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
} }
} }
} }
// Position 2 // Position 2
// Find which ball position this is in the spindexer // Find which ball position this is in the spindexer
@@ -205,11 +205,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) { if (detectFrontColor) {
double green = robot.color2.getNormalizedColors().green; NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue); double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
if (gP >= 0.4) { if (gP >= 0.4) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -234,11 +232,9 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
if (detectFrontColor) { if (detectFrontColor) {
double green = robot.color3.getNormalizedColors().green; NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue); double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP >= 0.42) { if (gP >= 0.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
@@ -269,8 +265,7 @@ public class Spindexer {
} }
// Has code to unjam spindexer // Has code to unjam spindexer
private void moveSpindexerToPos(double pos) { private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos); servos.setSpinPos(pos);
robot.spin2.setPosition(1-pos);
// double currentPos = servos.getSpinPos(); // double currentPos = servos.getSpinPos();
// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){ // if (currentPos > pos){
@@ -281,13 +276,14 @@ public class Spindexer {
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// } // }
// } // }
// prevPos = currentPos; // prevPos = pos;
} }
public void stopSpindexer() { public void stopSpindexer() {
} }
private double prevLight = 0.0;
public void ballCounterLight(){ public void ballCounterLight(){
int counter = 0; int counter = 0;
if (!ballPositions[0].isEmpty){ if (!ballPositions[0].isEmpty){
@@ -299,15 +295,21 @@ public class Spindexer {
if (!ballPositions[2].isEmpty){ if (!ballPositions[2].isEmpty){
counter++; counter++;
} }
double light;
if (counter == 3){ if (counter == 3){
robot.light.setPosition(Light3); light = Light3;
} else if (counter == 2){ } else if (counter == 2){
robot.light.setPosition(Light2); light = Light2;
} else if (counter == 1){ } else if (counter == 1){
robot.light.setPosition(Light1); light = Light1;
} else { } else {
robot.light.setPosition(Light0); light = Light0;
} }
if (light != prevLight){
robot.light.setPosition(light);
}
prevLight = light;
} }
public boolean slotIsEmpty(int slot){ public boolean slotIsEmpty(int slot){
@@ -325,7 +327,7 @@ public class Spindexer {
case UNKNOWN_START: case UNKNOWN_START:
// For now just set position ONE if UNKNOWN // For now just set position ONE if UNKNOWN
commandedIntakePosition = 0; commandedIntakePosition = 0;
moveSpindexerToPos(intakePositions[0]); servos.setSpinPos(intakePositions[0]);
currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
break; break;
case UNKNOWN_MOVE: case UNKNOWN_MOVE:
@@ -336,7 +338,7 @@ public class Spindexer {
unknownColorDetect = 0; unknownColorDetect = 0;
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); servos.setSpinPos(intakePositions[commandedIntakePosition]);
} }
break; break;
case UNKNOWN_DETECT: case UNKNOWN_DETECT:
@@ -355,7 +357,7 @@ public class Spindexer {
} else { } else {
// Maintain Position // Maintain Position
spindexerWiggle *= -1.0; spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
} }
break; break;
case FINDNEXT: case FINDNEXT:
@@ -387,7 +389,7 @@ public class Spindexer {
//commandedIntakePosition = bestFitMotif(); //commandedIntakePosition = bestFitMotif();
currentIntakeState = Spindexer.IntakeState.FULL; currentIntakeState = Spindexer.IntakeState.FULL;
} }
moveSpindexerToPos(intakePositions[commandedIntakePosition]); servos.setSpinPos(intakePositions[commandedIntakePosition]);
break; break;
case MOVING: case MOVING:
@@ -403,7 +405,7 @@ public class Spindexer {
//detectBalls(false, false); //detectBalls(false, false);
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); servos.setSpinPos(intakePositions[commandedIntakePosition]);
} }
break; break;
@@ -416,7 +418,7 @@ public class Spindexer {
} }
// Maintain Position // Maintain Position
spindexerWiggle *= -1.0; spindexerWiggle *= -1.0;
moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle);
break; break;
case SHOOT_ALL_PREP: case SHOOT_ALL_PREP:
@@ -425,7 +427,7 @@ public class Spindexer {
commandedIntakePosition = 0; commandedIntakePosition = 0;
if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions"
} }
break; break;
@@ -437,7 +439,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
} }
// Maintain Position // Maintain Position
moveSpindexerToPos(outakePositions[commandedIntakePosition]); servos.setSpinPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTNEXT: case SHOOTNEXT:
@@ -458,7 +460,7 @@ public class Spindexer {
// Empty return to intake state // Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT; currentIntakeState = IntakeState.FINDNEXT;
} }
moveSpindexerToPos(outakePositions[commandedIntakePosition]); servos.setSpinPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTMOVING: case SHOOTMOVING:
@@ -467,7 +469,7 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(outakePositions[commandedIntakePosition]); servos.setSpinPos(outakePositions[commandedIntakePosition]);
} }
break; break;
@@ -492,14 +494,14 @@ public class Spindexer {
} }
// Keep moving the spindexer // Keep moving the spindexer
spindexerOuttakeWiggle *= -1.0; spindexerOuttakeWiggle *= -1.0;
moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
break; break;
case SHOOT_PREP_CONTINOUS: case SHOOT_PREP_CONTINOUS:
if (servos.spinEqual(spinStartPos)){ if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
} else { } else {
moveSpindexerToPos(spinStartPos); servos.setSpinPos(spinStartPos);
} }
break; break;
@@ -510,11 +512,11 @@ public class Spindexer {
if (servos.getSpinPos() > spinEndPos){ if (servos.getSpinPos() > spinEndPos){
currentIntakeState = IntakeState.FINDNEXT; currentIntakeState = IntakeState.FINDNEXT;
} else { } else {
double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){ if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03; spinPos = spinEndPos + 0.03;
} }
moveSpindexerToPos(spinPos); servos.setSpinPos(spinPos);
} }
break; break;
@@ -565,7 +567,7 @@ public class Spindexer {
//break; //break;
case NONE: case NONE:
return 0; return 0;
//break; //break;
} }
return 0; return 0;
} }

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -71,10 +71,13 @@ public class Turret {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
} }
private double prevTurrPos = 0.501;
public void manualSetTurret(double pos) { public void setTurret(double pos) {
robot.turr1.setPosition(pos); if (prevTurrPos != 0.501 && prevTurrPos != pos){
robot.turr2.setPosition(1 - pos); robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
}
prevTurrPos = pos;
} }
public boolean turretEqual(double pos) { public boolean turretEqual(double pos) {
@@ -272,8 +275,7 @@ public class Turret {
} }
// Set servo positions // Set servo positions
robot.turr1.setPosition(turretPos + manualOffset); setTurret(turretPos + manualOffset);
robot.turr2.setPosition(1.0 - turretPos - manualOffset);
/* ---------------- TELEMETRY ---------------- */ /* ---------------- TELEMETRY ---------------- */