/**
* Rotate the robot on spot (differential heading) with a desired heading.
*
* @param angle angle for rotation
* @return false in case the rotation was interrupted, true otherwise
*/
public boolean setDiffHeading(double angle) {
if (angle == 0) return true;
stop = false;
boolean ret = true;
/* get the current heading */
double currentHead = transformAngle(device.getYaw());
/* calculate the goal heading */
double newGoal = transformAngle(currentHead + angle);
setGoal(newGoal);
double now = transformAngle(device.getYaw());
/* keep rotating while the goal was not reached */
while (now != newGoal) {
if (stop == true) {
ret = false;
break;
}
/* no point in rotating at all if we're at +/-180 */
if (Math.abs(now - newGoal) <= 1 && newGoal == 180)
break; /* exit if we reached our destination */
/* in case a diff. of maxError (default 0) between angles is acceptable */
if (Math.abs(now - newGoal) <= maxError) break; /* exit if we reached our destination */
/* get the current heading */
now = transformAngle(device.getYaw());
/* get the motor command and check if within the desired limits */
double command = getCommand(now);
command = boundCommand(command);
device.setSpeed(0, command);
try {
Thread.sleep(100);
} catch (Exception e) {
}
if (isDebugging)
System.err.println("[HeadingControl][Debug] Angle left : " + Math.abs(now - newGoal));
}
device.setSpeed(0, 0); /* stop the robot from rotating */
return ret;
}
/**
* Rotate the robot on spot (absolute heading) to the desired heading.
*
* @param angle goal angle
* @return false in case the rotation was interrupted, true otherwise
*/
public boolean setHeading(double angle) {
/* get the current heading */
double currentAngle = transformAngle(device.getYaw());
/* difference between the current heading and the goal heading */
double deltaAngle = (angle - currentAngle);
if (deltaAngle != 0) {
if (deltaAngle <= 180 && deltaAngle > 0) return setDiffHeading(deltaAngle);
else if (deltaAngle > -180) return setDiffHeading(-360 + deltaAngle);
else return setDiffHeading(360 + deltaAngle);
}
return true;
}
