-}
-
-/* Homologation states */
-FSM_STATE(homolog_approach_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.65,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
- robot_trajectory_add_point_trans(
- 0.65,
- 1.3);
- robot_trajectory_add_final_point_trans(
- 0.5,
- 1.1,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- case EV_TIMER:
- FSM_TRANSITION(homolog_push_bottle);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(homolog_push_bottle)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.64,
- 0.7);
- robot_trajectory_add_final_point_trans(
- 0.64 + 0.08,
- ROBOT_AXIS_TO_FRONT_M + 0.05,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- break;
- default:
- break;
- }
-}
-
-
-/* State for odometry calibration */
-
-FSM_STATE(go_back_for_cal)
-{
- double x1 = 0, y1 = 0;
- char buffer [20];
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
- 0);
- robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
- FSM_TIMER(200);
- break;
- case EV_MOTION_DONE:
- ROBOT_LOCK(est_pos_odo);
- robot.odo_cal_a = -1.0/robot.odo_distance_a;
- robot.odo_cal_b = -1.0/robot.odo_distance_b;
- x1 = robot.odo_distance_a;
- y1 = robot.odo_distance_b;
- ROBOT_UNLOCK(est_pos_odo);
- if(x1 != 0 || y1 != 0) {
- printf("Distance for calibration: \n");
- printf("Left%f\n", x1);
- printf("Right%f\n", y1);
- FILE * file;
- file = fopen ("odometry_cal_data","a+");
- sprintf(buffer, "%4.4f", -1/x1);
- fputs (buffer,file);
- fputs (" ", file);
- sprintf(buffer, "%4.4f", -1/y1);
- fputs (buffer,file);
- fputs ("\n", file);
- fclose(file);
- }
- SUBFSM_RET(NULL);
- break;
- case EV_TIMER:
- ROBOT_LOCK(est_pos_odo);
- if(x1 == robot.odo_distance_a){
- x1 = robot.odo_distance_a;
- y1 = robot.odo_distance_b;
- FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- } else {
- FSM_TIMER(200);
- }
- ROBOT_UNLOCK(est_pos_odo);
- break;
- default:
- break;
- }