#define _XOPEN_SOURCE 500 /* For PTHREAD_PRIO_INHERIT etc. */
#include <map.h>
-#include <movehelper.h>
#include <pthread.h>
#include <robomath.h>
#include <robot.h>
#include "actuators.h"
#include <robodim.h>
#include <ul_log.h>
+#include "guard.hpp"
+#include "fsmmove.cc"
-UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */
+//UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */
#define MOTION_CONTROL_INIT_ONLY
#include "motion-control.h"
/* Global definition of robot structure */
-struct robot robot;
-
-#ifdef CONFIG_LOCK_CHECKING
-struct lock_log robot_lock_log;
-#endif
+Robot robot;
static void block_signals()
{
static void int_handler(int sig)
{
- robot_exit();
+ robot.robot_exit();
}
void fill_in_known_areas_in_map()
static void trans_callback(struct robo_fsm *fsm)
{
- if (fsm == &robot.fsm.main) {
+/* if (fsm == &robot.fsm.main) {
strncpy(robot.orte.fsm_main.state_name, fsm->state_name, sizeof(robot.orte.fsm_main.state_name));
ORTEPublicationSend(robot.orte.publication_fsm_main);
} else if (fsm == &robot.fsm.motion) {
strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name));
ORTEPublicationSend(robot.orte.publication_fsm_act);
}
-
+*/
}
/**
*
* @return 0
*/
-int robot_init()
+int Robot::init()
{
int rv;
pthread_mutexattr_t mattr;
pthread_mutex_init(&robot.lock_joy_data, &mattr);
pthread_mutex_init(&robot.lock_disp, &mattr);
- fsm_main_loop_init(&robot.main_loop);
-
- /* FSM initialization */
- fsm_init(&robot.fsm.main, "MAIN", &robot.main_loop);
- fsm_init(&robot.fsm.motion, "\tmot", &robot.main_loop);
- fsm_init(&robot.fsm.act, "\tACT", &robot.main_loop);
- robot.fsm.main.transition_callback = trans_callback;
- robot.fsm.act.transition_callback = trans_callback;
- robot.fsm.motion.transition_callback = trans_callback;
+ MOTION = sched.create_processor<FSMMotion>();
robot.team_color = BLUE;
- robot_set_est_pos_trans(1, 1, DEG2RAD(0));
+ set_est_pos_trans(1, 1, DEG2RAD(0));
robot.ignore_hokuyo = false;
robot.map = ShmapInit(1);
robot.orte.camera_control.on = false;
- robot.fsm.motion.state = &fsm_state_motion_init;
/* Only activate display if it is configured */
/* FIXME: display
*
* @return 0
*/
-int robot_start()
+int Robot::start()
{
int rv = 0;
goto err;
}
- sem_init(&robot.start, 0, 0);
-
- fsm_main_loop(&robot.main_loop);
err:
return rv;
void robot_exit()
{
/* stop FSMs */
- fsm_exit(&robot.fsm.main);
- fsm_exit(&robot.fsm.motion);
- fsm_exit(&robot.fsm.act);
}
/**
* Stops the robot. All resources alocated by robot_init() or
* robot_start() are dealocated here.
*/
-void robot_destroy()
+void Robot::destroy()
{
motion_control_done();
robottype_roboorte_destroy(&robot.orte);
- fsm_destroy(&robot.fsm.main);
- fsm_destroy(&robot.fsm.motion);
- fsm_destroy(&robot.fsm.act);
ShmapFree();
ul_logdeb("robofsm: stop.\n");
}
-void robot_get_est_pos_trans(double *x, double *y, double *phi)
+void Robot::get_est_pos_trans(double &x, double &y, double &phi)
{
- robot_get_est_pos(x, y, phi);
- *x = __trans_x(*x);
- *y = __trans_y(*y);
- *phi = __trans_ang(*phi);
+ get_est_pos(x, y, phi);
+ x = trans_x(x);
+ y = trans_y(y);
+ phi = trans_angle(phi);
}
-void robot_get_est_pos(double *x, double *y, double *phi)
+void Robot::get_est_pos(double &x, double &y, double &phi)
{
if (robot.indep_odometry_works) {
- ROBOT_LOCK(est_pos_indep_odo);
- *x = robot.est_pos_indep_odo.x;
- *y = robot.est_pos_indep_odo.y;
- *phi = robot.est_pos_indep_odo.phi;
- ROBOT_UNLOCK(est_pos_indep_odo);
+ Guard g(robot.lock_est_pos_indep_odo);
+ x = robot.est_pos_indep_odo.x;
+ y = robot.est_pos_indep_odo.y;
+ phi = robot.est_pos_indep_odo.phi;
} else if (robot.odometry_works) {
- ROBOT_LOCK(est_pos_odo);
- *x = robot.est_pos_odo.x;
- *y = robot.est_pos_odo.y;
- *phi = robot.est_pos_odo.phi;
- ROBOT_UNLOCK(est_pos_odo);
+ Guard g(robot.lock_est_pos_odo);
+ x = robot.est_pos_odo.x;
+ y = robot.est_pos_odo.y;
+ phi = robot.est_pos_odo.phi;
} else {
- ROBOT_LOCK(ref_pos);
- *x = robot.ref_pos.x;
- *y = robot.ref_pos.y;
- *phi = robot.ref_pos.phi;
- ROBOT_UNLOCK(ref_pos);
+ Guard g(robot.lock_ref_pos);
+ x = robot.ref_pos.x;
+ y = robot.ref_pos.y;
+ phi = robot.ref_pos.phi;
}
}
+
+/** Sets actual position of the robot and with respoect to color of
+ * the team. Should be used for setting initial position of the
+ * robot. */
+void Robot::set_est_pos(double x, double y, double phi) {
+ if (x<0) x=0;
+ if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
+ if (y<0) y=0;
+ if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
+
+ {
+ Guard g(lock_est_pos_indep_odo);
+ est_pos_indep_odo.x = x;
+ est_pos_indep_odo.y = y;
+ est_pos_indep_odo.phi = phi;
+ }
+
+ {
+ Guard g(lock_est_pos_odo);
+ est_pos_odo.x = x;
+ est_pos_odo.y = y;
+ est_pos_odo.phi = phi;
+ }
+
+ {
+ Guard g(lock_ref_pos);
+ ref_pos.x = x;
+ ref_pos.y = y;
+ ref_pos.phi = phi;
+ }
+}
\ No newline at end of file
#include <stdio.h>
#include <trgenconstr.h>
#include <robottype.h>
-#include <robottype.h>
+//#include <robottype.h>
#include <roboorte_robottype.h>
#include <robodim.h>
#include <roboevent.h>
-#include <fsm.h>
+//#include <fsm.h>
#include <robot_config.h>
#include <can_msg_def.h>
-//#include <ul_log.h>
+#include <scheduler.hpp>
+#include <boost/statechart/asynchronous_state_machine.hpp>
+#include <movehelper.h>
//UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */
/**
* Competition parameters
*/
-enum team_color {
+enum team_colour {
BLUE = 0,
YELLOW
};
#define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id)
-/**
- * LOCKING MANIPULATION
- */
-
-#ifdef CONFIG_LOCK_CHECKING
-#define LOCK_CHECK_COUNT 10
-struct lock_log {
- pthread_mutex_t *locked[LOCK_CHECK_COUNT];
- int idx;
-};
-
-extern struct lock_log robot_lock_log;
-
-#define __LOCK_CHECK(mutex) robot_lock_log.locked[robot_lock_log.idx++] = mutex;
-#define __UNLOCK_CHECK(mutex) \
- if (robot_lock_log.locked[--robot_lock_log.idx] != mutex || \
- robot_lock_log.idx < 0) \
- ul_logerr("!!! Locking error %s:%d\n", __FILE__, __LINE__);
-#else
-#define __LOCK_CHECK(mutex)
-#define __UNLOCK_CHECK(mutex)
-#endif
-/**
- * Locks the robot structure.
- * @param var Field in the structure you are going to work with.
- */
-#define ROBOT_LOCK(var) \
- do { \
- pthread_mutex_lock(&robot.__robot_lock_##var); \
- __LOCK_CHECK(&robot.__robot_lock_##var); \
- } while(0)
-
-/**
- * Unlocks the robot structure.
- * @param var Field in the structure, which was locked by ROBOT_LOCK.
- */
-#define ROBOT_UNLOCK(var) \
- do { \
- __UNLOCK_CHECK(&robot.__robot_lock_##var); \
- pthread_mutex_unlock(&robot.__robot_lock_##var); \
- } while(0)
-
-/* Mapping of robot structure fields to locks */
-//#define __robot_lock_ lock /* ROBOT_LOCK() */
-#define __robot_lock_ref_pos lock_ref_pos
-#define __robot_lock_est_pos_odo lock_est_pos_odo
-#define __robot_lock_est_pos_indep_odo lock_est_pos_indep_odo
-#define __robot_lock_joy_data lock_joy_data
-#define __robot_lock_meas_angles lock_meas_angles
-#define __robot_lock_drives lock
-#define __robot_lock_sharps lock
-#define __robot_lock_hokuyo lock
-#define __robot_lock_cmu lock
-#define __robot_lock_bumper lock
-#define __robot_lock_disp lock_disp
-#define __robot_lock_motion_irc lock
-#define __robot_lock_odo_data lock
-#define __robot_lock_corr_distances lock
-#define __robot_lock_camera_result lock_camera
-
enum robot_status {
STATUS_OK,
STATUS_WARNING,
};
/* robot's main data structure */
-struct robot {
+class Robot {
+
+ public:
pthread_mutex_t lock;
pthread_mutex_t lock_ref_pos;
pthread_mutex_t lock_est_pos_odo;
pthread_mutex_t lock_camera;
/* competition parameters */
- enum team_color team_color;
+ team_colour team_color;
/** State variable used for controlling the robot by pluggin
* in and out start connector. */
- enum robot_start_state start_state;
+ robot_start_state start_state;
/** Temporary storage for new trajectory. After the trajectory creation is
* finished, this trajectory is submitted to fsmmove. */
- void *new_trajectory;
-
- unsigned char isTrajectory;
- sem_t start;
-
- struct fsm_main_loop main_loop;
-
- /* partial FSMs */
- struct {
- struct robo_fsm main;
- struct robo_fsm motion;
- struct robo_fsm act;
- } fsm;
/* actual position */
- struct robot_pos_type ref_pos;
+ robot_pos_type ref_pos;
/* estimated position */
- struct robot_pos_type est_pos_odo;
- struct robot_pos_type est_pos_indep_odo;
+ robot_pos_type est_pos_odo;
+ robot_pos_type est_pos_indep_odo;
/** True if est_pos_odo is updated according to reception of motion_irc */
bool odometry_works;
struct odo_data_type odo_data; /* independent odometry */
struct corr_distances_type corr_distances; /* ultrasound */
- struct hokuyo_scan_type hokuyo;
+ hokuyo_scan_type hokuyo;
bool ignore_hokuyo;
/* variables for target detection */
/** is set to true in separate thread when there is short time left */
bool short_time_to_end;
bool check_turn_safety;
+ /** time when competition is started*/
+ timespec startTime;
+ MoveHelper move_helper;
+ Scheduler sched;
+ /* State machines */
+ Scheduler::processor_handle MAIN;
+ Scheduler::processor_handle MOTION;
+
+ int init() __attribute__ ((warn_unused_result));
+ int start() __attribute__ ((warn_unused_result));
+ void robot_exit();
+ void destroy();
+
+ void set_est_pos_trans(double x, double y, double phi){set_est_pos(trans_x(x), trans_y(y), trans_angle(phi));}
+ void set_est_pos(double x, double y, double phi);
+ void get_est_pos_trans(double &x, double &y, double &phi);
+ void get_est_pos(double &x, double &y, double &phi);
}; /* robot */
-extern struct robot robot;
+extern Robot robot;
#ifdef __cplusplus
extern "C" {
#endif
-int robot_init() __attribute__ ((warn_unused_result));
-int robot_start() __attribute__ ((warn_unused_result));
-void robot_exit();
-void robot_destroy();
-
-void robot_get_est_pos_trans(double *x, double *y, double *phi);
-void robot_get_est_pos(double *x, double *y, double *phi);
/* Hack to easily disable display if it is not configured */
void serial_comm(int status);
-
-FSM_STATE_FULL_DECL(main, init);
-FSM_STATE_FULL_DECL(motion, init);
-FSM_STATE_FULL_DECL(disp, init);
-FSM_STATE_FULL_DECL(act, wait_for_command);
-
#ifdef __cplusplus
}
#endif
projects / eurobot / public.git / commitdiff