Odometry autocalibration

[eurobot/public.git] / src / robofsm / robot_orte.c

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 6ccf605ba5e6329ea3d8aaf087e3503be127dfcb..02475186dca0734520fedf7d865dd0c8d9ccbdc3 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -23,6 +23,7 @@
 #include <math.h>
 #include <robomath.h>
 #include "map_handling.h"
+#include "match-timing.h"
 #include <string.h>
 #include <can_msg_def.h>
 #include <actuators.h>
@@ -86,6 +87,17 @@ void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
 {
 }
 
+void send_match_time_cb(const ORTESendInfo *info, void *vinstance,
+                       void *sendCallBackParam)
+{
+       struct match_time_type *instance = (struct match_time_type *)vinstance;
+
+        if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) {
+                instance->time = 90;
+        } else {
+                instance->time = 90 - robot_current_time();
+        }
+}
 /* ---------------------------------------------------------------------- 
  * SUBSCRIBER CALLBACKS - GENERIC
  * ---------------------------------------------------------------------- */
@@ -111,13 +123,15 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
                                break;
                        }
                        
-                       dleft = ((robot.odo_data.left - instance->left) >> 8) * c;      // TODO >> 8 ?
-                       dright = ((instance->right - robot.odo_data.right) >> 8) * c;
+                       dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a;    // TODO >> 8 ?
+                       dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b;
 
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 
                        ROBOT_LOCK(est_pos_indep_odo);
+                       robot.odo_distance_a +=dleft;
+                       robot.odo_distance_b +=dright;
                        double a = robot.est_pos_indep_odo.phi;
                        robot.est_pos_indep_odo.x += dtang*cos(a);
                        robot.est_pos_indep_odo.y += dtang*sin(a);
@@ -150,11 +164,10 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
        double dleft, dright, dtang, dphi;
        static bool first_run = true;
        /* spocitat prevodovy pomer */
-       const double n = (double)(28.0 / 1.0); 
+       const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
 
        /* vzdalenost na pulz IRC */
-       const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-       
+       const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
        switch (info->status) {
                case NEW_DATA:
                        if (first_run) {
@@ -165,9 +178,13 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                                break;
                        }
                        
-                       dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
-                       dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
 
+                       /* 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;
+                       
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 
@@ -409,22 +426,35 @@ void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
                           void *recvCallBackParam)
 {
        struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
-       static bool last_left, last_right;
+       static bool last_strategy;
        switch (info->status) {
                case NEW_DATA:
                        robot.team_color = instance->team_color;
-                       if (instance->bumper_pressed)
+
+                       if (!last_strategy && instance->strategy) {
+                                       /* strategy switching */
+                                       FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
+                       }
+                       last_strategy = instance->strategy;
+                       break;
+               case DEADLINE:
+                       break;
+       }
+}
+
+void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
+                          void *recvCallBackParam)
+{
+       struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
+       static bool last_left, last_right;
+       switch (info->status) {
+               case NEW_DATA:
+                       if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
                                FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
-                       if ((!last_left && instance->bumper_left) ||
-                           (!last_right && instance->bumper_right)) {
+
+                       if (instance->bumper_left || instance->bumper_right) {
                                FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
-                               if (robot.start_state == POWER_ON) {
-                                       /* Reuse VIDLE done for strategy switching */
-                                       FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
-                               }
                        }
-                       last_right = instance->bumper_right;
-                       last_left = instance->bumper_left;
                        break;
                case DEADLINE:
                        break;
@@ -461,6 +491,7 @@ int robot_init_orte()
        robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
        robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
        robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
+       robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte);
        //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 
        // I didn't know what was the difference between the callback function pointer
@@ -487,6 +518,7 @@ int robot_init_orte()
        robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
         robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
        robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
+       robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
        robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 
        return rv;