PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
0);
robot_move_by(-1.1, NO_TURN(), &tcSlow);
- FSM_TIMER(500);
+ FSM_TIMER(200);
break;
case EV_MOTION_DONE:
ROBOT_LOCK(est_pos_odo);
- robot.odo_cal_a = 1/robot.odo_distance_a;
- robot.odo_cal_b = 1/robot.odo_distance_b;
+ 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);
FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
} else {
- FSM_TIMER(500);
+ FSM_TIMER(200);
}
ROBOT_UNLOCK(est_pos_odo);
break;
dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
ROBOT_LOCK(est_pos_odo);
- robot.odo_distance_a +=dleft;
- robot.odo_distance_b +=dright;
double a = robot.est_pos_odo.phi;
robot.est_pos_odo.x += dtang*cos(a);
robot.est_pos_odo.y += dtang*sin(a);