void BASE_Halt(void)
{
BASE_TranslateAcceleration(transDeceleration);
BASE_TranslateHalt();
BASE_RotateAcceleration(rotDeceleration);
BASE_RotateHalt();
BASE_TranslateAcceleration(transDeceleration);
BASE_RotateAcceleration(rotDeceleration);
}
void BASE_reset(void)
{
struct timeval poll_interval;
JoystickDisable();
HalfDuplex();
setErrorDelay(128); /* number of 256Hz cycles between error and shutdown. */
TorqueLimit(100);
BASE_RotateAcceleration(rotAcceleration);
BASE_TranslateAcceleration(transAcceleration);
// BASE_RotateVelocity(init_rot_speed);
// BASE_TranslateVelocity(init_trans_speed);
BASE_StatusReportData(SR_POS_X |
SR_POS_Y |
SR_TOP_HEADING |
SR_BASE_HEADING |
SR_TRANS_VELOCITY |
SR_TRANS_STATUS |
SR_ROT_VELOCITY |
SR_ROT_STATUS |
SR_TRANS_MOTOR_PULSE |
SR_ROT_MOTOR_PULSE);
/* commands in the streaming data from base */
BASE_SetIntervalUpdates(STREAMING_MILLISECONDS);
poll_interval.tv_usec = 0;
poll_interval.tv_sec = CHECKING_SECONDS;
devStartPolling(base_device.dev, &poll_interval, BASE_report_check, NULL);
BASE_InstallHandler((Handler)BASE_SetRotationReference, STATUS_REPORT,
(Pointer) NULL, TRUE);
}
void COLLI_SetMode( int mode)
{
int intMode = (int) mode;
if (intMode >= NUMBER_OF_MODES || intMode < 0) {
fprintf(stderr, "Wrong mode number (%d). Use default mode.\n", intMode);
ACTUAL_MODE = mode_structure_array[DEFAULT_MODE];
mode_number = DEFAULT_MODE;
}
else {
ACTUAL_MODE = mode_structure_array[intMode];
mode_number = intMode;
if ( mode_number == ARM_OUT_MODE) {
armState = OUTSIDE;
/* COLLI_GoBackward(); */
}
}
fprintf(stderr, "\n\nmode: %d\n", intMode);
fprintf (stderr, "tv : %f ", (ACTUAL_MODE->target_max_trans_speed));
fprintf (stderr, "ta : %f\n", (ACTUAL_MODE->target_trans_acceleration));
fprintf (stderr, "rv : %f ", (RAD_TO_DEG(ACTUAL_MODE->target_max_rot_speed)));
fprintf (stderr, "ra : %f\n", (RAD_TO_DEG(ACTUAL_MODE->target_rot_acceleration)));
fprintf (stderr, "velocity: %f \n", (ACTUAL_MODE->velocity_factor));
fprintf (stderr, "angle : %f\n", (ACTUAL_MODE->angle_factor));
fprintf (stderr, "distance: %f\n", (ACTUAL_MODE->distance_factor));
/* Set the corresponding values. */
BASE_TranslateVelocity( ACTUAL_MODE->target_max_trans_speed);
BASE_RotateVelocity( RAD_TO_DEG( ACTUAL_MODE->target_max_rot_speed));
BASE_TranslateAcceleration( ACTUAL_MODE->target_trans_acceleration);
BASE_RotateAcceleration(RAD_TO_DEG( ACTUAL_MODE->target_rot_acceleration));
}
void BASE_TranslateForward(void)
{
if (rwi_base.emergency)
return;
if (rwi_base.trans_current_speed == 0 ||
(rwi_base.trans_direction != POSITIVE &&
rwi_base.trans_set_speed != 0.0))
{
BASE_TranslateAcceleration(transAcceleration);
SET_ROBOT_STATE(trans_set_direction, POSITIVE);
SET_ROBOT_STATE(trans_set_speed, FABS(rwi_base.trans_set_speed));
WriteCommand("T+", 0, 0);
}
}
