From 9de11695d9f12221e00398aa4e5481d2dcb80580 Mon Sep 17 00:00:00 2001 From: petr Date: Sat, 28 Apr 2012 20:51:45 +0200 Subject: [PATCH] Odometry autocalibration Remove calculation of distance using motor irc. --- src/robofsm/common-states.cc | 8 ++++---- src/robofsm/robot_orte.c | 2 -- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index 970e9eda..b7d27b2d 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -435,12 +435,12 @@ FSM_STATE(go_back_for_cal) 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); @@ -463,7 +463,7 @@ FSM_STATE(go_back_for_cal) 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; diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 10dad10a..02475186 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -189,8 +189,6 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance, 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); -- 2.39.2