config_include_HEADERS = robot_config.h
robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
-bin_PROGRAMS += competition
-competition_SOURCES = competition.cc common-states.cc \
- strategy_opp_corn.cc strategy_opp_oranges.cc \
- strategy_12_oranges.cc strategy_six_oranges.cc
-
-bin_PROGRAMS += homologation
-homologation_SOURCES = homologation.cc
+#bin_PROGRAMS += demo
+#demo_SOURCES = demo.cc
# Library with general support functions for the robot
lib_LIBRARIES += robot
robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc \
motion-control.cc map_handling.c \
- match-timing.c eb2010misc.cc
+ match-timing.c
robot_GEN_SOURCES = roboevent.c
include_GEN_HEADERS += roboevent.h
lib_LIBRARIES += actlib
actlib_SOURCES = actuators.c
-lib_LIBRARIES += cornslib
-cornslib_SOURCES = corns_configs.c
-
# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robomath roboorte robottype pthread rt m \
orte pathplan sharp map fsm rbtree motion robodim \
- actlib cornslib ulut
+ actlib ulut
# Automatic generation of event definition files
include-pass_HOOKS = roboevent.c roboevent.h
{
switch (FSM_EVENT) {
case EV_ENTRY:
- if (robot.localization_works) {
- robot_pos_type p;
- ROBOT_LOCK(est_pos_uzv);
- p = robot.est_pos_uzv;
- ROBOT_UNLOCK(est_pos_uzv);
-
- robot_set_est_pos_notrans(p.x, p.y, p.phi);
- }
break;
case EV_TRAJECTORY_DONE:
// Skip close to target because sometimes it turn the robot to much
/*
- * robot_eb2008.c 08/04/20
+ * robot_demo2011.c
*
* Robot's generic initialization and clean up functions.
*
#include "map_handling.h"
#include <string.h>
#include "actuators.h"
-#include "corns_configs.h"
#include <robodim.h>
#include <ul_log.h>
ShmapSetRectangleFlag(0.6, 1.45, 2.45, 1.55, MAP_FLAG_WALL, 0); /* plateau, slopes */
ShmapSetRectangleFlag(1.46, PLAYGROUND_HEIGHT_M - 0.5, 1.57, PLAYGROUND_HEIGHT_M, MAP_FLAG_WALL, 0); /* plateau, slopes */
- /* corns */
- struct corns_group *corns = get_all_corns(0, 0);
- struct corn * corn;
- for(corn = corns->corns; corn < &corns->corns[corns->corns_count]; corn++) {
- ShmapSetCircleFlag(corn->position.x, corn->position.y, CORN_NEIGHBOURHOOD_RADIUS_M, MAP_FLAG_WALL, 0);
- }
- dispose_corns_group(corns); // robot.corns will be set later, when the corns' configuration is known
-
ShmapSetRectangleFlag(0.32, 0.25, 0.38, 0.55, 0, MAP_FLAG_WALL); /* destination position near blue container */
ShmapSetRectangleFlag(2.62, 0.25, 2.68, 0.55, 0, MAP_FLAG_WALL); /* destination position near yellow container */
}
strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name));
ORTEPublicationSend(robot.orte.publication_fsm_act);
}
-
+
}
-/**
+/**
* Initializes the robot.
* Setup fields in robot structure, initializes FSMs and ORTE.
- *
+ *
* @return 0
*/
int robot_init()
#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_est_pos_indep_odo, &mattr);
pthread_mutex_init(&robot.lock_meas_angles, &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, "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.main, "MAIN", &robot.main_loop);
fsm_init(&robot.fsm.motion, "\tmot", &robot.main_loop);
fsm_init(&robot.fsm.act, "\tACT", &robot.main_loop);
ul_loginf("We are BLUE!\n");
}
- robot_set_est_pos_trans(0.16, PLAYGROUND_HEIGHT_M - 0.16, DEG2RAD(-45));
-
+ robot_set_est_pos_trans(1, 1, DEG2RAD(0));
+
robot.ignore_hokuyo = false;
robot.map = ShmapInit(1);
fill_in_known_areas_in_map();
robot.orte.pwr_ctrl.voltage80 = 1;
robot.orte.camera_control.on = true;
-
+
robot.fsm.motion.state = &fsm_state_motion_init;
/* Only activate display if it is configured */
return rv;
}
-/**
+/**
* Starts the robot FSMs and threads.
*
* @return 0
perror("robot_start: pthread_attr_setschedparam()");
goto err;
}
- rv = pthread_create(&thr_obstacle_forgeting,
+ rv = pthread_create(&thr_obstacle_forgeting,
&tattr, thread_obstacle_forgeting, NULL);
if (rv) {
perror("robot_start: pthread_create");
return rv;
}
-/**
+/**
* Signals all the robot threads to finish.
*/
void robot_exit()
fsm_exit(&robot.fsm.act);
}
-/**
+/**
* Stops the robot. All resources alocated by robot_init() or
* robot_start() are dealocated here.
*/
motion_control_done();
// FIXME: set actuators to well defined position (FJ)
-
+
robottype_roboorte_destroy(&robot.orte);
fsm_destroy(&robot.fsm.main);
#define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id)
/**
- * LOCKING MANIPULATION
+ * LOCKING MANIPULATION
*/
#ifdef CONFIG_LOCK_CHECKING
#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.
*/
__LOCK_CHECK(&robot.__robot_lock_##var); \
} while(0)
-/**
+/**
* Unlocks the robot structure.
* @param var Field in the structure, which was locked by ROBOT_LOCK.
*/
/* 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_uzv lock_est_pos_uzv
#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_hokuyo lock
#define __robot_lock_cmu lock
#define __robot_lock_bumper lock
-#define __robot_lock_drives lock
#define __robot_lock_disp lock_disp
#define __robot_lock_motion_irc lock
#define __robot_lock_odo_data lock
COMPONENT_HOKUYO,
COMPONENT_START,
COMPONENT_VIDLE,
-
+
ROBOT_COMPONENT_NUMBER
};
struct robot {
pthread_mutex_t lock;
pthread_mutex_t lock_ref_pos;
- pthread_mutex_t lock_est_pos_uzv;
pthread_mutex_t lock_est_pos_odo;
pthread_mutex_t lock_est_pos_indep_odo;
pthread_mutex_t lock_meas_angles;
/** 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;
/* actual position */
struct robot_pos_type ref_pos;
/* estimated position */
- struct robot_pos_type est_pos_uzv;
struct robot_pos_type est_pos_odo;
struct robot_pos_type est_pos_indep_odo;
- /** True if localization data arrives correctly and therfore
- * localization runs */
- bool localization_works;
/** True if est_pos_odo is updated according to reception of motion_irc */
bool odometry_works;
/** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */
bool indep_odometry_works;
-
+
bool use_back_switch;
bool use_left_switch;
bool use_right_switch;
* (taken as sudden zero velocities)
*/
bool motion_irc_received;
-
+
/* orte */
struct robottype_orte_data orte;
#ifdef __cplusplus
extern "C" {
-#endif
+#endif
int robot_init() __attribute__ ((warn_unused_result));
int robot_start() __attribute__ ((warn_unused_result));
void serial_comm(int status);
-
+
FSM_STATE_FULL_DECL(main, init);
FSM_STATE_FULL_DECL(motion, init);
FSM_STATE_FULL_DECL(disp, init);
#ifdef __cplusplus
}
-#endif
+#endif
/*Thread priorities*/
#define THREAD_PRIO_TRAJ_FOLLOWER 90 /* As high as possible */
break;
case EV_TIMER:
FSM_TRANSITION(robot_goto_test);
- if (!robot.localization_works) {
- robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2,
- PLAYGROUND_HEIGHT_M/2,
- DEG2RAD(0));
- }
+ robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2,
+ PLAYGROUND_HEIGHT_M/2,
+ DEG2RAD(0));
break;
default:;
}
break;
case EV_TIMER:
FSM_TRANSITION(rectangle);
- if (!robot.localization_works) {
- robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
- } else {
- // Set odo position
- robot_set_est_pos_trans(robot.est_pos_uzv.x, robot.est_pos_uzv.y, DEG2RAD(0));
- }
+ robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0));
break;
default:;
}
switch (FSM_EVENT) {
case EV_ENTRY:
robot_trajectory_new(NULL);
- robot_trajectory_add_final_point_trans(1, 0.5,
+ robot_trajectory_add_final_point_trans(1, 0.5,
TURN_CCW(DEG2RAD(0)));
break;
case EV_MOTION_DONE:
projects / eurobot / public.git / commitdiff