#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>
{
}
+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
* ---------------------------------------------------------------------- */
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);
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) {
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);
case NEW_DATA:
robot.team_color = instance->team_color;
- if (!last_strategy && instance->strategy && (robot.start_state == POWER_ON)) {
+ if (!last_strategy && instance->strategy) {
/* strategy switching */
FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
}
static bool last_left, last_right;
switch (info->status) {
case NEW_DATA:
- if (instance->bumper_right_across || instance->bumper_left_across)
+ 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 (instance->bumper_left || instance->bumper_right) {
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