Odometry Tracking Multiple Positions

Programming Support
#1

I am the programmer for 32789B (middle school) and I was wondering how I could get my Odometry code to track multiple points. I can already get it to do one. I am pretty new with Odometry so any help would be greatly appreciated.

float LeftPosition;
float RightPosition;
float Orientation;
float TotalDistance;
float Radius;
float LateralDistance;
float XPosition;
float YPosition;
float PrevX;
float PrevY;

bool enableOdometry = true;

int Odometry(){
while (enableOdometry){

LeftPosition = (LeftEncoder.position(degrees)/360) * 12.56;
RightPosition = (RightEncoder.position(degrees)/360) * 12.56 * -1;
Orientation = (LeftPosition - RightPosition)/12;
TotalDistance = (LeftPosition + RightPosition)/2;
Radius = TotalDistance/Orientation;
LateralDistance = (Radius * sin(Orientation/2)) * 2;
XPosition = (LateralDistance * cos(Orientation/2));
YPosition = (LateralDistance * sin(Orientation/2));

vex::task::sleep(100);

}

return 1;

}

#2

I’m reformatting your code so it will be easier to read. In the future, you can put triple tick marks (```) on either side of the code to format it nicely.

float LeftPosition;
float RightPosition;
float Orientation;
float TotalDistance;
float Radius;
float LateralDistance;
float XPosition;
float YPosition;
float PrevX;
float PrevY;

bool enableOdometry = true;

int Odometry(){

while (enableOdometry){

LeftPosition = (LeftEncoder.position(degrees)/360) * 12.56;
RightPosition = (RightEncoder.position(degrees)/360) * 12.56 * -1;
Orientation = (LeftPosition - RightPosition)/12;
TotalDistance = (LeftPosition + RightPosition)/2;
Radius = TotalDistance/Orientation;
LateralDistance = (Radius * sin(Orientation/2)) * 2;
XPosition = (LateralDistance * cos(Orientation/2));
YPosition = (LateralDistance * sin(Orientation/2));

vex::task::sleep(100);

}

return 1;

}
#3

I’m not exactly sure what you mean when you say you want the code to track multiple points. Do you mean you want the robot to drive to multiple different points during the autonomous run?

As a side note, I would recommend changing the wait time vex::task::sleep(100) to a lower value like 10 or 20 to increase the accuracy of your odometry by taking readings more frequently.

1 Like
#4

Yeah. I want it to track a point, and then track another one without resetting the position to zero. Also thanks for the advice.

#5

Assuming your code correctly tracks your tracking center, you could just measure the displacement of the second point from your tracking center, and you would always know its relative position.

2 Likes
#7

Good idea. I will try it.