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 shoot2GateTime = 1.7;
public static double endGateTime = 22;
public static double waitToPickupGateWithPartner = 1;
public static double waitToPickupGateSolo = 0.2;
public static double intakeGateTime = 3;
public static double waitToPickupGateWithPartner = 0.7;
public static double waitToPickupGateSolo = 0.01;
public static double intakeGateTime = 8;
public static double shootGateTime = 1.7;
public static double shoot1GateTime = 1.7;
public static double intake1GateTime = 3.3;
@@ -111,7 +111,7 @@ public class Auto_LT_Close extends LinearOpMode {
int ballCycles = 3;
int prevMotif = 0;
boolean gateCycle = true;
boolean withPartner = false;
boolean withPartner = true;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 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;
// path profile parameters (in inches)
public double maxWheelVel = 90;
public double maxWheelVel = 70;
public double minProfileAccel = -40;
public double maxProfileAccel = 90;
public double maxProfileAccel = 70;
// turn profile parameters (in radians)
public double maxAngVel = Math.PI; // shared with path
public double maxAngAccel = Math.PI;
public double maxAngVel = 2 *Math.PI; // shared with path
public double maxAngAccel = 2 * Math.PI;
// path controller gains
public double axialGain = 4;
public double lateralGain = 4;
public double headingGain = 4; // shared with turn
public double axialGain = 6;
public double lateralGain = 6;
public double headingGain = 6; // shared with turn
public double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
@@ -345,7 +345,16 @@ public final class MecanumDrive {
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);
leftBack.setPower(0);
rightBack.setPower(0);
@@ -354,10 +363,6 @@ public final class MecanumDrive {
return false;
}
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new HolonomicController(
PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
@@ -388,7 +393,6 @@ public final class MecanumDrive {
p.put("y", localizer.getPose().position.y);
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("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));

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

@@ -129,11 +129,14 @@ public class Turret {
}
}
if (xPos != null){
limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX);
limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY);
limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ);
limelightTagH = (alphaPosConstant*hPos) + ((1-alphaPosConstant)*limelightTagH);
if (xPos != null ){
if (zPos>0) {
limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
}
}
}