Odometry Help, Crying rn

Programming Support VEXcode V5 Tech Support
#1

Ok so I’m trying really hard to make a working position tracking program, but its not working properly. The x and y positions don’t update correctly and I have no idea why. The facing theta angle seems to be just fine though. I’m using three wheels and an inertial sensor. I’m using the inertial sensor to calculate the heading instead of the two wheels.

I’m going to link my updatePostion function here. This method is run in the competition userControl while loop. All variables are default set to zero accept for the Sl Ss Sr and wheel diameter.

I would greatly appreciate if someone could take a look or try running it to see what’s going on lol. I’m thinking it might be in my equations but I honestly have no clue at this point.

Thanks so much, - Team 23880A

int curLeft = 0;
int curRight = 0;
int curSide = 0;

int deltaLeft = 0;
int deltaRight = 0;
int deltaSide = 0;

int lastLeftPos = 0;
int lastRightPos = 0;
int lastSidePos = 0;

double pi = 3.14;
double wheelDiameter = 2;

double deltaLr = 0;
double deltaRr = 0;

double thetaNew = 0;

double Sl = 1.5;
double Sr = 1.5;
double Ss = 5;

double deltaTheta = 0;

double ang = 0;

double deltaX = 0;
double deltaY = 0;

double thetaM = 0;

double theta = 0;
double radius = 0;

double x = 0;
double y = 0;

void updatePosition(){

  curLeft = lefter.position(degrees);
  curRight = righter.position(degrees);
  curSide = back.position(degrees);

  deltaLeft = (curLeft - lastLeftPos) / 360 * pi * wheelDiameter;
  deltaRight = (curRight - lastRightPos) / 360 * pi * wheelDiameter;
  deltaSide = (curSide - lastSidePos) / 360 * pi * wheelDiameter;

  lastLeftPos = curLeft;
  lastRightPos = curRight;
  lastSidePos = curSide;

  deltaLr = (curLeft) / 360 * pi *wheelDiameter;
  deltaRr = (curRight) / 360 * pi * wheelDiameter;

  //thetaNew = (thetaReset + (deltaLr - deltaRr)/ (Sl + Sr));

  thetaNew = Inertial1.heading(degrees) * (pi / 180);
  deltaTheta = thetaNew - ang;

  if(deltaTheta == 0){
    deltaX = deltaSide;
    deltaY = deltaRight;
  }else{
    deltaX = (2*sin(deltaTheta/2))*(deltaSide/deltaTheta + Ss);
    deltaY = (2*sin(deltaTheta/2))*(deltaRight/deltaTheta + Sr);
  }

  thetaM = ang + deltaTheta/2;

  theta = deltaTheta/2;
  radius = sqrt(deltaX*deltaX + deltaY*deltaY);
  theta = theta-thetaM;
  deltaX = radius *cos(theta);
  deltaY = radius*sin(theta);

  ang = thetaNew;
  x = x - deltaX;
  y = y + deltaY;

  Brain.Screen.setCursor(1,1);
  Brain.Screen.print(x);

  Brain.Screen.setCursor(2,1);
  Brain.Screen.print(y);

  Brain.Screen.setCursor(3,1);
  Brain.Screen.print(ang);

}

edit: mods added code tags

2 Likes
#2

First you should comment your code, so others can understand what you are doing. Second please format code by enclosing with “```”.

Here is the document may help you.

3 Likes
#3

Hello,
I am trying to understand your code and how it works but it is hard for me to figure out what is going on. Would it be possible to take a picture of your drivetrain base and help me understand what is your motor and sensors setup?

1 Like
#5

Were using the regular 3 wheel odometry with 2 parallel wheels facing the front of the robot and one side wheel perpendicular to those two. We also have an inertial sensor that is being used to find the heading of the robot rather than the equation using the two wheels wheel which is commented out in the code. I’m using the inertial sensor because in my opinion it seems a lot more accurate. Sorry the robot is at my friends house.

#6

Thanks I didn’t comment anything because the program goes in order of the steps in this book. Ha but yea I know comments make stuff a lot easier lol.

#7

This part doesn’t make sense to me. The “theta” variable that goes into the deltaX and deltaY should be calculated using atan2() on the local X and Y changes. So instead of that you would have
theta=atan2(deltaX,deltaY);

4 Likes
#8

It’s actually atan2(y, x) instead of atan2(x, y). You can read more about it here atan2 - C++ Reference.

5 Likes
#9

yeah my bad

#10

Ha unfortunately this doesn’t work ;(

#11

Could you be more specific, please? What exactly is the robot doing? What values are being printed and what are you expecting?

2 Likes
#13

Yea sorry, I just send a video of the numbers changing. I don’t know a good way to explain it

#14

Ok so I figured out the main issue. I had some of the variables set to ints which I didn’t even think about when making it lol. It solved my wierd values issue. But now whats going is the robot is only displaying its local variables, with the tangent equation you gave me (thanks btw helped very much lol). So when i push it forward when its facing any direction the y postion goes up no matter the angle. Is this an easy fix?

#15

Gotta post your code again or we can’t see what’s wrong

1 Like