SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Remove mecanum forward kinematics rotation factors

Browse Source
This commit is contained in:
Ryan Brott
2023-09-10 09:12:50 -07:00
parent b12b121447
commit dc1b43e13f
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -136,19 +136,19 @@ public final class MecanumDrive {
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
(leftFrontPosVel.position - lastLeftFrontPos),
leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(leftRearPosVel.position - lastLeftRearPos) + kinematics.trackWidth * headingDelta,
(leftRearPosVel.position - lastLeftRearPos),
leftRearPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightRearPosVel.position - lastRightRearPos) - kinematics.trackWidth * headingDelta,
(rightRearPosVel.position - lastRightRearPos),
rightRearPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos) - kinematics.trackWidth * headingDelta,
(rightFrontPosVel.position - lastRightFrontPos),
rightFrontPosVel.velocity,
}).times(PARAMS.inPerTick)
));