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

moved velocity calculations to the localizers

Browse Source
This commit is contained in:
Anyi Lin
2024-11-21 17:13:30 -05:00
parent 9711a268e5
commit 089fc3ae68
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -262,9 +262,10 @@ public class PoseUpdater {
*/
public Vector getVelocity() {
if (currentVelocity == null) {
currentVelocity = new Vector();
currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY());
currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9)));
// currentVelocity = new Vector();
// currentVelocity.setOrthogonalComponents(getPose().getX() - previousPose.getX(), getPose().getY() - previousPose.getY());
// currentVelocity.setMagnitude(MathFunctions.distance(getPose(), previousPose) / ((currentPoseTime - previousPoseTime) / Math.pow(10.0, 9)));
currentVelocity = localizer.getVelocityVector();
return MathFunctions.copyVector(currentVelocity);
} else {
return MathFunctions.copyVector(currentVelocity);