search

acmerobotics / road-runner-quickstart

FTC quickstart for https://github.com/acmerobotics/road-runner
BSD 3-Clause Clear License
168 stars 855 forks source link

heading is wrong with ThreeTrackingWheelLocalizer #60

Closed jeevan-navudu closed 4 years ago

jeevan-navudu commented 4 years ago

I am using the RoadRunner-QuickStart project. Below is my Robot configuration

  1. Using Mecanum wheels
  2. Using three tracking wheels
  3. Tracking wheels are not positioned in the common configuration.
  4. Tracking wheels are placed a) The lateral distance between the wheels is 14 inches b) Vertical wheels are 2.5 inches behind the center of the robot c) This leaves the configuration for wheel 1 as (-2.5, 7.0, 0) and wheel 2 as (-2.5, -7.0, 0)
  5. A horizontal tracking wheel is placed on the right side of the robot along with the vertical wheel. the wheel is 2.5 inches front on x-axis to the center of the robot and 7.375 inches on y axis. This leaves the configuration for third wheel as (2.5, -7.375, Math.toRadians(270)). Angle is set to 270 instead of 90 to set the direction of encoder correctly.

With the above configuration when I run Localization test, X and Y are reporting good but the heading is showing OK for very little time and soon goes out of sync. After couple of rotations approximately -30 degrees shows as 0 degrees. (I was printing both in radians and degrees on telemetry). Just to summarize my robot is as below

X|-------Front--------|R
L|                    |I
E|                  HW|G
F|LW                RW|H
T|                    |T
X|--------Rear--------|

LW - Left tracking wheel RW - Right tracking wheel HW - Horizontal tracking wheel

Does the code work for the above configuration?

rbrott commented 4 years ago

Yes, the wheel positions and orientations you gave are consistent with your physical description. You may want to "tune" the heading by adjusting the distance between the left/right vertical wheels (similar to the built-in track width tuner).

jeevan-navudu commented 4 years ago

Thank you, I will do that tomorrow and let you know the results. For the heading calculation, is only the track width needs to be tuned? what about the x values of the three wheels? Is third wheel position data used at all in heading calculation?

rbrott commented 4 years ago

Yes, only the track width needs to be tuned. The x-positions of the left/right wheels are irrelevant. The x-position of the third wheel is used, while the y-position is not used. You can check this by computing the forward kinematics matrix yourself.

jeevan-navudu commented 4 years ago

Changing the track width by a small value fixed the error. Closing this ticket. I will see if I could write a test op to tune the trackwidth of the localizer.