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

added targeting information and unjaming code (both untested)

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-27 16:36:46 -06:00
parent 0549902505
commit dea9a10b08
4 changed files with 39 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
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_outtakeBall3;
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.utils.Servos.spinD;
@@ -123,6 +124,7 @@ public class TeleopV3 extends LinearOpMode {
private boolean transferIn = false;
boolean turretInterpolate = false;
public static double spinSpeedIncrease = 0.04;
public static int resetSpinTicks = 20;
public static double velPrediction(double distance) {
if (distance < 30) {
@@ -582,8 +584,12 @@ public class TeleopV3 extends LinearOpMode {
// }
if (enableSpindexerManager) {
if (!shootAll) {
// Gives some time to reset spindexer
if (!shootAll && intakeTicker > resetSpinTicks) {
spindexer.processIntake();
} else {
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1-spindexer_intakePos1);
}
// RIGHT_BUMPER
@@ -606,15 +612,16 @@ public class TeleopV3 extends LinearOpMode {
spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
}
intakeTicker++;
if (shootAll) {
intakeTicker = 0;
intake = false;
reject = false;
if (getRuntime() - shootStamp < 3.5) {
if (getRuntime() - shootStamp < 3.5 && servo.getSpinPos() < spindexer_outtakeBall3 + 0.1) {
if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
} else {

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

@@ -39,6 +39,8 @@ public class Spindexer {
public double distanceFrontDriver = 0.0;
public double distanceFrontPassenger = 0.0;
private double prevPos = 0.0;
public Types.Motif desiredMotif = Types.Motif.NONE;
// For Use
enum RotatedBallPositionNames {
@@ -246,10 +248,21 @@ public class Spindexer {
return newPos1Detection;
}
public void moveSpindexerToPos(double pos) {
// Has code to unjam spindexer
private void moveSpindexerToPos(double pos) {
robot.spin1.setPosition(pos);
robot.spin2.setPosition(1-pos);
double currentPos = servos.getSpinPos();
if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
if (currentPos > pos){
robot.spin1.setPosition(1);
robot.spin2.setPosition(0);
} else {
robot.spin1.setPosition(0);
robot.spin2.setPosition(1);
}
}
prevPos = currentPos;
}
public void stopSpindexer() {

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -67,19 +67,19 @@ public class Targeting {
KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
// ROW 4
KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1);
KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1);
KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1);
KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1);
KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1);
KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1);
// ROW 1
KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1);
KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1);
KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1);
KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1);
KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1);
KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1);
KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
// ROW 5
KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
}
public Targeting()

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

@@ -119,17 +119,14 @@ public class Turret {
}
public double getTy() {
limelightRead();
return ty;
}
public double getLimelightX() {
limelightRead();
return limelightPosX;
}
public double getLimelightY() {
limelightRead();
return limelightPosY;
}