Tracking Rotation Using Tracker Wheels

My team is using 3 wheels for odometry. We are trying to use the left and right tracker wheels to track the angle the robot is facing. We followed this guide and the BLRS wiki, but the angle tracking isn’t that accurate. It is sometimes over 50 degrees off. Is there a better algorithm for this or should we stick with using the inertial sensor? We used the inertial sensor last season, but it was often a few degrees off by the end of a skills run.

Here is our code:

#define DEG_TO_IN (2.75 * M_PI / 360)
#define sL 5.4
#define sR 5.4

void runOdom(){
   float lastLeft = Left.position(deg);
   float lastRight = RightEncoder.position(deg);
   float xPos = 0;
   float yPos = 0;

   while(true){

       float left = LeftEncoder.position(deg);
       float right = RightEncoder.position(deg);
       float deltaLeft = (left - lastLeft) * DEG_TO_IN;
       float deltaRight = (right - lastRight) * DEG_TO_IN; 

       angle += (deltaLeft - deltaRight) / (sL + sR);

       xPos += (sin(angle * M_PI / 180) * (vertical - lastVertical) +
                sin((90 + angle) * M_PI / 180) * (horizontal - lastHorizontal)) *
               DEG_TO_IN;
       yPos += (cos(angle * M_PI / 180) * (vertical - lastVertical) +
                cos((90 + angle) * M_PI / 180) * (horizontal - lastHorizontal)) *
               DEG_TO_IN;

       lastAngle = angle;
       lastLeft = left;
       lastRight = right;

       wait(20, msec);
  }
}

Is the angle moving at a consistent rate? Such as at 90 it reads 180 and at 180 it reads 360?

No, the angle is not consistently off. Sometimes it is really accurate and other times it is as far as 50 degrees off.

I understand that. But when is it moving randomly or is it moving with some kind of relationship between the numbers?

Inertial sensor is gonna be 1000% more accurate (most likely) (99.99% sure)

It always stays in the general area of where it should be, but it isn’t always accurate. If the robot turns right, the angle will correctly move in that direction, but it wouldn’t always move the exact amount it is supposed to.

Are your physical tracking wheels fine?

You might want to try changing your value by a constant. For example, multiply your angel by 0.5 before using it to account for error.

We already tried something like this. It isn’t scaled. One value of sL and sR makes it work well for when we rotate the robot 1080 degrees, but it will be far off when it rotates a few more times.

Check your physical trackers. Make sure they rotate freely when spun, they don’t drag on the field tiles, and that they don’t have dust or something (depending on if you’re using red encoders). You can test this by spinning them and looking at the brain screen’s readout. Also, inertial sensor works like a charm if you throw a couple wall resets into your code.

1 Like

The trackers spin freely. We will probably just use an inertial sensor and reset the angle with the wall instead.