From bfbd61a139592e6564fc79035e7d72e5668a002b Mon Sep 17 00:00:00 2001 From: petr Date: Fri, 4 May 2012 17:16:29 +0200 Subject: [PATCH] Odometry calibration Correct measured distance. --- src/robofsm/common-states.cc | 9 +++++---- src/robofsm/robot.c | 3 +++ src/robofsm/robot_orte.c | 4 ++-- src/robofsm/strategy_odo_calibration.cc | 2 +- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index f0b910d8..d4a0c7b8 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -35,9 +35,9 @@ UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file void set_initial_position() { - robot_set_est_pos_trans(ROBOT_START_X_M, - ROBOT_START_Y_M, - DEG2RAD(ROBOT_START_ANGLE_DEG)); + robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M, + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0) - 0.05, + 0); } void actuators_home() @@ -435,7 +435,7 @@ FSM_STATE(go_back_for_cal) PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0), 0); robot_move_by(-1.1, NO_TURN(), &tcVerySlow); - FSM_TIMER(200); + FSM_TIMER(150); break; case EV_MOTION_DONE: ROBOT_LOCK(est_pos_odo); @@ -454,6 +454,7 @@ FSM_STATE(go_back_for_cal) fputs ("\n", file); sprintf(buffer, "%4.4f", -1/y1); fputs (buffer,file); + fputs ("\n", file); fclose(file); SUBFSM_RET(NULL); break; diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c index e49f2690..5aeb5dd4 100644 --- a/src/robofsm/robot.c +++ b/src/robofsm/robot.c @@ -31,6 +31,7 @@ UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */ #define MOTION_CONTROL_INIT_ONLY #include "motion-control.h" +#include "robot.h" /* Global definition of robot structure */ struct robot robot; @@ -317,4 +318,6 @@ void robot_calibrate_odometry() } robot.odo_cal_a = a / num; robot.odo_cal_b = b / num; + printf("calibrated value left: %f\n",robot.odo_cal_a); + printf("calibrated value right: %f\n",robot.odo_cal_b); } diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 02475186..9d5a2317 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -182,8 +182,8 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance, /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright), what is the right solution? This was neccessary to view motor odometry correctly in robomon. */ - dright = ((robot.motion_irc.left - instance->left) >> 8) * c * robot.odo_cal_b; - dleft = ((instance->right - robot.motion_irc.right) >> 8) * c * robot.odo_cal_a; + dright = ((robot.motion_irc.left - instance->left) >> 8) * c; + dleft = ((instance->right - robot.motion_irc.right) >> 8) * c; dtang = (dleft + dright) / 2.0; dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M); diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc index fb13894a..6c032722 100644 --- a/src/robofsm/strategy_odo_calibration.cc +++ b/src/robofsm/strategy_odo_calibration.cc @@ -19,7 +19,7 @@ FSM_STATE(calibrate) break; case EV_RETURN: robot_calibrate_odometry(); - //FSM_TRANSITION(get_central_buillon_first); + FSM_TRANSITION(get_central_buillon_first); break; case EV_SWITCH_STRATEGY: //FSM_TRANSITION(get_central_buillon_first); -- 2.39.2