Help needed using inertial sensor to turn accurately

void turn::turn4(double d) 
{
  Inertial20.calibrate();

  while (Inertial20.isCalibrating()) 
  {
wait(100, msec);
  }

  if(d > 0) 
  { //spin direction: ---->
left_back.spin(forward);
left_front.spin(forward);
right_back.spin(reverse);
right_front.spin(reverse);

waitUntil((Inertial20.rotation() >= d));
left_back.stop();
left_front.stop();
right_back.stop();
right_front.stop();
  } 
  else 
  { //spin direction: <----
left_back.spin(reverse);
left_front.spin(reverse);
right_back.spin(forward);
right_front.spin(forward);

waitUntil((Inertial20.rotation() <= d));
left_back.stop();
left_front.stop();
right_back.stop();
right_front.stop();
  }
}

Am I doing this right? Im not really sure how to use the inertial sensor to get more accurate turns.

The code you have looks like it might work, but it is not optimal.

First, you don’t need to calibrate inertial sensor before each turn. You could do it once, when you start the program. Even before the autonomous, if you are using competition template.

Second, instead of going with the same (full) power all the way forward or reverse, you should really spin with power proportional to how far you are from your target (the ‘P’ in PID).

This way, not only the movement is going to be less jerky, but you could combine handling of both spin directions into a single piece of code where you pass either negative or positive velocity to the motor, which sign will come naturally from subtracting the current position from the desired target.

Take a look at various PID code examples posted on the forum: https://www.vexforum.com/search?q=PID+example

7 Likes

Thanks! I will try this!

It looks like this code would get you stuck in an infinite loop of turning back and forth. You’d need an error range to kill the waituntil. You can generate an error by subtracting your current angle by the angle you want to turn too.

Then like Weilin suggested you can use proportional control to multiply the error by a constant to get a power output.

Somthing like:

while(error > marginoferror){
error = targetangle - currentangle;
power = error * kP;

if(power > maxpower){
power = maxpower;
}

leftdrive(fwd, power);
rightdrive(rev, power);

}


Mind you, to an extent, this is purposefully vague. There are lots of improvements you could introduce for more accurate / faster turns like full PID control instead of P control or special exit conditions or whatever your heart desires.

1 Like

Thanks! Ill try this!

One note: you will need to use abs(error) for main loop exit condition and abs() or two if() statements to clip the max power, because error could be either positive or negative.

Also, it is always a good idea to add a small delay (i.e. sleep() statement) at the end of the body of PID loop.

1 Like

Calibrating the inertial sensor is a very slow process, and it only has to be done once

1 Like

Is this better?

{
    int dt = 20;
    double target = degrees;
    double error = target - Inertial20.rotation();
    double kP = 0;
    double kD = 0;
    double prevError = error;
    
    while (std::abs(error) > 1)
    {
      error = target - Inertial20.rotation();
      double derivative = (error - prevError)/dt;
      double percent = kP * error + kD * derivative;
      left_back.spin(directionType::fwd, percent, pct);
      left_front.spin(directionType::fwd, percent, pct);
      right_back.spin(directionType::rev, percent, pct);
      right_front.spin(directionType::rev, percent, pct);
      vex::task::sleep(dt);
      prevError = error;
    }
    left_back.stop();
    left_front.stop();
    right_back.stop();
    right_front.stop();
    left_back.resetRotation();
    left_front.resetRotation();
    right_back.resetRotation();
    right_front.resetRotation();
}```
1 Like

Just make sure kP and kD aren’t zero, otherwise the robot won’t move.

1 Like

It doesn’t matter if kD is zero, since kD can’t even move the robot if operating by itself and is simply added to the output value. kP however can’t be zero

2 Likes

alrighty, thank you!

You are so close, but in my opinion you should make it so that it exits if the error is within a margin for a period of time, as compared to if its within the margin. You may pass the point without actually settling at the point.

Sorry, you lost me. Could you explain what you mean? Thanks.

If you keep track of the current speed then you can exit the loop earlier if the speed is close to zero and you know that you are not going to overshoot.

Once you start experimenting you will know that your robot is going to stop in “x” units of position when it runs with small speed of “v” units and you cut the power.

3 Likes