void BASE_Halt(void)
{
BASE_TranslateAcceleration(transDeceleration);
BASE_TranslateHalt();
BASE_RotateAcceleration(rotDeceleration);
BASE_RotateHalt();
BASE_TranslateAcceleration(transDeceleration);
BASE_RotateAcceleration(rotDeceleration);
}
/* This procedure is called when an exception-handling is necessary. *
* It sends a stop-command to the base and the haltingFlag is set to TRUE */
void
startHalting( Point rpos,
float rrot)
{
BASE_TranslateCollisionAcceleration(ACTUAL_MODE->exception_trans_acceleration);
BASE_RotateCollisionAcceleration( RAD_TO_DEG( ACTUAL_MODE->exception_rot_acceleration));
BASE_TranslateHalt();
BASE_RotateHalt();
target_flag = TRUE;
haltingFlag = TRUE;
return;
}
/* This function controls the rotate-away-operation. It stops the robot
* when the angle is not reachable. In the case that the angle is not
* reachable because of a misreading it restarts the rotation to the
* old angle from the new position. Otherwise it decides wether a new
* angle should be calculated or wether rollback should be executed
* (compare rotateAwayNumber). */
void
keepOnRotatingAway(Point rpos, float rrot)
{
static BOOLEAN stoppedRotatingAway = FALSE;
float leftAngle,rightAngle,delta1,delta2,resultAngle;
if (stoppedRotatingAway)
{
if (stillInRotation())
{
if (isAngleReachable(rpos,rrot,&leftAngle,&rightAngle))
{
/* here we know that we are able to turn to the chosen angle "angle" */
delta1 = rrot - absoluteAngle;
if (delta1<= 0.0)
delta2 = delta1 + DEG_360;
else
delta2 = delta1 - DEG_360;
if (fabs(delta1)<= fabs(delta2))
{ /* We prefer delta1 as rotation angle because it is the smaller one */
if (delta1 >= 0.0)
{ /* We prefer turning right */
if (delta1 <= rightAngle )
{ /* It is possible to turn right */
resultAngle = delta1;
}
else
{ /* It is not possible to turn right, therefore we turn left */
resultAngle = delta2;
}
}
else
{ /* We prefer turning left */
if (fabs(delta1) <= leftAngle)
{ /* It is possible to turn left */
resultAngle = delta1;
}
else
{ /* It is not possible to turn left, therefore we turn right */
resultAngle = delta2;
}
}
}
else
{ /* We prefer delta2 as rotation angle because it is the smaller one */
if (delta2 >= 0.0)
{ /* We prefer turning right */
if (delta2 <= rightAngle )
{ /* It is possible to turn right */
resultAngle = delta2;
}
else
{ /* It is not possible to turn right, therefore we turn left */
resultAngle = delta1;
}
}
else
{ /* We prefer turning left */
if (fabs(delta2) <= leftAngle)
{ /* It is possible to turn left */
resultAngle = delta2;
}
else
{ /* It is not possible to turn left, therefore we turn right */
resultAngle = delta1;
}
}
}
startRotatingAway(rpos,rrot,resultAngle);
stoppedRotatingAway = FALSE;
return;
}
else
{
return;
}
}
else
{
stoppedRotatingAway = FALSE;
haltingFlag = TRUE;
rotatingAwayFlag = FALSE;
return;
}
}
else
{
if (!isAngleReachable(rpos,rrot,&leftAngle,&rightAngle))
{
BASE_TranslateCollisionAcceleration(ACTUAL_MODE->exception_trans_acceleration);
BASE_RotateCollisionAcceleration( RAD_TO_DEG( ACTUAL_MODE->exception_rot_acceleration));
BASE_TranslateHalt();
BASE_RotateHalt();
/* In the stop commands the target flag is set to FALSE.
* We still want to be in the target mode.
*/
target_flag = TRUE;
stoppedRotatingAway = TRUE;
return;
}
else
{
if (stillInRotation())
return;
else
{
rotatingAwayFlag = FALSE;
rotateAwayCounter = 0;
return;
}
}
}
}
