FTC Need Odometry Help

Hello! First time poster here so bear with me :).

I am currently trying to implement odometry for my team’s skystone robot. We are using Wizards.exe’s pod design with E8T encoders. I have tested the encoders alot and I am sure the hardware is working fine. We are not using the GoBilda chassis and the encoders are placed differently from Wizards.exe’s tutorial.

I have written some code to track the robot’s (x, y, theta) position. This code seems to somewhat work, here are some of my observations:

  • The robot’s angle (theta) is always accurate.
  • When the robot’s orientation is constant, it updates (x, y) position accurately.
  • When the robot moves and turns at the same time, the (x, y) become inaccurate very very fast.

Here are some of my theories of issues that may be happening:

  • The calibration is off. I am not using Wizards.exe’s calibration program and have too manually calibrate. I think the “tick per degree offset” (Value for adjusting the deltaX when you turn) value may be inaccurate. Unfortunately since my vertical encoders are not in the direct center of the robot I do not have a good way of getting this value accurate.
  • The update method’s cycle time is too high. On average there is around a 100ms delay between odometry updates. I believe this causes inaccuracies over time. However, I don’t believe that this is the sole issue since I have had lower cycle times in the past without much change in accuracy.

Below is my code, but first a few notes:

  • frontRight is the DcMotor with the right (vertical) encoder.

  • intakeLeftis the DcMotor with the left (vertical) encoder.

  • backLeft is the DcMotor with the back (horizontal) encoder.

  • countsPerDegree is the constant offset for adjusting the back encoder’s value when turning.

  • x, y, and angle are fields that hold the robot’s calculated position

  • getAngle() returns the robot’s angle (in degrees) by observing the difference between the left and right encoders (this is accurate).

  • I understand I can add both the x and y displacement vectors at the same time. I am adding them separately for simplicity.
    Here is the code loop (Sorry if formatted incorrectly):

      // Get the change in tick counts.
      double changeRight = robot.frontRight.getCurrentPosition() - lastRightCount;
      double changeLeft = -robot.intakeLeft.getCurrentPosition() - lastLeftCount;
      double changeBack = robot.backLeft.getCurrentPosition() - lastBackCount;
      // Update last tick fields.
      lastRightCount = robot.frontRight.getCurrentPosition();
      lastLeftCount = -robot.intakeLeft.getCurrentPosition();
      lastBackCount = robot.backLeft.getCurrentPosition();
      // Get the change in the angle.
      double changeAngle = getAngle() - angle;
      // Update the angle.
      angle = getAngle();
      // Get the Y distance.
      double yDistance = (changeRight + changeLeft) / 2.;
      // Update x, y with the y displacement.
      x += yDistance * Math.cos(Math.toRadians(angle));
      y += yDistance * Math.sin(Math.toRadians(angle));
      // Calculate the xShift
      double xShift = (countsPerDegree * changeAngle);
      // Get x distance.
      double xDistance = changeBack - xShift;
      double xAngle = angle - 90.;
      // Update x, y with the y displacement.
      x += xDistance * Math.cos(Math.toRadians(xAngle));
      y += xDistance * Math.sin(Math.toRadians(xAngle));
    

I have spent many hours troubleshooting this and I am currently at a standstill. If anyone has an idea of why I am getting these large inaccuracies I would love to hear them. Thank you for all your help :slight_smile:

Hi @Volt! Welcome to Chief! Thank you for such an accurate description of your code and process, it really help.

You mention as the robot turns and moves at the same time, the (x, y) become very inaccurate. Have you tested to see it they become over similar tests? For example, moving the robot 3 feet and rotating 90 degrees 5 times and seeing if the inaccuracies are near the same.

Thanks for your response!

The inaccuracies are pretty much the same if the same movements are performed.

Awesome, good to know. Next, do you have a measurement of the position of the encoder wheels relative to the center of the robot? If so, I could help you develop an algorithm to solve for the correct delta.

Ill post a image of the encoders with distance labels in a bit once I get access to the robot today.

1 Like

Here is the image. I took some new measurements by putting the robot on top of a clear surface and measuring the distance below.

Also, the encoder wheels have 307.699557 counts per inch