/// <summary>
/// Drives the robot to a location according to an <see cref="IMotionController"/>
/// </summary>
/// <param name="drive"></param>
/// <param name="controller"></param>
/// <param name="location"></param>
/// <param name="tolerance"></param>
/// <param name="brake"></param>
/// <param name="interval"></param>
public static void DriveToDistance(this IEncoderDrive drive, IMotionController controller, double location, double tolerance, bool brake, double interval = 0.02)
{
controller.SetPoint = location;
while (Math.Abs(location - drive.Encoders.LinearDistance) > tolerance)
{
double power = controller.Get(drive.Encoders.LinearDistance);
drive.SetPowers(power, power);
AccurateWaitSeconds(interval);
}
if (brake)
drive.SetPowers(0, 0);
}
/// <summary>
/// Turns the robot to an absolute angle using a <see cref="SimplePID"/>
/// </summary>
/// <param name="d"></param>
/// <param name="controller"></param>
/// <param name="angle"></param>
/// <param name="tolerance"></param>
/// <param name="brake"></param>
public static void TurnToAngle(this IGyroscopeDrive d, IMotionController controller, double angle, double tolerance, bool brake, double interval = 0.02)
{
controller.SetPoint = angle;
while (Math.Abs(d.Gyroscope.GetAngle() - angle) > tolerance)
{
double power = controller.Get(d.Gyroscope.GetAngle());
d.SetPowers(power, -power);
AccurateWaitSeconds(interval);
}
if(brake)
d.SetPowers(0, 0);
}
/// <summary>
/// Drives the robot at a set speed to a location
/// </summary>
/// <param name="drive"></param>
/// <param name="speedController"></param>
/// <param name="speed"></param>
/// <param name="location"></param>
/// <param name="brake"></param>
/// <param name="interval"></param>
public static void DriveToAtSpeed(this IEncoderDrive drive, IMotionController speedController, double speed, double location, bool brake, double interval = 0.02)
{
speedController.SetPoint = drive.Encoders.LinearDistance < location ? speed : -speed;
while ((drive.Encoders.LinearDistance - location)*Math.Sign(speedController.SetPoint) > 0)
{
double power = speedController.Get(drive.Encoders.LinearSpeed);
drive.SetPowers(power, power);
AccurateWaitSeconds(interval);
}
if (brake)
drive.SetPowers(0, 0);
}
/// <summary>
/// Drives the robot in a straight line for a set time using a <see cref="IMotionController"/> to correct heading
/// </summary>
/// <param name="d"></param>
/// <param name="correction"> <see cref="IMotionController"/> to use for correcting heading</param>
/// <param name="power"></param>
/// <param name="time"></param>
/// <param name="brake"></param>
public static void DriveStraightForTime(this IGyroscopeDrive d, IMotionController correction, double power, double time, bool brake, double interval = 0.02)
{
correction.SetPoint = d.Gyroscope.GetAngle();
Stopwatch s = new Stopwatch();
while (s.Elapsed.TotalSeconds < time)
{
double correctingPower = correction.Get(d.Gyroscope.GetAngle());
d.SetPowers(power + correctingPower, power - correctingPower);
AccurateWaitSeconds(interval);
}
if(brake)
d.SetPowers(0, 0);
}
