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

stash for dany

Browse Source
This commit is contained in:
Keshav Anand
2026-02-23 19:42:34 -06:00
parent 0264cf2c77
commit 7a2b275e66
3 changed files with 29 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -68,9 +68,9 @@ public class Auto_LT_Close extends LinearOpMode {
public static double intake2TimeGate = 3; public static double intake2TimeGate = 3;
public static double shoot2GateTime = 1.7; public static double shoot2GateTime = 1.7;
public static double endGateTime = 22; public static double endGateTime = 22;
public static double waitToPickupGateWithPartner = 1; public static double waitToPickupGateWithPartner = 0.7;
public static double waitToPickupGateSolo = 0.2; public static double waitToPickupGateSolo = 0.01;
public static double intakeGateTime = 3; public static double intakeGateTime = 8;
public static double shootGateTime = 1.7; public static double shootGateTime = 1.7;
public static double shoot1GateTime = 1.7; public static double shoot1GateTime = 1.7;
public static double intake1GateTime = 3.3; public static double intake1GateTime = 3.3;
@@ -111,7 +111,7 @@ public class Auto_LT_Close extends LinearOpMode {
int ballCycles = 3; int ballCycles = 3;
int prevMotif = 0; int prevMotif = 0;
boolean gateCycle = true; boolean gateCycle = true;
boolean withPartner = false; boolean withPartner = true;
double obeliskTurrPos1 = 0.0; double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0; double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0; double obeliskTurrPos3 = 0.0;

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
View File

@@ -201,18 +201,18 @@ public final class MecanumDrive {
public double kA = 0.00008; public double kA = 0.00008;
// path profile parameters (in inches) // path profile parameters (in inches)
public double maxWheelVel = 90; public double maxWheelVel = 70;
public double minProfileAccel = -40; public double minProfileAccel = -40;
public double maxProfileAccel = 90; public double maxProfileAccel = 70;
// turn profile parameters (in radians) // turn profile parameters (in radians)
public double maxAngVel = Math.PI; // shared with path public double maxAngVel = 2 *Math.PI; // shared with path
public double maxAngAccel = Math.PI; public double maxAngAccel = 2 * Math.PI;
// path controller gains // path controller gains
public double axialGain = 4; public double axialGain = 6;
public double lateralGain = 4; public double lateralGain = 6;
public double headingGain = 4; // shared with turn public double headingGain = 6; // shared with turn
public double axialVelGain = 0.0; public double axialVelGain = 0.0;
public double lateralVelGain = 0.0; public double lateralVelGain = 0.0;
@@ -345,7 +345,16 @@ public final class MecanumDrive {
t = Actions.now() - beginTs; t = Actions.now() - beginTs;
} }
if (t >= timeTrajectory.duration) { Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
if ((t >= timeTrajectory.duration && error.position.norm() < 2
&& robotVelRobot.linearVel.norm() < 0.5)
|| t >= timeTrajectory.duration + 0.5) {
leftFront.setPower(0); leftFront.setPower(0);
leftBack.setPower(0); leftBack.setPower(0);
rightBack.setPower(0); rightBack.setPower(0);
@@ -354,10 +363,6 @@ public final class MecanumDrive {
return false; return false;
} }
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new HolonomicController( PoseVelocity2dDual<Time> command = new HolonomicController(
PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain, PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
@@ -388,7 +393,6 @@ public final class MecanumDrive {
p.put("y", localizer.getPose().position.y); p.put("y", localizer.getPose().position.y);
p.put("heading (deg)", Math.toDegrees(localizer.getPose().heading.toDouble())); p.put("heading (deg)", Math.toDegrees(localizer.getPose().heading.toDouble()));
Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
p.put("xError", error.position.x); p.put("xError", error.position.x);
p.put("yError", error.position.y); p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble())); p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));

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

@@ -130,12 +130,15 @@ public class Turret {
} }
} }
if (xPos != null ){ if (xPos != null ){
if (zPos>0) {
limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ); limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH); limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
} }
} }
}
public double getBearing() { public double getBearing() {
tx = 1000; tx = 1000;