Turning P-Loop Help

Programming Support PROS Support
1 
void pointturnv2 (double angle)
{
  double currentangle;

  double leftpow;
  double rightpow;
  double kP = .835;

  while (!((currentangle<(angle+1.2)) && (currentangle>(angle-1.2))))
  {
      currentangle = gyro.get_rotation();

      printf("ANGLE (%f)", angle);
      printf("CURRENTANGLE (%f)", currentangle);
      printf("leftpow (%f)", leftpow);
      printf("rightpow (%f)", rightpow);



      if (currentangle > angle)
      {
        leftpow = kP*(angle-currentangle);
        rightpow = kP*(angle-currentangle);

        LBM.move_velocity(leftpow);
        LFM.move_velocity(leftpow);

        RBM.move_velocity(-rightpow);
        RFM.move_velocity(-rightpow);

      }

      if (angle > currentangle)
      {
        leftpow = kP*(angle-currentangle);
        rightpow = kP*(angle-currentangle);

        LBM.move_velocity(leftpow);
        LFM.move_velocity(leftpow);

        RBM.move_velocity(-rightpow);
        RFM.move_velocity(-rightpow);
      }


      pros::delay(5);
  }

  pros::delay(100); //An attempt to give the robot a chance to settle.
  LBM.move_velocity(0);
  LFM.move_velocity(0);
  RFM.move_velocity(0);
  RBM.move_velocity(0);

}

So basically the issue I need help with is as follows:

This is just meant to take a positive or negative value and turn to that angle. The issue is that it seems to be able to successfully turn to -180 degrees, but cannot then turn back to zero from that point. In fact, it seems to be unable to turn to -1 degrees also. It doesn’t even move at all as far as I’m aware, the issue isn’t that it is simply overshooting or something. Not sure why.

Anybody have an idea? It might be good to test the code and see if you replicate my issue, I’m unable to tell what’s wrong by looking at the code and running through it line by line… Probably a stupid mistake on my part but who knows.

1 Like
2 
void pointturnv2 (double angle)
{
  double currentangle=gyro.get_rotation();

  double leftpow;
  double rightpow;
  double kP = .835;

  while (!((currentangle<(angle+1.2)) && (currentangle>(angle-1.2))))
  {
      currentangle = gyro.get_rotation();

      printf("ANGLE (%f)", angle);
      printf("CURRENTANGLE (%f)", currentangle);
      printf("leftpow (%f)", leftpow);
      printf("rightpow (%f)", rightpow);



      if (currentangle > angle)
      {
        leftpow = kP*(angle-currentangle);
        rightpow = kP*(angle-currentangle);

        LBM.move_velocity(leftpow);
        LFM.move_velocity(leftpow);

        RBM.move_velocity(-rightpow);
        RFM.move_velocity(-rightpow);

      }

      if (angle > currentangle)
      {
        leftpow = kP*(angle-currentangle);
        rightpow = kP*(angle-currentangle);

        LBM.move_velocity(leftpow);
        LFM.move_velocity(leftpow);

        RBM.move_velocity(-rightpow);
        RFM.move_velocity(-rightpow);
      }


      pros::delay(5);
  }

  pros::delay(100); //An attempt to give the robot a chance to settle.
  LBM.move_velocity(0);
  LFM.move_velocity(0);
  RFM.move_velocity(0);
  RBM.move_velocity(0);

}

Problem solved. I think anyway. Don’t recommend attempting to program by yourself in the middle of the night while sleep deprived if at all avoidable. The problem appears to be that the currentangle variable did not have a value before the loop and was preventing the loop from running because of this. Not sure what the variable defaults to without initialization, but it was enough to mess it up it seems. Correct code up above for anyone who wants fast/accurate but somewhat primitive inertial sensor turning code.

3 Likes
3 
void setDrive (float left, float right){
        LBM.move_velocity(left);
        LFM.move_velocity(left);
        RBM.move_velocity(right);
        RFM.move_velocity(right);
}

void turn (double target){
  double currentAngle=gyro.get_rotation();

  double power, threshold = 1.2;
  double kP = .835;

  while (fabs(currentAngle) > threshold ){
      currentAngle = gyro.get_rotation(); //ditto
      
      power = kP*(target-currentAngle); //error*constant

      if (currentAngle > target) { 
        setDrive(power,-power); //turn right
      }else{
        setDrive(-power,power); //ditto
      }

       //debug
      printf("Target (%f)", target);
      printf("CURRENTANGLE (%f)", currentAngle);
      printf("Power (%f)", power);

      pros::delay(5);
  }
setDrive(0,0);
}

Here, I simplified your code and made it easier for you to debug. Notice how I eliminated redundancies –– you’ll want to do that so you can catch your mistakes more easily. You also might want to include comments.

6 Likes
4

I too am having an issue with my PID loop for turning. I followed Connor’s tutorial, but I am attempting to use the inertial sensor, instead of the built-in encoders as the tutorial did. I have been unable to get the loop to work. I noticed that the format of the code above is very different from my code. Would it be wise for me to do a P loop in the style of this one instead of what I tried?

This was my post if you want more information on what I tried.

2 Likes
5

Well, I would help you if I could but your code is a bit complicated for me. I will say that if you want something to work, this code is simple enough that it’s basically foolproof. Try this code and see if it is accurate enough for you.

6

I have been looking at your code and it seems to make a lot of sense. I will try this code, and see if it works. I apologize that my code was confusing. I have yet to really develop “the best practices” yet.

7

Don’t worry that’s just me. And trust me I don’t have the greatest practices either.