Odometry not returning values

I was attempting to write an absolute positioning system using odometry for our team because we are planning on doing a turret, but it keeps returning nan in the print console instead of the values that it should be producing. Any ideas or possible solutions would be immensely helpful.
Odometry .pdf (28.7 KB)

Are the values printing correctly to the brain screen?

No, the x and y values on the brain just sit at 0.0, but the gyro value is reading the same as on the computer

In the loop you set the x and y values equal to the math but you should be doing x=x+math and y=y+math.

1 Like

I havent checked the math so i dont know how correct that is, but because you are getting back NAN (not a number), it seems like the problem would likely be that you didnt initialize a variable, and so when ever it does the math that variable is NAN, making it equate to nan.

1 Like

Hey! I’ve had a similar problem before while developing odometry. The biggest problem right now is that while setting your rR and bR variables, you are dividing by a Dir_diff, which would be 0 if none of the encoders have moved yet. This division by 0 is probably where you’re getting the nans from. Second, it seems that you’re only calculating the local offsets, and that they aren’t being rotated to a global offset. I wish you luck in your turret journey!

1 Like

Did I hear my name

Ngl though don’t use blocks for a turret. I guess it’s better to prototype something like odom on but the amount of stuff you’re gonna have going into it is… a lot. Imo use PROS, people say it’s harder to learn but tbh it really isn’t, plus it’s very nice and kind and sweet because of okapi lib (in built Odometry???)

1 Like

It’s not in blocks…

20 char

1 Like

[0_0] so here’s the thingggg I may have just looked at the pic lolll still point stands for anyone else doing a turret

1 Like

I am pasting your code here wrapped in the [code] ... [/code] tags:

#include "vex.h"
#include "math.h"
#define PI 3.14159265
using namespace vex;
//Odometry Task///////////////////////////////////////////////////////////
//Initial values
double initial_Dir;
double initial_x;
double initial_y;
//X and Y coordinates
double x;
double y;
//Current values
double Dir;
double l_dist;
double r_dist;
double b_dist;
//Previous values
double prev_Dir;
double prev_L;
double prev_R;
double prev_B;
//Combo values
double Dir_diff = Dir - prev_Dir;
double r_diff = r_dist - prev_R;
double l_diff = l_dist - prev_L;
double b_diff = b_dist - prev_B;
double rR; //translation value
double bR;
double l_dfc = 5; //left track wheel distance from center
double r_dfc = 5; //right track wheel distance from center
double b_dfc = 5; //back track wheel dist from center
double twc = 10.2102; //track wheel circumference - (3.25 * 3.14159)
int OdometryTask()
      l_dist = LTrack.position(turns) * twc;
      r_dist = RTrack.position(turns) * twc;
      b_dist = BTrack.position(turns) * twc;
      Dir = (((l_dist - r_dist) / (l_dfc + r_dfc)) * (180 / 3.14159)) + initial_Dir;
      rR = (r_diff / Dir_diff) + r_dfc;
      bR = (b_diff / Dir_diff) + b_dfc;
      x = (2 * sin(Dir / 2)) * bR;
      y = (2 * sin(Dir / 2)) * rR;
      Brain.Screen.printAt(175, 60, "Gyro %.3f", Dir); //Output to brain screen
      Brain.Screen.printAt(175, 120, "x %.1f", x);
      Brain.Screen.printAt(175, 180, "y %.1f", y);
      Controller1.Screen.setCursor(3, 1);
      Controller1.Screen.print("Gyro %.2f", Dir);
      printf("Gyro %.4f", Dir);
      printf(" X_Pos %.2f", x);
      printf(" Y_Pos %.2f\n", y);
      prev_Dir = Dir;
      prev_L = l_dist;
      prev_R = r_dist;
      prev_B = b_dist;
vex::task o(OdometryTask); //Start Odometry Task

Is this your entire code?
Do you have main() function?
What are the LTrack, RTrack, and BTrack sensors?

My guess would be that, first, you need to have variables like Dir_diff initialized to some non-zero values to avoid division on the first iteration and, second, ensure that rotation sensors are properly setup and given enough time to initialize.

See here:


This is not my entire code, it’s just the part that I figured would have the problem in it. Thank you for putting the code in, I’m fairly new to the forum and had no clue how to do that. The rotation sensors are not the three wire kind, but are the kind that uses the smart motor ports. They are currently initialized to zero, which from everything that has been said seems to be a major part of the problem, but now that I’m thinking about it, wouldn’t the dir_diff, l_diff, and r_diff variables return to zero if the robot didn’t move in the cycle time of 15 msec? And would I be able to fix that by simply adding a constant to the value or would I need to have something else that would completely prevent that number from being zero?

Thanks for all the ideas, I’ll be sure to try them all out