CircleSurround
A prebuilt utility class designed to "surround" a circle. The main purpose of this class is to calculate the closest point on a circle's circumference to a given robot position. It provides methods to generate target points and trajectories for a robot to move towards and align with a circle.
Key Features
- Find Closest Point: Calculates the point on a circle's edge nearest to the robot's current position.
- Heading Control: Can generate target points with headings directed towards the circle's center, matching the robot's current heading, or a custom-defined heading.
- Trajectory Generation: Creates
LinearTrajectoryorFastTrajectoryobjects to guide the robot to the calculated closest point on the circle.
Methods
closestPoint(PointXYZ robotPosition, PointXY center, double radius)
Calculates the point on the circle's circumference that is closest to the robot's position. The heading of the returned point is set to face the center of the circle.
closestPointBetweenAngles(PointXYZ robotPosition, PointXY center, double radius, Angle minimumAngle, Angle maximumAngle)
Finds the closest point on a circular arc (defined by a minimum and maximum angle) to the robot's position. The heading of the returned point faces the center of the circle.
closestPointWithoutHeading(PointXYZ robotPosition, PointXY center, double radius)
Similar to closestPoint, but the returned point retains the same heading as the robot's current position.
closestPointWithCustomHeading(PointXYZ robotPosition, PointXY center, double radius, Angle customHeading)
Calculates the closest point on the circle and sets its heading to a custom-provided angle.
trajectoryToClosestPoint(PointXYZ robotPosition, PointXY center, double radius, double speed, double tolerance, Angle angleTolerance)
Creates a LinearTrajectory from the robot's current position to the closest point on the circle. The robot will be oriented to face the circle's center upon reaching the destination.
trajectoryToClosestPointWithoutHeading(PointXYZ robotPosition, PointXY center, double radius, double speed, double tolerance, Angle angleTolerance)
Generates a LinearTrajectory to the closest point on the circle, but the robot maintains its original heading throughout the movement.
trajectoryToClosestPointWithHeading(PointXYZ robotPosition, PointXY center, double radius, double speed, double tolerance, Angle angleTolerance, Angle customHeading)
Creates a LinearTrajectory to the closest point on the circle, with the robot turning to a specified custom heading.
fastTrajectoryToClosestPoint(PointXYZ robotPosition, PointXY center, double radius, double speed)
Generates a FastTrajectory to the closest point on the circle, with the robot's heading directed towards the circle's center.
fastTrajectoryToClosestPointWithoutHeading(PointXYZ robotPosition, PointXY center, double radius, double speed)
Creates a FastTrajectory to the closest point on the circle while maintaining the robot's initial heading.
fastTrajectoryToClosestPointWithHeading(PointXYZ robotPosition, PointXY center, double radius, double speed, Angle customHeading)
Generates a FastTrajectory to the closest point on the circle, with the robot turning to a specified custom heading.