From d72db8b6138fc54cc019d0ff2a710ae551876053 Mon Sep 17 00:00:00 2001
From: daviddudas <david.dudas@outlook.com>
Date: Wed, 15 Jan 2020 19:44:18 +0100
Subject: [PATCH] mecanum odometry fix
---
src/mecanum_drive/odometry.py | 10 +++++-----
1 file changed, 5 insertions(+), 5 deletions(-)
diff --git a/src/mecanum_drive/odometry.py b/src/mecanum_drive/odometry.py
index 24a55c5..93d817c 100755
--- a/src/mecanum_drive/odometry.py
+++ b/src/mecanum_drive/odometry.py
@@ -50,12 +50,12 @@ class Odometry:
rearRightTravel = self.rearRightEncoder.getDelta() / self.ticksPerMeter
deltaTime = newTime - self.lastTime
- deltaXTravel = (frontLeftTravel + frontRightTravel + rearLeftTravel + rearRightTravel) / 4
- deltaYTravel = (-frontLeftTravel + frontRightTravel + rearLeftTravel - rearRightTravel) / 4
- deltaTheta = (-frontLeftTravel + frontRightTravel - rearLeftTravel + rearRightTravel) / (4 * self.wheelSeparation + self.wheelSeparationLength)
+ deltaXTravel = (frontLeftTravel + frontRightTravel + rearLeftTravel + rearRightTravel) / 4.0
+ deltaYTravel = (-frontLeftTravel + frontRightTravel + rearLeftTravel - rearRightTravel) / 4.0
+ deltaTheta = (-frontLeftTravel + frontRightTravel - rearLeftTravel + rearRightTravel) / (2 * (self.wheelSeparation + self.wheelSeparationLength))
- self.pose.x += deltaXTravel
- self.pose.y += deltaYTravel
+ self.pose.x += deltaXTravel*cos(self.pose.theta) - deltaYTravel*sin(self.pose.theta)
+ self.pose.y += deltaYTravel*cos(self.pose.theta) + deltaXTravel*sin(self.pose.theta)
self.pose.theta = (self.pose.theta + deltaTheta) % (2*pi)
self.pose.xVel = deltaXTravel / deltaTime if deltaTime > 0 else 0.
self.pose.yVel = deltaYTravel / deltaTime if deltaTime > 0 else 0.
--
GitLab