Odometry Program Issue (VEXcode Pro V5)

Programming Support VEXcode Pro V5 Text Tech Support
, , , ,
1

Hi all,

My team and I have been trying to create an Odometry-based program on VEXcode Pro V5 to track the absolute coordinate position of our robot, but we are running into multiple odd issues… After extensive debugging, all the variables (including deltaX and deltaY) up until xPos and yPos are functioning as intended in that they are appropriately changing values when we move the robot. However, xPos and yPos (which we intend to be storing the absolute coordinate position of the robot) seem to have multiple issues, as noted within the code below (#1: double/float variable types are not working; #2: unwanted reset occurring)…

If anyone has any ideas regarding how to resolve these issues, that would be much much appreciated!

#include "vex.h"
#include "autonfunctions.h"   

// fields
double currentLeft;
double currentRight;
double previousLeft;
double previousRight;
double deltaL;
double deltaR;
double deltaX;
double deltaY;
double linearDistance;
double head;
double theta;
double lradius;

// constants
const double trackWidth = 4.625;

int getPos(double& xPos, double& yPos) {
  
  while (true) {

    // odometryWheelToInch is a constant global to all our program files... its value is (2.75 * M_PI) / 360
    currentLeft = getLeftEncoderRotation() * odometryWheelToInch; 
    currentRight = -getRightEncoderRotation() * odometryWheelToInch; 
    deltaL = currentLeft - previousLeft;
    deltaR = currentRight - previousRight;
    
    theta = ((deltaR - deltaL) / trackWidth);
    lradius = deltaL / theta;
    linearDistance = 2 * (lradius + (trackWidth / 2)) * sin(theta / 2);

    deltaX = linearDistance * cos(head + theta / 2);
    deltaY = linearDistance * sin(head + theta / 2);

    // Issue #1: When we change the type of these variables (tempX & tempY) to a double or float (as we intend to do), 
    //           xPos and yPos no longer change (they always remain at 0).
    // Issue #2: When tempX & tempY are integers as written below, xPos and yPos change when we move the robot; however, with this, (1) all
    //           precision is lost, and (2) xPos and yPos reset to zeroes when the robot stops moving. These values should never 
    //           reset--they should always store the absolute coordinate position of the robot on the field.
    int tempX = xPos;
    int tempY = yPos;

    head += theta;
    xPos = tempX + deltaX; // We tried xPos += deltaX, but that didn't work for some reason; 
                           // thus, we stored its original value in tempX (above) and used that... Same for Y below...
    yPos = tempY + deltaY;
    
    previousLeft = currentLeft;
    previousRight = currentRight;
    wait(10, msec); 

  }
    return 0;
}

void skills() {

  double xPos = 0;
  double yPos = 0;

  getPos(xPos, yPos); // We intend to run this line as a thread so that it can update
                      // xPos & yPos (localized to this skills() function) as the program 
                      // is running. We plan to reference xPos & yPos frequently in other functions
                      // throughout our program (by passing them in as parameters) 
                      // in order to determine the robot's path of motion. 
                      
}

Thank you so very much!

2

I cant tell you what your issue is, but I can tell you that if you intend to use xPos and yPos elsewhere in your code you should use Mutexes. Tbose variables are also double defined, first globally then as locals in skills(). Also I dont see previousLeft and previousRight being set anywhere.

3

That’s probably not necessary when using VEXcode.
However, you are right about too many variables called xPos and yPos.

4

probably not necessary with how the task scheduling works but its generally good practice fkr writing safe threaded code. or maybe thats just all the rust talking for me.

5

Thank you so much for catching those issues! We tested the updated code with the implemented changes (I’ve also edited the code in my original post), but we are unfortunately still seeing the same problems… Will you please tell me more about Mutexes/any other suggestions you have? Thanks again!!

6

the mutexes won’t fix your problem. the other thing i see wrong with this code is that a lot of your variables are global when they dont need to be. the deltas, the currentLeft and currentRight, lineardistance, theta, and lradius should all be local. I’m not sure exactly how clang treats global variables, but I know ive had issues with trying to read and write the same global var in quick succession.

7

what can i say? a broken clock is right twice a day.

8

slow the loop down a bit and add some printf statements to examine the variables at various places in the code. One possible issue is this.

    theta = ((deltaR - deltaL) / trackWidth);
    lradius = deltaL / theta;
    linearDistance = 2 * (lradius + (trackWidth / 2)) * sin(theta / 2);

if deltaR and deltaL were the same value (perhaps both 0), theta could go to 0, Then lradius could become NaN as you have a divide by 0 and cause all sorts of issues.

9

So, there are a 2 problems that I see,

#1 you need a case for when the change in angle is 0. This is why I think the two variables reset when you stop moving. Usually your program will just crash if you divide by zero but VEX code is weird.

#2 tempX and tempY always will return 0 if they are a float because when you print them I assume you forgot to switch from %d to %f, happens to me a lot.

10

Thank you so much!! Handling the divide by 0 issue solved many of our problems…

11

Thank you so much–this was very helpful.

13

read the 5225A doc again if you dont understand how to code it

14

In the future, it’s probably best for you to make your own post rather than reviving a 4 month old one. Also, odometry is fairly complicated and not something that most teams have, and isn’t something people are likely to just show you exactly how to make or give you; you’re going to have to figure some things out on your own. If you have a more specific question I’d be happy to answer it, but I don’t think anyone will just show you how to do the whole thing.

15

you have the reason man, thanks for the advice, i will work in the odometry program and learning more about it, if i have some specific questions, i will post the questions, thanks