How do you do Position Tracking?

VEX V5 General Discussion Technical Discussion
3

Take a look at this snip: https://hastebin.com/amuqixaruf.php

I took that from team WPI1’s autonomous robot code here: Team-Optimistic · GitHub

It works by computing the delta’s for left and right quad encoders for one time step (how far each encoder has moved since we last checked), recording the average and difference of these two values, then using it to compute our delta theta (how much we have rotated since we last checked). Then we compute our delta x and delta y in the global frame (field frame in this case), our velocity in the local frame (base_link frame), and then sums the new numbers from this time step into a running estimate. Here is the code linked above broken down line by line:

Compute our right and left change:

const int32_t rightDelta = (rightQuad - lastRightQuad),
      leftDelta = (leftQuad - lastLeftQuad);

Save the new quad values for the next loop:

lastRightQuad = rightQuad;
      lastLeftQuad = leftQuad;

Compute the average change and difference in change:

const float avg = (rightDelta + leftDelta) / 2.0,
                  dif = (rightDelta - leftDelta) / 2.0;

Compute the distance we moved and the angle we turned:

const float dist = (avg * straightConversion) / 1000.0, //robots coordinate frame
      dtheta = dif * thetaConversion;

Compute our new theta:


const float theta = thetaGlobal + dtheta;

Compute how far we moved in the global frame:

const float dx = cos(theta) * dist, //world coordinate frame
                  dy = sin(theta) * dist;

Compute our velocity in the local frame:

const float v = 1000* dist / dt,
                  vtheta = 1000 * dtheta / dt;

Here we construct a message for ROS. The important parts to see are the first line (we say that our linear velocity straight forward in the local frame is


v

) and last two lines one up from the last (we increment our global theta by


dtheta

and say our angular velocity around the straight upwards axis (z axis) is


vtheta

):

odom->twist.twist.linear.x = v;
      odom->twist.twist.linear.y = 0;
      odom->twist.twist.linear.z = 0;
      odom->twist.twist.angular.x = 0;
      odom->twist.twist.angular.y = 0;
      thetaGlobal += dtheta;
      odom->twist.twist.angular.z = vtheta;
      odom->twist.covariance = ODOM_TWIST_COV_MAT;

Here we make a second ROS message. Notice that we increment our global x and y positions by


dx

and


dy

, tell ROS about our new global position, and tell ROS our theta is


thetaGlobal

:

//Pose
      xPosGlobal += dx;
      yPosGlobal += dy;
      odom->pose.pose.position.x = xPosGlobal;
      odom->pose.pose.position.y = yPosGlobal;
      odom->pose.pose.position.z = 0;
      odom->pose.pose.orientation = tf::createQuaternionMsgFromYaw(thetaGlobal);
      odom->pose.covariance = ODOM_POSE_COV_MAT;