/// <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); }