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

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2025-12-31 16:39:43 -06:00
parent 61f314d71d
commit 713bafd9b4
1 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -73,13 +73,13 @@ public final class MecanumDrive {
public double kA = 0.000046; public double kA = 0.000046;
// path profile parameters (in inches) // path profile parameters (in inches)
public double maxWheelVel = 120; public double maxWheelVel = 180;
public double minProfileAccel = -30; public double minProfileAccel = -40;
public double maxProfileAccel = 120; public double maxProfileAccel = 180;
// turn profile parameters (in radians) // turn profile parameters (in radians)
public double maxAngVel = 2* Math.PI; // shared with path public double maxAngVel = 4* Math.PI; // shared with path
public double maxAngAccel = 2* Math.PI; public double maxAngAccel = 4* Math.PI;
// path controller gains // path controller gains
public double axialGain = 4; public double axialGain = 4;
@@ -185,19 +185,19 @@ public final class MecanumDrive {
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos), (leftFrontPosVel.position - lastLeftFrontPos),
leftFrontPosVel.velocity, leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick), }).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(leftBackPosVel.position - lastLeftBackPos), (leftBackPosVel.position - lastLeftBackPos),
leftBackPosVel.velocity, leftBackPosVel.velocity,
}).times(PARAMS.inPerTick), }).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(rightBackPosVel.position - lastRightBackPos), (rightBackPosVel.position - lastRightBackPos),
rightBackPosVel.velocity, rightBackPosVel.velocity,
}).times(PARAMS.inPerTick), }).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos), (rightFrontPosVel.position - lastRightFrontPos),
rightFrontPosVel.velocity, rightFrontPosVel.velocity,
}).times(PARAMS.inPerTick) }).times(PARAMS.inPerTick)
)); ));
lastLeftFrontPos = leftFrontPosVel.position; lastLeftFrontPos = leftFrontPosVel.position;
@@ -454,14 +454,14 @@ public final class MecanumDrive {
public PoseVelocity2d updatePoseEstimate() { public PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update(); PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose()); poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) { while (poseHistory.size() > 100) {
poseHistory.removeFirst(); poseHistory.removeFirst();
} }
estimatedPoseWriter.write(new PoseMessage(localizer.getPose())); estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel; return vel;
} }