Code:
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:
Code:
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
