#define FSM_MAIN
-#include "robodata.h"
#include <robot.h>
#include <fsm.h>
#include <unistd.h>
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "actuators.h"
-#include <sharp.h>
#include <trgen.h>
-#include "match-timing.h"
#include <stdbool.h>
#include <ul_log.h>
+#include <shape_detect.h>
-UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
-
+#include "robodata.h"
+#include "actuators.h"
+#include "match-timing.h"
#include "common-states.h"
+UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
+
/************************************************************************
* Functions used in and called from all the (almost identical)
* "wait for start" states in particular strategies.
fsm->debug_name, robot_current_time(), \
name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+/************************************************************************
+ * Trajectory constraints used; They are initialized in the main() function in competition.cc
+ ************************************************************************/
-void set_initial_position()
-{
- robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
- 0);
-}
-
-void actuators_home()
-{
- act_jaws(CLOSE);
+struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-// bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
- // drive lift to home position
- act_lift(0, 0, 1);
- // unset the homing request
- //act_lift(0, 0, 0);
-}
-void start_entry()
+void set_initial_position()
{
- pthread_t thid;
- robot.check_turn_safety = false;
- pthread_create(&thid, NULL, timing_thread, NULL);
- start_timer();
+ robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
}
-// We set initial position periodically in order for it to be updated
-// on the display if the team color is changed during waiting for
-// start.
-void start_timer()
+void actuators_home()
{
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
- actuators_home();
+ //act_crane(CRANE_UP);
+ //act_magnet(0);
}
-void start_go()
+/* Check if the new point is within the playground scene */
+bool goal_is_in_playground(double goalx, double goaly)
{
- sem_post(&robot.start);
- actuators_home();
- set_initial_position();
+ if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
+ return false;
+ else
+ return true;
}
-void start_exit()
+/* Check if the new point is close to the robot */
+bool close_goal(double goalx, double goaly)
{
+ double x, y, phi;
+ robot_get_est_pos(&x, &y, &phi);
+ if ((abs(goalx - x) < 0.5) && (abs(goaly - y) < 0.5) )
+ return true;
+ else
+ return false;
}
-bool read_sharp()
-{
- int sharp_data = robot.orte.jaws_status.act_pos.left;
- int sharp_dist = 0;
- sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
- printf("sharp data: %dmm\n", sharp_dist);
- return (sharp_dist <= 150 ? true : false);
-}
-
-void inline enable_bumpers(bool enabled)
+static bool detect_target()
{
- robot.use_left_bumper = enabled;
- robot.use_right_bumper = enabled;
- robot.use_back_bumpers = enabled;
-}
+ // run shape detection
+ // return TRUE when any circle detected
+ struct hokuyo_scan_type hokuyo = robot.hokuyo;
+ Shape_detect sd;
+ std::vector<Shape_detect::Arc> arcs;
+ sd.prepare(hokuyo.data);
+ sd.arc_detect(arcs);
-/************************************************************************
- * Trajectory constraints used; They are initialized in the main() function in competition.cc
- ************************************************************************/
+ robot_pos_type e;
+ sharp_pos hok, target;
-struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
-double totem_x;
-double totem_y;
-bool up;
+ hok.x = HOKUYO_CENTER_OFFSET_M;
+ hok.y = 0;
-FSM_STATE(approach_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcVeryFast); // new trajectory
- robot_trajectory_add_point_trans(
- 0.6,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
- robot_trajectory_add_point_trans(
- 0.64,
- 1.2);
- robot_trajectory_add_point_trans(
- 0.64,
- 0.7);
- robot_trajectory_add_final_point_trans(
- 1.0,
- 0.45,
- ARRIVE_FROM(DEG2RAD(24),0.1));
-// robot_trajectory_add_final_point_trans(
-// 1.3,
-// 0.58,
-// NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(catch_central_buillon);
- break;
- default:
- break;
- }
-}
+ robot_get_est_pos(&e.x, &e.y, &e.phi);
-FSM_STATE(catch_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- 1.3,
- 0.58,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(CATCH);
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_central_buillon);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(leave_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- 0.85,
- 0.38,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- break;
- default:
- break;
- }
-}
+ if (arcs.size() > 0) {
+ Shape_detect::Arc * a = &arcs[0];
+ hok.x += (double)a->center.x / 1000.0;
+ hok.y = (double)a->center.y / 1000.0;
-FSM_STATE(push_bottle_bw)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.use_back_bumpers = false;
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.7,
- 0.3);
- robot_trajectory_add_final_point_trans(
- 0.64+0.08,
- ROBOT_AXIS_TO_BACK_M + 0.01,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(return_home);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(return_home)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_point_trans(
- 0.64 + 0.08,
- 0.8);
- robot_trajectory_add_final_point_trans(
- 0.55,
- 1.1,
- ARRIVE_FROM(DEG2RAD(180), 0.01));
- break;
- case EV_MOTION_DONE:
- robot.use_back_bumpers = true;
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- SUBFSM_RET(NULL);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(leave_home)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- //robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_move_by(-0.18, NO_TURN(), &tcSlow);
- break;
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TRANSITION(leave_home_for_totem);
- break;
- default:
- break;
- }
+ /* transform target position which is relative to Hokuyo center to absolute position in space */
+ robot.target_pos.x = (hok.x * cos(e.phi)) - (hok.y * sin(e.phi)) + e.x;
+ robot.target_pos.y = (hok.x * sin(e.phi)) + (hok.y * cos(e.phi)) + e.y;
+ return true;
+ } else {
+ return false;
+ }
}
-FSM_STATE(leave_home_for_totem)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- if(up) {
- robot_trajectory_add_final_point_trans(
- 0.7,
- 1.3,
- NO_TURN());
- }
- else {
- robot_trajectory_add_final_point_trans(
- 0.7,
- 0.9,
- NO_TURN());
- }
- break;
- case EV_MOTION_DONE:
- if(up) FSM_TRANSITION(approach_totem_up);
- else FSM_TRANSITION(approach_totem_down);
- break;
- default:
- break;
- }
-}
-FSM_STATE(approach_totem_down)
+/* automaton to survey neighborhood in two stages */
+FSM_STATE(survey)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 0.4,
- TURN_CCW(DEG2RAD(90)));
- break;
- case EV_MOTION_DONE:
- act_lift(UP, 0, 0);
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(catch_totem_buillon_down);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(catch_totem_buillon_down)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
- ARRIVE_FROM(DEG2RAD(90), 0.10));
- break;
- case EV_MOTION_DONE:
- act_jaws(CATCH);
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_totem_down);
- default:
- break;
- }
-}
+ static char turn_cntr = 0;
+ double x, y, phi;
-FSM_STATE(leave_totem_down)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 0.42,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- act_lift(DOWN, 0, 0);
- FSM_TIMER(2000);
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ /* run shape detection
+ - if detection is positive go to the target
+ - if no target detected turn 120° to the left */
+ if (detect_target() & 0) {
+ // target detected, go to the target
+ robot.target_detected = true;
+ FSM_TRANSITION(approach_target);
+ DBG_PRINT_EVENT("Target detected!");
+ } else {
+ // no target detected in this heading, turn 120°
+ robot.target_detected = false;
+ robot_get_est_pos(&x, &y, &phi);
+ robot_move_by(0, TURN(DEG2RAD(120)+phi), &tcFast);
+ turn_cntr++;
+ }
+ break;
+ case EV_JAWS_DONE:
+ //FSM_TIMER(1000);
break;
case EV_TIMER:
- FSM_TRANSITION(place_buillon_home);
+ if (turn_cntr > 2) {
+ turn_cntr = 0;
+ FSM_TRANSITION(move_around);
+ } else {
+ FSM_TRANSITION(survey);
+ }
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ break;
+ case EV_START:
+ case EV_RETURN:
break;
- default:
- break;
- }
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ FSM_TRANSITION(move_around);
+ break;
+ case EV_EXIT:
+ break;
+ }
}
-FSM_STATE(place_buillon_home)
+FSM_STATE(approach_target)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- act_jaws(CATCH);
- robot_trajectory_new(&tcSlow); // new trajectory
- if(up) {
- robot_trajectory_add_point_trans(
- 0.9,
- 1.52);
- }
- else {
- robot_trajectory_add_point_trans(
- 0.9,
- 0.48);
- robot_trajectory_add_point_trans(
- 0.7,
- 0.8);
- }
- robot_trajectory_add_final_point_trans(
- 0.55,
- 1.1,
- ARRIVE_FROM(DEG2RAD(180),0.01));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- SUBFSM_RET(NULL);
- default:
- break;
- }
-}
+ double x, y;
-FSM_STATE(approach_totem_up)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 1.6,
- TURN_CW(DEG2RAD(270)));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- act_lift(UP, 0, 0);
- FSM_TIMER(2000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(catch_totem_buillon_up);
- break;
- default:
- break;
- }
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ x = robot.target_pos.x;
+ y = robot.target_pos.y;
+
+ if (goal_is_in_playground(x, y)) {
+ /*TODO
+ 1. calculate position for approach
+ 2. approach the target to 30cm distance
+ 3. then use camera? to validate the target
+ 4. approach the target closely - using hokuyo and sharp?
+ 5. load the cargo
+ 6. move backwards 0.5m
+ 7. transport cargo to the home position
+ */
+ //ShmapSetCircleFlag(x, y, 0.1, MAP_FLAG_TARGET, 0);
+ //ShmapSetCircleFlag(x, y, 0.1, MAP_FLAG_IGNORE_OBST, NULL);
+ //robot_goto_notrans(x, y, NO_TURN(), &tcSlow);
+ printf("regular target x:%f y:%f\n",x,y);
+ } else {
+ DBG_PRINT_EVENT("Target out of range!");
+ printf("false target x:%f y:%f\n",x,y);
+ }
+ break;
+ case EV_START:
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ DBG_PRINT_EVENT("Mission completed :)");
+ break;
+ case EV_RETURN:
+ case EV_JAWS_DONE:
+ break;
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
}
-FSM_STATE(catch_totem_buillon_up)
+FSM_STATE(move_around)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- act_jaws(CATCH);
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_totem_up);
- break;
- default:
- break;
- }
-}
+ double goalx, goaly, phi;
+ static int survey_cnt = 0;
-FSM_STATE(leave_totem_up)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 1.6,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- act_lift(DOWN, 0, 0);
- SUBFSM_RET(NULL);
- default:
- break;
- }
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ /* TODO upravit generovani nahodne pozice tak aby se lepe filtrovaly
+ uz prozkomane body a prekazky i body mimo hriste.
+ Oznacit v mape vsechny oblasti prozkoumane pomoci hokuya na kterych nedoslo k detekci??
+ */
+ do {
+ robot_get_est_pos(&goalx, &goaly, &phi);
+ goalx += ((rand()%20000)/1000.0) - 10;
+ goaly += ((rand()%20000)/1000.0) - 10;
+ } while (!ShmapIsFreePoint(goalx, goaly)
+ && !goal_is_in_playground(goalx, goaly)
+ && close_goal(goalx, goaly));
+
+ robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
+ survey_cnt++;
+ break;
+ case EV_MOTION_ERROR:
+ ul_logdeb("Goal is not reachable\n");
+ FSM_TIMER(1000);
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(survey);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(move_around);
+ break;
+ case EV_RETURN:
+ break;
+ case EV_START:
+ /* do nothing */
+ break;
+ case EV_EXIT:
+ //ShmapFree();
+ break;
+ default:
+ break;
+ }
}
-
-FSM_STATE(push_second_bottle_fw)
+FSM_STATE(go_home)
{
- switch(FSM_EVENT) {
+ switch (FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
-// robot_trajectory_add_point_trans(0.7, 0.5);
- robot_goto_notrans(
- 1.85,
- ROBOT_AXIS_TO_FRONT_M,
- ARRIVE_FROM(DEG2RAD(270), 0.15), &tcFast);
+ robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(0, 0.5), &tcFast);
+ break;
+ case EV_MOTION_ERROR:
+ ul_logdeb("Goal is not reachable\n");
+ FSM_TIMER(1000);
break;
case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_home);
+ break;
+ case EV_START:
+ /* do nothing */
+ break;
+ case EV_EXIT:
+ //ShmapFree();
+ break;
default:
break;
}
}
-FSM_STATE(go_back_from_home)
+/*
+FSM_STATE()
{
switch(FSM_EVENT) {
case EV_ENTRY:
- act_jaws(OPEN);
- robot_move_by(-0.2, NO_TURN(), &tcSlow);
break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_CRANE_DONE:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TRANSITION(push_second_bottle_fw);
- break;
- default:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
break;
}
-}
\ No newline at end of file
+}
+*/
#include "roboevent.h"
#include <fsm.h>
-extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
-extern double totem_x, totem_y;
-extern bool up;
-/* strategy FSM */
-FSM_STATE_DECL(central_buillon_wait_for_start);
-FSM_STATE_DECL(homologation_wait_for_start);
-FSM_STATE_DECL(calibrate);
-
-/* Strategy catch buillon in center */
-FSM_STATE_DECL(approach_central_buillon);
-FSM_STATE_DECL(catch_central_buillon);
-FSM_STATE_DECL(leave_central_buillon);
-FSM_STATE_DECL(push_bottle_bw);
-FSM_STATE_DECL(return_home);
-
-/* Ignore central buillon */
-//FSM_STATE_DECL(push_nearest_buillon);
-//FSM_STATE_DECL(push_bottle_fw);
-
-/* Autocalibration */
-FSM_STATE_DECL(go_back_for_cal);
-
-/* Common states */
-FSM_STATE_DECL(leave_home);
-FSM_STATE_DECL(leave_home_for_totem);
-FSM_STATE_DECL(approach_totem_down);
-FSM_STATE_DECL(catch_totem_buillon_down);
-FSM_STATE_DECL(leave_totem_down);
-FSM_STATE_DECL(place_buillon_home);
-FSM_STATE_DECL(approach_totem_up);
-FSM_STATE_DECL(catch_totem_buillon_up);
-FSM_STATE_DECL(leave_totem_up);
-
-/*FSM_STATE_DECL(place_down_buillon);
-FSM_STATE_DECL(approach_totem_up);
-FSM_STATE_DECL(catch_up_totem_buillon);
-FSM_STATE_DECL(leave_totem_up);
-FSM_STATE_DECL(place_up_buillon);
-
-FSM_STATE_DECL(push_second_bottle);
-*/
-/* HOMOLOGATION states */
-/* movement states - buillon */
-FSM_STATE_DECL(homologation_approach_buillon);
-/* Pushing the bottle */
-FSM_STATE_DECL(homologation_push_bottle);
-FSM_STATE_DECL(push_second_bottle_fw);
-FSM_STATE_DECL(go_back_from_home);
-
-
-void start_entry();
-void start_timer();
-void start_go();
-void start_exit();
-bool read_sharp();
-
-void robot_calibrate_odometry();
-
-
-
+#define HOME_POS_X_M 10
+#define HOME_POS_Y_M 1
+#define HOME_POS_ANG_DEG 90
+
+extern struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+FSM_STATE_DECL(survey);
+FSM_STATE_DECL(approach_target);
+FSM_STATE_DECL(move_around);
+FSM_STATE_DECL(go_home);
+
+void set_initial_position(void);
+void actuators_home(void);
+bool goal_is_in_playground(double goalx, double goaly);
+bool close_goal(double goalx, double goaly);
#endif
#endif
#define FSM_MAIN
+
#include <robot.h>
#include <fsm.h>
#include <unistd.h>
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "actuators.h"
#include <trgen.h>
-#include "match-timing.h"
#include <ul_log.h>
#include <path_planner.h>
+#include <can_msg_def.h>
-UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
-
-#define HOME_POS_X_M 5
-#define HOME_POS_Y_M 5
-#define HOME_POS_ANG_RAD 0
-
-/************************************************************************
- * Trajectory constraints used, are initialized in the init state
- ************************************************************************/
-
-struct TrajectoryConstraints tcSlow, tcFast;
+#include "actuators.h"
+#include "match-timing.h"
+#include "common-states.h"
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
+UL_LOG_CUST(ulogd_demo); /* Log domain name = ulogd + name of the file */
/* initial and starting states */
FSM_STATE_DECL(init);
FSM_STATE_DECL(wait_for_start);
-/* movement states */
-FSM_STATE_DECL(move_around);
-FSM_STATE_DECL(go_home);
-
-
-void set_initial_position()
-{
- robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, HOME_POS_ANG_RAD);
-}
-void actuators_home()
-{
- //act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
-}
+/* mission competed */
+FSM_STATE_DECL(go_home);
-/* Check if the new point is within the playground scene */
-bool goal_is_in_playground(double goalx, double goaly) {
- if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
- return false;
- else
- return true;
-}
-
-/* Check if the new point is close to the robot */
-bool close_goal(double goalx, double goaly) {
- if ((abs(goalx - robot.ref_pos.x) < 0.5) && (abs(goaly - robot.ref_pos.y) < 0.5) )
- return true;
- else
- return false;
-}
FSM_STATE(init)
{
switch (FSM_EVENT) {
case EV_ENTRY:
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.5;
- tcSlow.maxacc = 0.5;
-
tcFast = trajectoryConstraintsDefault;
tcFast.maxv = 1;
- tcFast.maxacc = 2;
- tcFast.maxomega = 3;
+ tcFast.maxacc = 0.5;
+ tcFast.maxomega = 0.5;
+
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.2;
+ tcSlow.maxomega = 0.2;
FSM_TRANSITION(wait_for_start);
break;
switch (FSM_EVENT) {
case EV_ENTRY:
pthread_create(&thid, NULL, timing_thread, NULL);
- FSM_TIMER(1000);
+ set_initial_position();
+ FSM_TIMER(2000);
break;
case EV_START:
/* start competition timer */
sem_post(&robot.start);
- actuators_home();
- set_initial_position();
- FSM_TRANSITION(move_around);
+ FSM_TRANSITION(survey);
break;
case EV_TIMER:
- FSM_TIMER(1000);
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
+ FSM_TIMER(1000);
+ if (robot.start_state == START_PLUGGED_IN)
actuators_home();
break;
case EV_RETURN:
case EV_MOTION_ERROR:
case EV_MOTION_DONE:
+ case EV_JAWS_DONE:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
case EV_EXIT:
break;
}
}
-FSM_STATE(move_around)
-{
- double goalx, goaly;
- static int survey_cnt = 0;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- do {
- goalx = robot.ref_pos.x + ((rand()%4000)/1000.0) - 2;
- goaly = robot.ref_pos.y + ((rand()%4000)/1000.0) - 2;
- //goalx = ((rand()%10000)/1000.0);
- //goaly = ((rand()%10000)/1000.0);
- } while (!ShmapIsFreePoint(goalx, goaly)
- && !goal_is_in_playground(goalx, goaly)
- && close_goal(goalx, goaly));
-
- robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
- break;
- case EV_MOTION_ERROR:
- ul_logdeb("Goal is not reachable\n");
- FSM_TIMER(1000);
- break;
- case EV_MOTION_DONE:
- if(++survey_cnt > 10) {
- FSM_TRANSITION(go_home);
- survey_cnt = 0;
- } else {
- FSM_TRANSITION(move_around);
- }
- break;
- case EV_TIMER:
- FSM_TRANSITION(move_around);
- break;
- case EV_START:
- /* do nothing */
- break;
- case EV_EXIT:
- //ShmapFree();
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(go_home)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, NO_TURN(), &tcFast);
- break;
- case EV_MOTION_ERROR:
- ul_logdeb("Goal is not reachable\n");
- FSM_TIMER(1000);
- break;
- case EV_MOTION_DONE:
- case EV_TIMER:
- FSM_TRANSITION(wait_for_start);
- break;
- case EV_START:
- /* do nothing */
- break;
- case EV_EXIT:
- //ShmapFree();
- break;
- default:
- break;
- }
-}
-
/************************************************************************
* MAIN FUNCTION
projects / eurobot / public.git / commitdiff