Can't figure out why my odometry code is acting weird (first time user and unsure how to separately post code)

Programming Support
1

This is my first year coding in c++ and my teams first year using it to code a rotation sensor for autonomous

The goal is to create a loop where the rotation sensor # will increase as the robot runs, and then the motors will all stop once they reach a certain distance traveled

However, its just jumping straight to the end number its set to stop at and does not run at all(regardless of the number)

rotation Rotation1 (PORT7, true);

  rotation Rotation2 (PORT8, true);

  double positionLeft = Rotation1.position(turns);

  double positionRight = Rotation2.position(turns);

  void DriveForwardWithOdometry(double targetdistance) {

    double kp = 0.1;

    double initialLeft = positionLeft;

    double initialRight = positionRight;

    double CurrentDistance = 0.0;

    while (CurrentDistance < targetdistance) {

      double CurrentLeft = positionLeft - initialLeft;

      double CurrentRight = positionRight - initialRight;

      CurrentDistance += (CurrentLeft + CurrentRight)/2;

    Brain.Screen.setCursor(6,1);

    Brain.Screen.print(CurrentDistance);

      FL.spin(reverse);

      FR.spin(forward);

      BL.spin(forward);

      BR.spin(reverse);

  positionLeft = Rotation1.position(turns);

  positionRight = Rotation2.position(turns);

   }

  FL.stop();

  FR.stop();

  BL.stop();

  BR.stop();

  }
2

Well, a couple of things to think about and fix.

reading positions outside of your while loop

and using CurrentDistance before you calculate it

3

I’m not sure if they do what I think that do, but I want Currentdistance to be set to “0” each time the code starts to go to a new set distance, so that’s why I have it reset to “0” every time it runs a new number and I have the double positionleft read by the code every time it repeats in the while loop. I just initialize it in the beginning.

4

You are adding your distance since you started to the current distance every time.

Each time you loop, you are adding (CurrentLeft + CurrentRight)/2 to CurrentDistance. However, CurrentLeft and CurrentRight each represent how far you have driven in total since the function started (on that wheel). They are already cumulative, so the += is redundant. You should either change the += to an =, or keep track of the last position of each side every time.

5

Thank you for your advice. The problem with that is that when i do that, “Currentdistance” doesn’t update outside of the loop, and it will continue to read as 0 regardless of how far it actually goes. Is there a way I could code it differently so that it doesn’t do that?(I’ve been researching and I found a command called a “do while” loop. I don’t have access to my robot currently but I was wondering if that might work instead of a regular while loop?)

Ex.

do {

      double CurrentLeft = positionLeft - initialLeft;

      double CurrentRight = positionRight - initialRight;

      CurrentDistance = (CurrentLeft + CurrentRight)/2;

    Brain.Screen.setCursor(6,1);

    Brain.Screen.print(CurrentDistance);

      FL.spin(reverse);

      FR.spin(forward);

      BL.spin(forward);

      BR.spin(reverse);

  positionLeft = Rotation1.position(turns);

  positionRight = Rotation2.position(turns);

    }

    while (CurrentDistance < targetdistance);
6

I don’t entirely understand what you mean. Are you keeping track of your last motor values? In that case, you should make sure to declare them outside of the loop so that they are not cleaned up at the end of each iteration. Could you post that code and what it is doing?

A do while loop would not help in the case you seem to be describing. Do whiles are only different in that they check the condition at the end of the loop rather than the start.

7

Also, be careful that when you are calling the function, it has the newest position values. If the values in your variables are stale, then your code could perceive a huge jump in the values of positionLeft and positionRight. You should resample those values at the start of your function.