public bool Run()
{
bool done = false;
double heading = m_chassis.Get_heading(); //Finds the headingits traveling
double error = DesiredHeading - heading; //Finds how far off the angle the bot is
double distance = m_chassis.Get_distance();
if (m_chassis.Is_Bumper_Switch_Pressed())
{
m_chassis.drive(0, 0); //Stops the bot when bumper pressed
Deliver_Mechanism.setState(Deliver.DELIVERSTATE.Deliver); //Delivers the balls
Deliver_Mechanism.Run();
done = true;
}
else
{
if (DistanceToDrive < SlowDownDistance)
{
right_correction = Drive_Speed * (1 + (Correction * error)); //Corrects the right motor
left_correction = Drive_Speed * (1 - (Correction * error)); //Corrects the left motor
m_chassis.drive(right_correction, left_correction); //If else statement finds if the bot has hit its target. If it hasn't, then it keeps driving
}
else if (distance < TargetDistance) //Sets the speed slower so the bot can hit the target more accurate
{
right_correction = SlowDownSpeed * (1 + (Correction * error)); // The speed the right motor is going when at slow down
left_correction = SlowDownSpeed * (1 - (Correction * error)); // The speed the left motor is going when at the slow down
m_chassis.drive(right_correction, left_correction);
}
else if (distance < ExtraDistance) //Sets the speed slower so the bot can hit the target more accurate
{
right_correction = ExtraSpeed * (1 + (Correction * error)); // The speed of the right motor is going when at extra distance
left_correction = ExtraSpeed * (1 - (Correction * error)); // The speed of the left motor is going when at extra distance
m_chassis.drive(right_correction, left_correction);
}
else
{
m_chassis.drive(0, 0); //Stops the motors
Deliver_Mechanism.setState(Deliver.DELIVERSTATE.Deliver); //Delivers the balls
Deliver_Mechanism.Run();
done = true;
}
}
return(done);
}
