* License: GNU GPL v.2
*/
+#define _XOPEN_SOURCE 500 /* For PTHREAD_PRIO_INHERIT etc. */
+
#include <map.h>
#include <movehelper.h>
#include <pthread.h>
#include "map_handling.h"
#include <uoled.h>
#include <string.h>
+#include "actuators.h"
+
+/* Timing analysis */
+#ifdef CONFIG_TIMING_ANALYSIS
+#include <timing.h>
+#endif
+
+#ifdef ROBOT_DEBUG
+ #define DBG(format, ...) printf(format, ##__VA_ARGS__)
+#else
+ #define DBG(format, ...)
+#endif
#define MOTION_CONTROL_INIT_ONLY
#include "motion-control.h"
/* ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0); */
/* ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); */
-
- ShmapSetRectangleFlag(0, 1.15, 0.4, 1.55, MAP_FLAG_IGNORE_OBST, 0); /* Left white vert. dispenser */
- ShmapSetRectangleFlag(2.7, 1.25, 3, 1.45, MAP_FLAG_IGNORE_OBST, 0); /* Right white vert. dispenser */
+ //ShmapSetRectangleFlag(0, 1.15, 0.4, 1.55, MAP_FLAG_IGNORE_OBST, 0); /* Left white vert. dispenser */
+ //ShmapSetRectangleFlag(2.7, 1.25, 3, 1.45, MAP_FLAG_IGNORE_OBST, 0); /* Right white vert. dispenser */
- ShmapSetRectangleFlag(0.50, 1.8, 0.95, 2.2, MAP_FLAG_IGNORE_OBST, 0); /* Blue vert. dispenser */
- ShmapSetRectangleFlag(2.30, 1.8, 2.55, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* Red vert. dispenser */
+ //ShmapSetRectangleFlag(0.50, 1.8, 0.95, 2.2, MAP_FLAG_IGNORE_OBST, 0); /* Blue vert. dispenser */
+ //ShmapSetRectangleFlag(2.30, 1.8, 2.55, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* Red vert. dispenser */
/* Ignore other obstacles at edges */
ShmapSetRectangleFlag(0.0, 0.0, 0.09, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* left */
ShmapSetRectangleFlag(0.0, 2.01, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* top */
ShmapSetRectangleFlag(2.91, 0.0, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* right */
- ShmapSetRectangleFlag(0.0, 0.0, 0.2, 0.6, MAP_FLAG_IGNORE_OBST, 0); /* left basket */
- ShmapSetRectangleFlag(2.8, 0.0, 3.0, 0.6, MAP_FLAG_IGNORE_OBST, 0); /* right basket */
+ //ShmapSetRectangleFlag(0.0, 0.0, 0.2, 0.6, MAP_FLAG_IGNORE_OBST, 0); /* left basket */
+ //ShmapSetRectangleFlag(2.8, 0.0, 3.0, 0.6, MAP_FLAG_IGNORE_OBST, 0); /* right basket */
+
+ /* Construction areas (Building zones) */
+ //ShmapSetRectangleFlag(ACROPOLIS_CENTER_X - ACROPOLIS_RADIUS, ACROPOLIS_CENTER_Y - ACROPOLIS_RADIUS, ACROPOLIS_CENTER_X + ACROPOLIS_RADIUS, ACROPOLIS_CENTER_Y + ACROPOLIS_RADIUS, MAP_FLAG_WALL, 0); /* Central construction area */
+ //ShmapSetRectangleFlag(1.35, 0.9, 1.65, 1.2, MAP_FLAG_WALL, 0); /* Central construction area */
+
+ // Enclosure wall
+ ShmapSetRectangleFlag(0.0, 0.0, 0.199, 2.1, MAP_FLAG_WALL, 0); /* left */
+ ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0); /* bottom */
+ ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0); /* top */
+ ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); /* right */
+ // Bottom-middle unloading area
+ ShmapSetRectangleFlag(1.0, 0.0, 2.0, 0.3, MAP_FLAG_WALL, 0); /* bottom */
+ // Unloading area - dwarf wall
+ ShmapSetRectangleFlag(0.4, 0.0, 0.8, 0.3, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(2.2, 0.0, 2.6, 0.3, MAP_FLAG_WALL, 0);
+ // Acropolis wall
+ ShmapSetCircleFlag(ACROPOLIS_CENTER_X, ACROPOLIS_CENTER_Y, ACROPOLIS_DIAMETER + 0.09, MAP_FLAG_WALL, 0);
+
+}
+
+static void trans_callback(struct robo_fsm *fsm)
+{
+ 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_motion.state_name, fsm->state_name, sizeof(robot.orte.fsm_motion.state_name));
+ ORTEPublicationSend(robot.orte.publication_fsm_motion);
+ } else if (fsm == &robot.fsm.act) {
+ strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name));
+ ORTEPublicationSend(robot.orte.publication_fsm_act);
+ }
+
}
/**
int robot_init()
{
int rv;
+ pthread_mutexattr_t mattr;
- pthread_mutex_init(&robot.lock, NULL);
- pthread_mutex_init(&robot.lock_ref_pos, NULL);
- pthread_mutex_init(&robot.lock_est_pos, NULL);
- pthread_mutex_init(&robot.lock_meas_angles, NULL);
- pthread_mutex_init(&robot.lock_joy_data, NULL);
- pthread_mutex_init(&robot.lock_disp, NULL);
+#ifdef CONFIG_TIMING_ANALYSIS
+ timing_init(25000000);
+#endif
+
+ rv = pthread_mutexattr_init(&mattr);
+#ifdef HAVE_PRIO_INHERIT
+ rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
+#endif
+ pthread_mutex_init(&robot.lock, &mattr);
+ pthread_mutex_init(&robot.lock_ref_pos, &mattr);
+ pthread_mutex_init(&robot.lock_est_pos_uzv, &mattr);
+ pthread_mutex_init(&robot.lock_est_pos_odo, &mattr);
+ pthread_mutex_init(&robot.lock_meas_angles, &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.main, "main", &robot.main_loop);
fsm_init(&robot.fsm.motion, "motion", &robot.main_loop);
fsm_init(&robot.fsm.display, "display", &robot.main_loop);
- fsm_init(&robot.fsm.act, "actuators", &robot.main_loop);
+ fsm_init(&robot.fsm.act, "actuators", &robot.main_loop); */
+ fsm_init(&robot.fsm.main, "MAIN", &robot.main_loop);
+ fsm_init(&robot.fsm.motion, "\tmot", &robot.main_loop);
+ fsm_init(&robot.fsm.display, "\tdisp", &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;
- /* FIXME: there should be team color initilization */
-#ifdef WE_ARE_RED
- robot.team_color = RED;
-#endif
-#ifdef WE_ARE_GREEN
robot.team_color = GREEN;
-#endif
if (robot.team_color == RED) {
printf("We are RED!\n");
printf("We are GREEN!\n");
}
-#ifdef CONFIG_OPEN_LOOP
- printf("OPEN_LOOP enabled\n");
-#endif
-
+ robot_set_est_pos_trans(0.16, PLAYGROUND_HEIGHT_M - 0.16, DEG2RAD(-45));
+
robot.map = ShmapInit(1);
fill_in_known_areas_in_map();
robot.orte.pwr_ctrl.voltage50 = 1;
robot.orte.pwr_ctrl.voltage80 = 1;
+ robot.orte.camera_control.on = true;
+
robot.fsm.motion.state = &fsm_state_motion_init;
robot.fsm.act.state = &fsm_state_act_wait_for_command; // puck handling actuators FSM's initial state
robot.fsm.display.state = &fsm_state_disp_init;
robot.obstacle_avoidance_enabled = true;
- robot.state = JUST_STARTED;
+ robot.use_back_switch = false; /* Switched on sime time after start */
+ robot.state = POWER_ON;
/* init ORTE domain, create publishers, subscribers, .. */
rv = robot_init_orte();
*/
void robot_destroy()
{
+ printf("Robot destroy\n");
motion_control_done();
+
+ act_chelae(CHELA_OPEN, CHELA_OPEN);
+ act_holder(HOLDER_OPENED);
robottype_roboorte_destroy(&robot.orte);
fsm_destroy(&robot.fsm.main);
fsm_destroy(&robot.fsm.motion);
fsm_destroy(&robot.fsm.display);
+ fsm_destroy(&robot.fsm.act);
+#ifdef CONFIG_TIMING_ANALYSIS
+ timing_finish();
+ timing_output_trace();
+#endif
+
+ ShmapFree();
DBG("robofsm: stop.\n");
}
projects / eurobot / public.git / blobdiff