Gyro Programming

[Straberrie]

Hi, how are you?

I am trying to get gyros to work and I never programmed the sensor before, so I made use of the FIRST programming guide to implement gyros in the autonomous period.

Gyro *gyro;
gyro = new Gyro (1);

gyro->Reset();
gyro->SetSensitivity(.005);

float angle = gyro->GetAngle();

myRobot->Drive(-1.0, -angle / 30);
Wait(4.0);

According to the guide, the above should have corrected the robot's path and set it to match the heading of the original angle. Since the original angle was 0, it should have went straight.

However, when we tested it on the robot, it immediately made a sharp turn.

In order to see what is happening with the angle values, I created an if statement

float angle = gyro->GetAngle();

if (angle<0.5)
{
myRobot->Drive(1.0, 0.0);
Wait(0.0);
}
else
{
myRobot->Drive(0.0, 0.0);
Wait(0.0);
}

And it stood, I switched the statements around, and the robot didnt move either. So, I am wondering what I did wrong and how I can make the robot follow a straight path.

Any help would be appreciated,

Thank you!

[biojae]

if (angle<0.5)
{
myRobot->Drive(1.0, 0.0);
Wait(0.0);
}
else
{
myRobot->Drive(0.0, 0.0);
Wait(0.0);
}

if there is any sort of noise, or movement of the robot,
then the angle will be larger then 0.5,
when that happens, the robot will just sit where it is without moving
( like you said)

What I would do is make a PID controller (I don't know how to use WPI's)
sortof like this:

float desiredHeading = 0;
float speed = 0.5;

const float P = .5; // You will need to tune these values 
const float I  = 0.0; // as they are just made up
const float D = 0.0;

float headingIntegral = 0.0;
float prevHeadingError = 0.0;

void autonomousPeriodic()
{
    float heading = fmod(gyro->GetHeading(), 360.0); // limit to + - 360 // EDIT: forgot the denominator
    float headingError = (desiredHeading - heading);
    headingIntegral += headingError;
    float headPID = (P * headingError) +
                              (I * headingIntegral) +
                                  (D * (headingError - prevHeadingError));
    prevHeadingError = headingError;
    myRobot->TankDrive(speed + headPID, speed - headPID);
}

The speed variable is the forward movement that you want

The real advantage of this method as opposed to the wpi example is that you can set any heading you want
(At least within the range of - 360 to 360)

Just remember to tune the PID

[Straberrie]

oh wow O.O XD Thank you so much!!!

[biojae]

Originally Posted by Straberrie

oh wow O.O XD Thank you so much!!!

You are quite welcome.

I fixed a small error in the code, i forgot the denominator in the fmod operator.
(360, by the way)