Inertial sensor turn help

Programming Support VEXcode Pro V5 Text Tech Support
1

Hi everyone. We have the code below that we are using for turning out drivetrain. When we are running this code, our robot turns randomly and does not turn and stop. We are using an inertial sensor for its gyro functionality. Any help would be greatly appriciated.

void turnP(float angle) {
  Inertial.resetRotation();
  float Kp = 0.3;
  angle = angle / 2;
  if (angle < 0) {
    while (Inertial.rotation(deg) > angle) {
      double gError = abs(angle - Inertial.rotation(deg));
      float speed = gError * Kp;
      leftBack.spin(fwd, -speed, velocityUnits::pct);
      leftFront.spin(fwd, -speed, velocityUnits::pct);
      rightFront.spin(fwd, speed, velocityUnits::pct);
      rightBack.spin(fwd, speed, velocityUnits::pct);
    }
  } else if (angle > 0) {
    while (Inertial.rotation(deg) < angle) {
      double gError = abs(angle - Inertial.angle(deg));
      float speed = gError * Kp;
      leftBack.spin(fwd, speed, velocityUnits::pct);
      leftFront.spin(fwd, speed, velocityUnits::pct);
      rightFront.spin(fwd, -speed, velocityUnits::pct);
      rightBack.spin(fwd, -speed, velocityUnits::pct);
    }
  } else {
  }
  leftBack.stop();
  leftFront.stop();
  rightFront.stop();
  rightBack.stop();
}
2

I think you shouldn’t call abs() for gError statement, since you need to know the sign of the error. You want to have something like this:

while(true)
{
   double gError = angle - Inertial.rotation(deg);
   if( abs(gError) < 3)  // we will stop within 3 deg from target
   {
      break;
   }
   double speed = gError * Kp;
   leftBack.spin(fwd, -speed, velocityUnits::pct);
   ...
}

leftBack(stop);
...
3

For my intertial turns, I set the rotation to the opposite of the given angle. This means that the angle I want to turn to is 0 degrees. This let’s me use a ratio of rotation/angle to get pretty good motion profiling.

4

@weilin could you please explain why did you make this specific calculation for the speed ?

5

This is a standard PID notation for P- loop, inherited from the original post. Kp is the coefficient that scales velocity proportional to the gError, or angle to target from the current position.

Unlike the heading() that could jump between 0 and 359 deg, InertialSensor.rotation() returns continuous value.