Odometry converting the local to global

Programming Support
,
#1

Hey, I am just writing my own odometry and I am just stuck on covertion my local variables to global.
My code look like this for now-

// encoders values
int L_ENC_value = 0.0;
int M_ENC_value = 0.0;

// change in encoders
int lDelta = 0.0;
int mDelta = 0.0;

// previous encoders values
int last_lDelta = 0.0;
int last_mDelta = 0.0;

// distance in inches
float lDist = 0.0;
float mDist = 0.0;

// The current angle of the bot (RADIANS)
float currentAbsoluteOrientation = 0;
// The previous angle of the bot (RADIANS)
float last_Theta = 0;

// change in the angle(RADIANS)
float deltaTheta = 0;

// average theta for the arc
float avgThetaForArc = 0;

// local position
float x_pos_local = 0;
float y_pos_local = 0;

// the global postion in arc
float delta_x_pos_global = 0;
float delta_y_pos_global = 0;

// global position of the robot
float x_pos_global = 0;
float y_pos_global = 0;

// total local position
float t_x_pos_local = 0;
float t_y_pos_local = 0;

int odomtrack()
{
  while (true)
  {

    // getting values from encoders
    L_ENC_value = L_Encoder.get_value();
    M_ENC_value = M_Encoder.get_value();

    // get the change of the values
    lDelta = L_ENC_value - last_lDelta;
    mDelta = M_ENC_value - last_mDelta;

    // trasform values to inches
    lDist = lDelta / degrees_side_per_inch;
    mDist = mDelta / degrees_side_per_inch;

    // calculate the current angle of the robot in radians
    currentAbsoluteOrientation = (int)inertial.get_rotation() * drive_pi / 180;

    // get change in the angle
    deltaTheta = currentAbsoluteOrientation - last_Theta;

    // transform my current values to previous values
    last_lDelta = L_ENC_value;
    last_mDelta = M_ENC_value;
    last_Theta = currentAbsoluteOrientation;

    // if there is no angle change, then it is only change in encoders
    if (deltaTheta == 0)
    { 
      y_pos_local = lDist;
      x_pos_local = mDist;
      
    }
    // calculate the x-position and y-position
    else
    {
      y_pos_local = (lDist / deltaTheta + dRobotSide) * (2 * sinf(deltaTheta / 2.0));
      x_pos_local = (mDist / deltaTheta + dRobotMiddle) * (2 * sinf(deltaTheta / 2.0));
    }

    t_x_pos_local += x_pos_local;
    t_y_pos_local += y_pos_local;
   


 
    // printing the values
    lcd::print(4, "pl:%d, pm:%d, pt:%d", last_lDelta, last_mDelta, deltaTheta);//left encoder delta, middle encoder delta, theate delte(radians)
    lcd::print(5, "ty:%f,tx:%f",t_y_pos_local, t_x_pos_local);// total y local, total, x local
    lcd::print(6, "lenc:%d, menc:%d",L_ENC_value, M_ENC_value);// values from left encoder, values from middle encoder

    delay(20);
  }

  return 1;
}

Any tips or help are welcome

1 Like
#2

Which variables do you think are local and want to make global? As long as a variable is declared (int ___ = __; or float ___;) outside of all functions (with some exceptions), it’s global and can be accessed by every function.

#3

Actually, that is not the problem. I make these variables global, so they can be change in fuctuon and also keep the changed.

The problem is I want to convert my odometry local values to my global values. I know there is some calculation.

#4

You mean you want to know the math to convert x and y movement to global x and y coordinates and heading?

I don’t know that. Try searching the forum and if that doesn’t solve it, ask someone who was helping in another thread.

3 Likes
#5

you want to rotate the local delta x and delta y by the opposite of the robot’s local heading. Then add them to the global x and y.

To rotate you can either use a rotation matrix, or convert to polar coordinates and then rotate and then back to rectangular.

For further explanation please see the E-Pilons odometry document:http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf

or feel free to ask here

2 Likes
#6

I saw that however I will need more explanation cause I did not understand that.

Please, if you know someone, who can help, just let me know.

#7

okay so have variables called globalX and globalY and start with them equal to 0.

Then we’re going to rotate the local coordinates by the opposite of the robots heading.

So take it’s heading and multiply it by negative one.

Then we’re going to use this formula to do the rotation.

globalDeltaX = x * cos(oppHeading) - y * sin(oppHeading)
globalDeltaY = x * sin(oppHeading) + y * cos(oppHeading)

globalX += globalDeltaX
globalY += globalDeltaY

5 Likes
#8

should be the oppheading still in radians or degrees?

I thinks in deg but I want to make sure

Also, wil this works with the x and y distance in inch?

It look similar to 23880E code, which use instead inch the radian (rotation of wheels).

Nevertheless, thanks

#9

should be the oppheading still in radians or degrees?

I suggest you google whether c++'s cosine and sine functions take radians or degrees. I believe it’s radians but double check me.

Also, wil this works with the x and y distance in inch?

Yes this works for all units

I’m not sure what this means lol

Glad to be of some help

2 Likes
#10

hell yeah,

It is working. Sometimes, it is 1 inch off; however, it is not big deal.

Thanks