Odometry Help Please!

Programming Support
#1

I’m trying to develop a positioning system for our skills runs and to use odometry with three senor wheels. I understand the code isn’t completely done, but its not even changing my x and y values. Ha if anyone knows what I’m doing wrong it would be greatly appreciated thanks. Code is under this.

double tl = 8.125;
double tr = 8.125;
double ts = 6;
double delR = 0;
double delL = 0;
double delS = 0;

double x = 0;
double y = 0;
double facing = 0;

void usercontrol(void) {
// User control code here, inside the loop
while (1) {

delR = (RIGHT.position(degrees)/360)/6.283; //6.283 being the circumference of the 2 in tracking wheels

delL = ((LEFT.position(degrees))/360)*6.283;

delS = ((BACK.position(degrees))/360)*6.283;

facing += (delL - delR)/(16.25);

if (facing == 0){
  x += delS;
  y += delR;
} else{
  x += (2*(sin(facing/2))*((delS/facing)+ts)); //add value to positions
  y += (2*(sin(facing/2))*((delR/facing)+tr));
}




Brain.Screen.setCursor(5,1);
Brain.Screen.print("facing: ");
Brain.Screen.print(facing);

Brain.Screen.setCursor(6,1);
Brain.Screen.print("x: ");
Brain.Screen.print(x);

Brain.Screen.setCursor(7,1);
Brain.Screen.print("y: ");
Brain.Screen.print(y);

Brain.Screen.setCursor(1,1);
Brain.Screen.print("RIGHT: ");
Brain.Screen.print((double)(RIGHT.position(degrees)));

Brain.Screen.setCursor(2,1);
Brain.Screen.print("LEFT: ");
Brain.Screen.print((double)(LEFT.position(degrees)));

Brain.Screen.setCursor(3,1);
Brain.Screen.print("BACK: ");
Brain.Screen.print((double)(BACK.position(degrees)));

RIGHT.setPosition(0,degrees); //reset sensors for re analyzation
LEFT.setPosition(0,degrees);
BACK.setPosition(0,degrees);

}

#2

Do you know if the odometry wheels are reading change?

#3

It seems to me that the calculations within the if statements is incorrect. Imagine that your robot is angled at 45 degrees, dS and dL no longer represent changes in the x and y axis. Therefore, directly adding the displacement to the global coordinate is mathematically incorrect.

The calculations in the else statement is also incorrect due to the same reason above. deltaS doesn’t exclusively affect the x axis reading. The linear / side wheels both affect the x and y value when the robot has a heading other than zero degrees.

It also looks like you don’t have any delay in your loop. Encoders take time to update: resetting them infinitely will make them unable to read any values. Instead of resetting the encoders every iteration, I suggest recording the encoder values from the previous iteration in another variable, and calculate dTick by subtracting the new encoder reading to the previous one.

Overall, there are quite a lot of issues associated with your position tracking code. I will strongly suggest going through the math again.

5 Likes