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);