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

@Abhiram pls fix this

Browse Source
This commit is contained in:
Keshav Anand
2026-01-15 21:04:56 -06:00
parent 66e76285b2
commit bfcecd42d3
4 changed files with 128 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,6 +3,8 @@ 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.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.utils.Servos.*;
import static org.firstinspires.ftc.teamcode.utils.Servos.spin_scalar;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
@@ -13,10 +15,12 @@ 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.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -47,6 +51,9 @@ public class TeleopV3 extends LinearOpMode {
public boolean circle = false;
public boolean square = false;
public boolean triangle = false;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
Robot robot;
@@ -85,7 +92,7 @@ public class TeleopV3 extends LinearOpMode {
double turretPID = 0.0;
double turretPos = 0.5;
double spindexPID = 0.0;
double spindexPos = spindexer_intakePos1;
public static double spindexPos = spindexer_intakePos1;
double error = 0.0;
double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0;
boolean reverse = false;
@@ -97,6 +104,21 @@ public class TeleopV3 extends LinearOpMode {
private int tickerA = 1;
private boolean transferIn = false;
public static double velPrediction(double distance) {
if (distance < 30) {
return 2750;
} else if (distance > 100) {
if (distance > 160) {
return 4200;
}
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));
}
}
@Override
public void runOpMode() throws InterruptedException {
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
@@ -180,11 +202,16 @@ public class TeleopV3 extends LinearOpMode {
// robot.turr1.setPower(turretPID);
// robot.turr2.setPower(-turretPID);
// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
// spindexPID = servo.setSpinPos(spindexPos);
// robot.spin1.setPower(spindexPID);
// robot.spin2.setPower(-spindexPID);
// }
if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) {
spinCurrentPos = servo.getSpinPos();
double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos);
robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID);
}
//TODO: Use color sensors to switch between positions...switch after ball detected in
@@ -199,7 +226,7 @@ public class TeleopV3 extends LinearOpMode {
}
spinInitPos = spinCurrentPos;
}
if (reverse && intakeTicker % intakeJamSwap < (intakeJamSwap/2)) {
if (reverse && intakeTicker % intakeJamSwap < (intakeJamSwap / 2)) {
robot.spin1.setPower(1);
robot.spin2.setPower(-1);
} else if (reverse) {
@@ -214,8 +241,19 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Reverse?", reverse);
TELE.update();
} else {
if (!servo.spinEqual(spindexPos)) {
spinCurrentPos = servo.getSpinPos();
double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos);
robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID);
} else {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
}
if (getRuntime() - intakeStamp < 1) {
robot.intake.setPower(-(getRuntime() - intakeStamp) * 2);
@@ -226,41 +264,6 @@ public class TeleopV3 extends LinearOpMode {
intakeTicker = 0;
}
// // INTAKE
// if (gamepad1.right_bumper) {
// robot.transferServo.setPosition(transferServo_out);
//// intakeTicker++;
//// if (intakeTicker % 16 == 0) {
//// spinCurrentPos = servo.getSpinPos();
//// if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) {
//// reverse = !reverse;
//// }
//// spinInitPos = spinCurrentPos;
//// }
// if (reverse) {
// robot.spin1.setPower(spinningPow);
// robot.spin2.setPower(-spinningPow);
// } else {
// robot.spin1.setPower(-spinningPow);
// robot.spin2.setPower(spinningPow);
// }
// robot.intake.setPower(1);
// intakeStamp = getRuntime();
// TELE.addData("Reverse?", reverse);
//
// } else if (gamepad1.left_bumper) {
// robot.intake.setPower(-(getRuntime() - intakeStamp) * 2);
// spindexPos = spindexer_intakePos2;
// intakeTicker = 0;
// } else {
// if (getRuntime() - intakeStamp < 1) {
// robot.intake.setPower(-(getRuntime() - intakeStamp) * 2);
// } else {
// robot.intake.setPower(0);
// }
// intakeTicker = 0;
// }
//COLOR:
double s1D = robot.color1.getDistance(DistanceUnit.MM);
@@ -276,8 +279,9 @@ public class TeleopV3 extends LinearOpMode {
double gP = green / (green + red + blue);
s1G.add(gP);
TELE.addData("gp1", gP);
if (gP >= 0.43) {
if (gP >= 0.36) {
s1.add(true);
} else {
s1.add(false);
@@ -296,6 +300,7 @@ public class TeleopV3 extends LinearOpMode {
double gP = green / (green + red + blue);
s2G.add(gP);
TELE.addData("gp2", gP);
if (gP >= 0.43) {
s2.add(true);
@@ -314,6 +319,8 @@ public class TeleopV3 extends LinearOpMode {
double gP = green / (green + red + blue);
TELE.addData("gp3", gP);
s3G.add(gP);
if (gP >= 0.43) {
@@ -336,42 +343,66 @@ public class TeleopV3 extends LinearOpMode {
green3 = checkGreen(s3, s3T);
}
// //SHOOTER:
//
// double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
//
// robot.shooter1.setPower(powPID);
// robot.shooter2.setPower(powPID);
//
// robot.transfer.setPower(1);
//
// double offset;
//
// double robX = drive.localizer.getPose().position.x;
// double robY = drive.localizer.getPose().position.y;
//
// double robotX = robX - xOffset;
// double robotY = robY - yOffset;
// double robotHeading = drive.localizer.getPose().heading.toDouble();
//
// double goalX = -10;
// double goalY = 0;
//
// double dx = goalX - robotX; // delta x from robot to goal
// double dy = goalY - robotY; // delta y from robot to goal
//
// double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
//
// desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
//
// desiredTurretAngle += manualOffset;
//
// offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset));
//
// if (offset > 135) {
// offset -= 360;
// }
//
robot.transfer.setPower(1);
double offset;
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotX = robX - xOffset;
double robotY = robY - yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -10;
double goalY = 0;
double dx = goalX - robotX; // delta x from robot to goal
double dy = goalY - robotY; // delta y from robot to goal
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
desiredTurretAngle += manualOffset;
offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset));
if (offset > 135) {
offset -= 360;
}
//SHOOTER:
double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
//VELOCITY AUTOMATIC
if (autoVel) {
vel = velPrediction(distanceToGoal);
} else {
vel = manualVel;
}
if (gamepad2.right_stick_button) {
autoVel = true;
} else if (gamepad2.right_stick_y < -0.5) {
autoVel = false;
manualVel = 4600;
} else if (gamepad2.right_stick_y > 0.5) {
autoVel = false;
manualVel = 2700;
} else if (gamepad2.right_stick_x > 0.5) {
autoVel = false;
manualVel = 3600;
} else if (gamepad2.right_stick_x < -0.5) {
autoVel = false;
manualVel = 3100;
}
// //TODO: test the camera teleop code
// double pos = turrDefault + (error / 8); // adds the overall error to the default
//
@@ -431,30 +462,7 @@ public class TeleopV3 extends LinearOpMode {
// manualOffset += 2;
// }
//
// //VELOCITY AUTOMATIC
//
// if (autoVel) {
// vel = velPrediction(distanceToGoal);
// } else {
// vel = manualVel;
// }
//
// if (gamepad2.right_stick_button) {
// autoVel = true;
// } else if (gamepad2.right_stick_y < -0.5) {
// autoVel = false;
// manualVel = 4100;
// } else if (gamepad2.right_stick_y > 0.5) {
// autoVel = false;
// manualVel = 2700;
// } else if (gamepad2.right_stick_x > 0.5) {
// autoVel = false;
// manualVel = 3600;
// } else if (gamepad2.right_stick_x < -0.5) {
// autoVel = false;
// manualVel = 3100;
// }
//
// //HOOD:
//
// if (autoHood) {
@@ -682,9 +690,9 @@ public class TeleopV3 extends LinearOpMode {
hub.clearBulkCache();
}
//
// TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
// TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
// TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
@@ -864,19 +872,4 @@ public class TeleopV3 extends LinearOpMode {
return times.get(times.size() - 1) > getRuntime() - 2;
}
// public static double velPrediction(double distance) {
// if (distance < 30) {
// return 2750;
// } else if (distance > 100) {
// if (distance > 160) {
// return 4200;
// }
// 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));
// }
}

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java
View File

@@ -6,8 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -71,4 +69,5 @@ public class PIDServoTest extends LinearOpMode {
}
}
}

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

@@ -73,12 +73,18 @@ public class FlywheelV2 {
targetVelocity = commandedVelocity;
velo = getVelo(shooter1CurPos, shooter2CurPos);
// Flywheel PID code here
if (targetVelocity - velo > 500) {
if (targetVelocity - velo > 4500) {
powPID = 1.0;
} else if (velo - targetVelocity > 500) {
} else if (velo - targetVelocity > 4500) {
powPID = 0.0;
} else {
double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
double a = 2539.07863;
double c = 1967.6498;
double d = -0.289647;
double h = -1.1569;
double feed = Math.log((a / (targetVelocity + c)) + d) / h;
// --- PROPORTIONAL CORRECTION ---
double error = targetVelocity - velo;
@@ -88,7 +94,7 @@ public class FlywheelV2 {
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
powPID = feed;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));

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

@@ -14,7 +14,7 @@ public class Servos {
//PID constants
// TODO: get PIDF constants
public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
public static double spin_scalar = 1.0086;