GyroTurn Proportional Control

Making a Turn using the V5 IMU

here we introduce basic and somewhat advanced code and sensor concepts to create a function that makes precise turns. This will be done in 2 steps, the first will demonstrate use of the sensor making a right turn only. The second will extend the example to making both right and left turns.

Object of type inertial

Create device object of type inertial and name it Gyro. Choose any port. i use a capital letter since this is a Global name.

Drive Function

following good code practice where a function only does one thing it is expected that you already have a drive function like this example:

void drive(int lspeed, int rspeed, int wt)
{
  LeftMotor.spin(forward, lspeed, percent);
  RightMotor.spin(forward, rspeed, percent);
  wait(wt,msec);
}

This is for a simple two motor tank drive. The code may be extended to multiple motors on right and left side as long as only one speed input is needed per side. if you are using x-drive your code will be different.

Heading

the heading is the direction the robot is facing. 0 degrees is straight ahead, to the right is positive angles, to the left is negative angles. 

 float heading = Gyro.rotation(deg);

Reads the value of rotation angle from the inertial sensor into the variable named heading.

Here is a simple function to read and print the heading to the brain screen.

void gyroPrint()
{
float heading = Gyro.rotation(deg);
Brain.Screen.printAt(1, 60, "heading  =  %.2f. degrees", heading);
}

To test the gyro call this from your while true in driver control

 gyroPrint();

Target

Target is the desired angle you want the robot to turn. It can be positive for a right turn or negative for a left turn.

The concept of the gyroTurn function

• turn the robot a little bit
• measure the heading
• compare the heading to target
• repeat this until we get to the target
• when at target stop.

The above is pseudocode just showing the steps the code needs to take.

Right Turn

Here is code to make a right turn. Since the speed is hard coded it will only turn to the right.

void gyroTurnRight(float target)
{
float heading=0.0; //initialize a variable for heading
Gyro.setRotation(0.0, degrees);  //reset Gyro to zero degrees
while(heading<=target)
{
  drive(50, -50, 10); //turn right at half speed
  heading=Gyro.rotation(deg);  //measure the heading of the robot
}
drive(0, 0, 0);  //stop the drive
}

Call this function in your auton using

  gyroTurnRight(90);

to make a 90 degree turn.

Positive or Negative Targets and the Concept of Error

One of the reasons we can not make a left turn with target a negative number is the condition we are using in the while loop

while(heading<=target)

When target is negative and heading starts at zero this condition is already false and we exit the while loop without making the turn.

Instead we want to look at something called error which is the difference between our desired target and our heading. This is a number that get smaller in magnitude the closer we are to the target. For left turns it is negative, for right turns positive. We take care of this by looking at the absolute value of the error.

to make an accurate turn we want to reduce this error to a small value but not zero. If we try to make the value of error 0 we will need to stop exactly at 0 and that really means all balls. 0.0000000000000. so it will never happen instead we allow for some accuracy like 2 or 5 degrees and make our condition like this:

float heading=0.0; //initialize a variable for heading
float accuracy=2.0; //how accurate to make the turn in degrees
float error=target-heading;

while(fabs(error)>=accuracy)

This function is set up to exit when the turn is within + or - 2 degrees of the target.

Right or Left Turns and the Concept of Proportional Speed

To turn right or left we need to set the speed correctly, we can not just hard code the value it needs to be calculated from the value of error. This way when we have a negative value for error it will turn to the left and a positive value of error will turn to the right.

The math for this is sppeed = kp x error, where kp is some value that is the proportional constant. To start we select the value of kp to make the speed about 10 percent when we error is at our value of accuracy, so when accuracy = 2 select kp = 5.0

here is the code:

void gyroTurn(float target)
{
float heading=0.0; //initialize a variable for heading
float accuracy=2.0; //how accurate to make the turn in degrees
float error=target-heading;
float kp=5.0;
float speed=kp*error;
Gyro.setRotation(0.0, degrees);  //reset Gyro to zero degrees
while(fabs(error)>=accuracy)
{
  speed=kp*error;
  drive(speed, -speed, 10); //turn right at half speed
  heading=Gyro.rotation();  //measure the heading of the robot
  error=target-heading;  //calculate error
}
drive(0, 0, 0);  //stope the drive
}

Debugging and Tuning

If you are unsure about how the code is working you might change the condition in the while loop to while(true). this will demonstrate turning with overshoots and settling in. In this condition you can adjust the values of kp and see what happens. You may also try to adjust the values of accuracy , make it something bug and then slowly reduce it till it stops working. There wil be a balance between the value of accuracy and kp.

Stop Function

for this lesson it is assumed that the motors are stopping in brake mode if not set it with a brake function.

void driveBrake()
{
  LeftMotor.stop(brake);
  RightMotor.stop(brake);
}