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

Refactor drivetrain for full subsystem management

Browse Source
This commit is contained in:
Keshav Anand
2026-01-30 18:50:57 -06:00
parent e5cb48eefa
commit 61e47095f3
3 changed files with 118 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -10,6 +10,7 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -24,6 +25,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import java.util.ArrayList;
import java.util.List;
@Disabled
@TeleOp
@Config
public class TeleopV2 extends LinearOpMode {

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

@@ -2,38 +2,21 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
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_intakePos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
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;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinP;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@@ -41,40 +24,27 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.ArrayList;
import java.util.List;
@Config
@TeleOp
public class TeleopV3 extends LinearOpMode {
public static double manualVel = 3000;
public static int intakeJamSwap = 12;
public static double hoodDefaultPos = 0.5;
public static double desiredTurretAngle = 180;
public static double shootStamp2 = 0.0;
public static double spinningPow = 0.2;
public static double spindexPos = spindexer_intakePos1;
public static double spinPow = 0.09;
public static double bMult = 1, bDiv = 2200;
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static boolean manualTurret = true;
public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4;
public double vel = 3000;
public boolean autoVel = true;
public boolean targetingVel = true;
public boolean targetingHood = true;
public double manualOffset = 0.0;
public boolean autoHood = true;
public boolean green1 = false;
public boolean green2 = false;
public boolean green3 = false;
public double shootStamp = 0.0;
public boolean circle = false;
public boolean square = false;
public boolean triangle = false;
public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
boolean fixedTurret = false;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
Robot robot;
MultipleTelemetry TELE;
Servos servo;
@@ -83,8 +53,8 @@ public class TeleopV3 extends LinearOpMode {
Spindexer spindexer;
Targeting targeting;
Targeting.Settings targetingSettings;
Drivetrain drivetrain;
double autoHoodOffset = 0.0;
int shooterTicker = 0;
boolean intake = false;
boolean reject = false;
@@ -92,59 +62,18 @@ public class TeleopV3 extends LinearOpMode {
double yOffset = 0.0;
double headingOffset = 0.0;
int ticker = 0;
int camTicker = 0;
List<Double> s1G = new ArrayList<>();
List<Double> s2G = new ArrayList<>();
List<Double> s3G = new ArrayList<>();
List<Double> s1T = new ArrayList<>();
List<Double> s2T = new ArrayList<>();
List<Double> s3T = new ArrayList<>();
List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>();
boolean oddBallColor = false;
double hoodOffset = 0.0;
boolean shootA = true;
boolean shootB = true;
boolean shootC = true;
boolean autoSpintake = false;
boolean enableSpindexerManager = true;
List<Integer> shootOrder = new ArrayList<>();
boolean outtake1 = false;
boolean outtake2 = false;
boolean outtake3 = false;
boolean overrideTurr = false;
double turretPID = 0.0;
double turretPos = 0;
double spindexPID = 0.0;
double error = 0.0;
double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0;
boolean reverse = false;
int intakeTicker = 0;
Pose2d brakePos = new Pose2d(0, 0, 0);
boolean autoDrive = false;
private boolean shootAll = false;
private double transferStamp = 0.0;
private int tickerA = 1;
private boolean transferIn = false;
boolean turretInterpolate = false;
public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4;
public static double velPrediction(double distance) {
if (distance < 30) {
return 2750;
} else if (distance > 100) {
if (distance > 120) {
return 4500;
}
return 3700;
} else {
// linear interpolation between 40->2650 and 120->3600
double slope = (3700.0 - 2750.0) / (100.0 - 30);
return (int) Math.round(2750 + slope * (distance - 30));
}
}
boolean overrideTurr = false;
int intakeTicker = 0;
boolean turretInterpolate = false;
private boolean shootAll = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -156,7 +85,6 @@ public class TeleopV3 extends LinearOpMode {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap);
flywheel = new Flywheel(hardwareMap);
@@ -165,13 +93,15 @@ public class TeleopV3 extends LinearOpMode {
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0, 0.0);
drivetrain = new Drivetrain(robot, drive);
PIDFController tController = new PIDFController(tp, ti, td, tf);
tController.setTolerance(0.001);
Turret turret = new Turret(robot, TELE, robot.limelight);
robot.light.setPosition(1);
while (opModeInInit()){
while (opModeInInit()) {
robot.limelight.start();
if (redAlliance) {
robot.limelight.pipelineSwitch(4);
@@ -194,63 +124,14 @@ public class TeleopV3 extends LinearOpMode {
//DRIVETRAIN:
double y = 0.0;
double x = 0.0;
double rx = 0.0;
if (!autoDrive) {
y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
rx = gamepad1.left_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower);
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
}
if (gamepad1.left_trigger > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
drive.updatePoseEstimate();
brakePos = drive.localizer.getPose();
autoDrive = true;
} else if (gamepad1.left_trigger > 0.4) {
drive.updatePoseEstimate();
Pose2d currentPos = drive.localizer.getPose();
TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
.strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);
if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) {
Actions.runBlocking(
traj2.build()
drivetrain.drive(
gamepad1.right_stick_y,
gamepad1.right_stick_x,
gamepad1.left_stick_x,
gamepad1.left_trigger
);
}
} else {
autoDrive = false;
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
}
//TODO: Use color sensors to switch between positions...switch after ball detected in
if (gamepad1.right_bumper){
if (gamepad1.right_bumper) {
shootAll = false;
robot.transferServo.setPosition(transferServo_out);
@@ -277,7 +158,7 @@ public class TeleopV3 extends LinearOpMode {
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
targetingSettings = targeting.calculateSettings
(robotX,robotY,robotHeading,0.0, turretInterpolate);
(robotX, robotY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
@@ -398,7 +279,7 @@ public class TeleopV3 extends LinearOpMode {
//spindexer.processIntake();
}
if (gamepad1.left_stick_button){
if (gamepad1.left_stick_button) {
robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1;
@@ -407,8 +288,6 @@ public class TeleopV3 extends LinearOpMode {
}
}
//EXTRA STUFFINESS:
drive.updatePoseEstimate();
@@ -457,167 +336,4 @@ public class TeleopV3 extends LinearOpMode {
}
}
// Helper methods
private boolean checkGreen(List<Boolean> s, List<Double> sT) {
if (s.isEmpty()) return false;
double lastTime = sT.get(sT.size() - 1);
int countTrue = 0;
int countWindow = 0;
for (int i = 0; i < s.size(); i++) {
if (lastTime - sT.get(i) <= 3.0) { // element is within 2s of last
countWindow++;
if (s.get(i)) {
countTrue++;
}
}
}
if (countWindow == 0) return false; // avoid divide by zero
return countTrue > countWindow / 2.0; // more than 50% true
}
//
// public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
// // Set spin positions
// spindexPos = spindexer;
//
// // Check if spindexer has reached the target position
// if (spinOk || getRuntime() - stamp > 1.5) {
// if (tickerA == 1) {
// transferStamp = getRuntime();
// tickerA++;
// TELE.addLine("tickerSet");
// }
//
// if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
// robot.transferServo.setPosition(transferServo_in);
// transferIn = true;
// TELE.addLine("transferring");
//
// return true; // still in progress
//
// } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) {
// robot.transferServo.setPosition(transferServo_out);
// transferIn = false; // reset for next shot
// tickerA = 1; // reset ticker
// transferStamp = 0.0;
//
// TELE.addLine("shotFinished");
//
// return false; // finished shooting
// } else {
// TELE.addLine("sIP");
// return true; // still in progress
// }
// } else {
// robot.transferServo.setPosition(transferServo_out);
// tickerA = 1;
// transferStamp = getRuntime();
// transferIn = false;
// return true; // still moving spin
// }
// }
//
public double hoodAnglePrediction(double x) {
double a = 1.44304;
double b = 0.0313707;
double c = 0.0931136;
double result = a * Math.exp(-b * x) + c;
// Clamp between min and max
if (result < 0.1) {
return 0.1;
} else if (result > 0.96) {
return 0.96;
} else {
return result;
}
}
//
// void addOddThenRest(List<Integer> order, boolean oddColor) {
// // Odd ball first
// for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);
// TELE.addData("1", shootOrder);
// for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i);
// TELE.addData("works", shootOrder);
// TELE.addData("oddBall", oddColor);
// shootAll = true;
//
// }
//
// void addOddInMiddle(List<Integer> order, boolean oddColor) {
//
// boolean[] used = new boolean[4]; // index 1..3
//
// // 1) Add a NON-odd ball first
// for (int i = 1; i <= 3; i++) {
// if (getBallColor(i) != oddColor) {
// order.add(i);
// used[i] = true;
// break;
// }
// }
//
// // 2) Add the odd ball second
// for (int i = 1; i <= 3; i++) {
// if (!used[i] && getBallColor(i) == oddColor) {
// order.add(i);
// used[i] = true;
// break;
// }
// }
//
// // 3) Add the remaining non-odd ball third
// for (int i = 1; i <= 3; i++) {
// if (!used[i] && getBallColor(i) != oddColor) {
// order.add(i);
// used[i] = true;
// break;
// }
// }
//
// TELE.addData("works", order);
// TELE.addData("oddBall", oddColor);
// shootAll = true;
//
// }
//
// void addOddLast(List<Integer> order, boolean oddColor) {
// // Odd ball last
// for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i);
// TELE.addData("1", shootOrder);
// for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);
// TELE.addData("works", shootOrder);
// TELE.addData("oddBall", oddColor);
// shootAll = true;
//
// }
//
// // Returns color of ball in slot i (1-based)
// boolean getBallColor(int slot) {
// switch (slot) {
// case 1:
// return green1;
// case 2:
// return green2;
// case 3:
// return green3;
// }
// return false; // default
// }
//
boolean ballIn(int slot) {
List<Double> times =
slot == 1 ? s1T :
slot == 2 ? s2T :
slot == 3 ? s3T : null;
if (times == null || times.isEmpty()) return false;
return times.get(times.size() - 1) > getRuntime() - 2;
}
}

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java Normal file
View File

@@ -0,0 +1,86 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
public class Drivetrain {
Robot robot;
boolean autoDrive = false;
Pose2d brakePos = new Pose2d(0, 0, 0);
MecanumDrive drive;
private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
public Drivetrain (Robot rob, MecanumDrive mecanumDrive){
this.robot = rob;
this.drive = mecanumDrive;
}
public void drive(double y, double x, double rx, double brake){
if (!autoDrive) {
x = x* 1.1; // Counteract imperfect strafing
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower);
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
}
if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
drive.updatePoseEstimate();
brakePos = drive.localizer.getPose();
autoDrive = true;
} else if (brake > 0.4) {
drive.updatePoseEstimate();
Pose2d currentPos = drive.localizer.getPose();
TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
.strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);
if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) {
Actions.runBlocking(
traj2.build()
);
}
} else {
autoDrive = false;
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
}
}
}