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; } } } }
static void InitializeTorsoPosition(void) { BASE_TranslateHalt(); WriteCommand("IX", 0, 0); }