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

teleop basically complete

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-29 18:53:06 -06:00
parent edc300c1d5
commit 5d2a2e1da8
6 changed files with 73 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -102,7 +102,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
public static double shoot0Time = 1.6;
public static double intake1Time = 3.0;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 25;
public static double pickup1Speed = 17;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shoot1Hood = 0.93;
@@ -720,7 +720,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
@@ -729,7 +728,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
@@ -737,7 +735,6 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java
View File

@@ -111,7 +111,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double shoot0Time = 1.6;
public static double intake1Time = 3.0;
public static double flywheel0Time = 3.5;
public static double pickup1Speed = 25;
public static double pickup1Speed = 17;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shoot1Hood = 0.93;
@@ -836,7 +836,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
@@ -845,7 +844,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
@@ -853,7 +851,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep)
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
TELE.addData("Red?", redAlliance);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java
View File

@@ -7,5 +7,5 @@ public class Color {
public static boolean redAlliance = true;
public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5;
public static double colorFilterAlpha = 0.15;
public static double colorFilterAlpha = 1;
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -16,10 +16,11 @@ public class ServoPositions {
public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
public static double spinStartPos = spindexer_outtakeBall3 - 0.1;
public static double spinStartPos = 0.25;
public static double spinEndPos = 0.85;
public static double shootAllAutoSpinStartPos = 0.2;
public static double shootAllSpindexerSpeedIncrease = 0.02;
public static double shootAllSpindexerSpeedIncrease = 0.04;
public static double shootAllTime = 1.8;
public static double transferServo_out = 0.15;

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

@@ -168,14 +168,6 @@ public class TeleopV3 extends LinearOpMode {
PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001);
//
// if (redAlliance) {
// robot.limelight.pipelineSwitch(3);
// } else {
// robot.limelight.pipelineSwitch(2);
// }
// robot.limelight.start();
Turret turret = new Turret(robot, TELE, robot.limelight);
robot.light.setPosition(1);
@@ -264,57 +256,6 @@ public class TeleopV3 extends LinearOpMode {
}
// if (autoSpintake) {
//
// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
//
//
// robot.spin1.setPosition(spindexPos);
// robot.spin2.setPosition(1-spindexPos);
//
// }
//
// if (gamepad1.right_bumper) {
//
// shootAll = false;
//
// intakeTicker++;
//
//// if (intakeTicker % 20 < 2) {
////
//// robot.spin1.setPower(-1);
//// robot.spin2.setPower(1);
////
//// } else if (intakeTicker % 20 < 10) {
//// robot.spin1.setPower(-0.5);
//// robot.spin2.setPower(0.5);
//// } else if (intakeTicker % 20 < 12) {
//// robot.spin1.setPower(1);
//// robot.spin2.setPower(-1);
//// } else {
//// robot.spin1.setPower(0.5);
//// robot.spin2.setPower(-0.5);
//// }
//
// robot.intake.setPower(1);
// intakeStamp = getRuntime();
// TELE.addData("Reverse?", reverse);
// TELE.update();
// } else {
// if (!servo.spinEqual(spindexPos)) {
// robot.spin1.setPosition(spindexPos);
// robot.spin2.setPosition(1-spindexPos);
//
// }
//
// spindexPos = spindexer_intakePos1;
//
// robot.intake.setPower(0);
//
// intakeTicker = 0;
// }
// }
//COLOR:
double s1D = robot.color1.getDistance(DistanceUnit.MM);
@@ -447,31 +388,6 @@ public class TeleopV3 extends LinearOpMode {
//SHOOTER:
flywheel.manageFlywheel(vel);
//TODO: test the camera teleop code
// if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
// double bearing;
//
// LLResult result = robot.light.getLatestResult();
// if (result != null) {
// if (result.isValid()) {
// bearing = result.getTx() * bMult;
//
// double bearingCorrection = bearing / bDiv;
//
// error += bearingCorrection;
//
// camTicker++;
// TELE.addData("tx", bearingCorrection);
// TELE.addData("ty", result.getTy());
// }
// }
//
// } else {
// camTicker = 0;
// overrideTurr = false;
// }
//HOOD:
if (targetingHood) {
@@ -491,7 +407,6 @@ public class TeleopV3 extends LinearOpMode {
autoHoodOffset += 0.02;
}
//
if (gamepad2.cross) {
manualOffset = 0;
@@ -527,77 +442,6 @@ public class TeleopV3 extends LinearOpMode {
// yOffset = robotY;
// headingOffset = robotHeading;
// }
// }
//if (gamepad1.left_bumper && !enableSpindexerManager) {
// robot.transferServo.setPosition(transferServo_out);
// autoSpintake = false;
//
// intakeTicker++;
//
// if (intakeTicker % 10 < 1) {
//
// robot.spin1.setPower(-1);
// robot.spin2.setPower(1);
//
// } else if (intakeTicker % 10 < 5) {
// robot.spin1.setPower(-0.5);
// robot.spin2.setPower(0.5);
// } else if (intakeTicker % 10 < 6) {
// robot.spin1.setPower(1);
// robot.spin2.setPower(-1);
// } else {
// robot.spin1.setPower(0.5);
// robot.spin2.setPower(-0.5);
// }
//
// intake = false;
// reject = false;
//
// robot.intake.setPower(0.5);
//
// }
//
// if (gamepad1.leftBumperWasReleased() && !enableSpindexerManager) {
// shootStamp = getRuntime();
// shootAll = true;
//
// shooterTicker = 0;
// }
//
// if (shootAll && !enableSpindexerManager) {
//
// TELE.addData("100% works", shootOrder);
//
// intake = false;
// reject = false;
//
// shooterTicker++;
//
// spindexPos = spindexer_intakePos1;
//
// if (getRuntime() - shootStamp < 3.5) {
//
// robot.transferServo.setPosition(transferServo_in);
//
// autoSpintake = false;
//
// robot.spin1.setPower(-spinPow);
// robot.spin2.setPower(spinPow);
//
// } else {
// robot.transferServo.setPosition(transferServo_out);
// spindexPos = spindexer_intakePos1;
//
// shootAll = false;
//
// autoSpintake = true;
//
// robot.transferServo.setPosition(transferServo_out);
// }
//
// }
if (enableSpindexerManager) {
@@ -672,12 +516,12 @@ public class TeleopV3 extends LinearOpMode {
if (shooterTicker == 0) {
spindexer.prepareShootAll();
spindexer.prepareShootAllContinous();
TELE.addLine("preparing to shoot");
} else if (shooterTicker == 2) {
//robot.transferServo.setPosition(transferServo_in);
spindexer.shootAll();
TELE.addLine("starting to shoot");
// } else if (shooterTicker == 2) {
// //robot.transferServo.setPosition(transferServo_in);
// spindexer.shootAll();
// TELE.addLine("starting to shoot");
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
TELE.addLine("shoot");
@@ -699,7 +543,6 @@ public class TeleopV3 extends LinearOpMode {
shootAll = false;
spindexer.resetSpindexer();
spindexer.processIntake();
}
}

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

@@ -5,6 +5,9 @@ import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
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.spinEndPos;
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_intakePos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
@@ -72,7 +75,9 @@ public class Spindexer {
SHOOTMOVING,
SHOOTWAIT,
SHOOT_ALL_PREP,
SHOOT_ALL_READY
SHOOT_ALL_READY,
SHOOT_PREP_CONTINOUS,
SHOOT_CONTINOUS
}
int shootWaitCount = 0;
@@ -143,6 +148,9 @@ public class Spindexer {
ballPositions[pos].isEmpty = true;
ballPositions[pos].foundEmpty = 0;
ballPositions[pos].ballColor = BallColor.UNKNOWN;
distanceRearCenter = 61;
distanceFrontDriver = 61;
distanceFrontPassenger = 61;
}
public void resetSpindexer () {
@@ -263,8 +271,8 @@ public class 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){
// double currentPos = servos.getSpinPos();
// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
// if (currentPos > pos){
// robot.spin1.setPosition(servos.getSpinPos() + 0.05);
// robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05);
@@ -272,8 +280,8 @@ public class Spindexer {
// robot.spin1.setPosition(servos.getSpinPos() - 0.05);
// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05);
// }
}
prevPos = currentPos;
// }
// prevPos = currentPos;
}
public void stopSpindexer() {
@@ -309,6 +317,8 @@ public class Spindexer {
public boolean isFull () {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
}
private double intakeTicker = 0;
public boolean processIntake() {
switch (currentIntakeState) {
@@ -359,15 +369,15 @@ public class Spindexer {
commandedIntakePosition = 0;
currentIntakeState = Spindexer.IntakeState.MOVING;
}
proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
//proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
//if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[1].isEmpty) {
else if (ballPositions[1].isEmpty) {
// Position 2
commandedIntakePosition = 1;
currentIntakeState = Spindexer.IntakeState.MOVING;
}
proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty) {
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
else if (ballPositions[2].isEmpty) {
// Position 3
commandedIntakePosition = 2;
currentIntakeState = Spindexer.IntakeState.MOVING;
@@ -383,8 +393,13 @@ public class Spindexer {
case MOVING:
// Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
if (intakeTicker > 1){
currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer();
intakeTicker = 0;
} else {
intakeTicker++;
}
//detectBalls(false, false);
} else {
// Keep moving the spindexer
@@ -476,10 +491,33 @@ public class Spindexer {
//detectBalls(true, false);
}
// Keep moving the spindexer
spindexerOuttakeWiggle *= -1.01;
spindexerOuttakeWiggle *= -1.0;
moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle);
break;
case SHOOT_PREP_CONTINOUS:
if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
} else {
moveSpindexerToPos(spinStartPos);
}
break;
case SHOOT_CONTINOUS:
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
if (servos.getSpinPos() > spinEndPos){
currentIntakeState = IntakeState.FINDNEXT;
} else {
double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
if (spinPos > spinEndPos + 0.03){
spinPos = spinEndPos + 0.03;
}
moveSpindexerToPos(spinPos);
}
break;
default:
// Statements to execute if no case matches
}
@@ -539,12 +577,21 @@ public class Spindexer {
public void prepareShootAll(){
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
}
public void prepareShootAllContinous(){
currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS;
}
public void shootAll () {
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
public void shootAllContinous(){
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
}
public boolean shootAllComplete ()
{
@@ -552,7 +599,9 @@ public class Spindexer {
(currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTWAIT));
(currentIntakeState != Spindexer.IntakeState.SHOOTWAIT) &&
(currentIntakeState != Spindexer.IntakeState.SHOOT_PREP_CONTINOUS) &&
(currentIntakeState != Spindexer.IntakeState.SHOOT_CONTINOUS));
}
void shootAllToIntake () {