Chief Delphi - [FTC]: I really need help on gyro!!!

They gyro sensor reads the CHANGE in degrees of the rotation of your sensor. in order to get the number of degrees your robot has traveled, you need to integrate it over the number of cycles.

Start first by calibrating your gyro sensor to the current heading. My Gyro sensor's name is HTGYRO

Code:

HTGYROstartCal(HTGYRO);

This will set the current heading as the center point

now we need to create variables for the current heading and the elapsed time

Code:

  float heading = 0.0;

  long currTime;

  long prevTime = nPgmTime;

now, we create a while loop with the main code to determine the number of degrees turned by the robot

Code:

while(true)

  {

    currTime = nPgmTime;

    heading += ((float)HTGYROreadRot(HTGYRO))*(currTime-prevTime)/1000;

first we set the current time to the amount of time the program has been running on.

Then we take the heading (currently 0) and add the degrees rotated by the change in time (comes in as milliseconds, we divide by 1000 to change to seconds).

Code:

prevTime = currTime;

Then we set the value of the previous time equal to the time after our calculation.

Then we put in the logic for looking at the direction to turn.

Code:

if(turningRight) //If you want to turn to the right

{

  if(heading < (target - buffer)) // If the # of degrees turned is less than the target value...

  {

    turnRight(drivePower); //Turn Right

  }else if(heading > (target + buffer)) // If the # of degrees turned is greater than the target value...

  {

    turnLeft(drivePower); // Turn to the left

  }else if(heading < (target + buffer) && heding > (target - buffer)) // If in the "buffer zone"...

  {

    stopMotors(); // Stop the robot

    wait1Msec(waitTime);

    return;

  }

}else if(direction == 2) // If turning to the left...

{

  if(heading < (target - buffer)) // If # of degrees turned is less than target...

  {

    turnLeft(drivePower); // Turn to the left

  }else if(heading > (target + buffer)) // If # of degrees turned is greater than target...

  {

    turnRight(drivePower); // Turn to the right

  }else if(heading < (target + buffer) && heading > (target - buffer)) // If in "buffer zone"

  {

    stopMotors(); // Stop the robot

    wait1Msec(waitTime);

    return;

  }

}

So because the integration will give us only POSITIVE values, we need to account for the fact that sometimes we may want to turn left and other times we may want to turn right. This is why we have the "turningRight" check. Then we look at the different logic. If we are turning right, and we have 70 degrees turned so far, we need to turn right still. If we have 100 degrees turned so far, we need to turn left still. If we are turning left, and we have 70 degrees turned so far, we need to turn left still. If we have 100 degrees turned so far we need to turn right still.

The buffer values are put in to ensure you dont have the "jiggle" that often happens because the robot shoots past the target while turning then has to compensate backwards again. This is essentially a "dead-zone" where the robot will stop moving if you are within a few degrees of the target.

So, the entire function I have written is as follows:

Code:

void gyroTurn(int target, int buffer, int drivePower, bool turningRight, int waitTime)

{

  stopMotors();

  wait1Msec(waitTime);

  HTGYROstartCal(HTGYRO);



  float heading = 0.0;

  long currTime;

  long prevTime = nPgmTime;



  while(true)

  {

    currTime = nPgmTime;

    heading += ((float)HTGYROreadRot(HTGYRO))*(currTime-prevTime)/1000;

    prevTime = currTime;

    if(direction == 1)

    {

      if(heading < (target - buffer))

      {

        turnRight(drivePower);

      }else if(heading > (target + buffer))

      {

        turnLeft(drivePower);

      }else if(heading < (target + buffer) && heading > (target - buffer))

      {

        stopMotors();

        wait1Msec(waitTime);

        return;

      }

    }else if(direction == 2)

    {

      if(heading < (target - buffer))

      {

        turnLeft(drivePower);

      }else if(heading > (target + buffer))

      {

        turnRight(drivePower);

      }else if(heading < (target + buffer) && heading > (target - buffer))

      {

        stopMotors();

        wait1Msec(waitTime);

        return;

      }

    }

  }

}

You will have to put in your own drive code where it says

Code:

turnLeft(drivePower);

and

Code:

turnRight(drivePower);

If you have any question feel free to ask them! Hope this was helpful!