-#ifndef SUB_STATES_H
-#define SUB_STATES_H
-
#define FSM_MAIN
+#include <robot.h>
+//#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include <trgen.h>
+#include <stdbool.h>
+#include <ul_log.h>
+#include <hokuyo.h>
+#include "robodata.h"
+#include "actuators.h"
+#include "guard.hpp"
#include "roboevent.h"
-#include <fsm.h>
+#include <timedFSM.h>
+#include <boost/mpl/list.hpp>
+#include <boost/statechart/transition.hpp>
+#include <boost/statechart/custom_reaction.hpp>
+#include "state_defines.h"
+#include "common-states.h"
+//UL_LOG_CUST(ulogd_sub_states); /* Log domain name = ulogd + name of the file */
/************************************************************************
- * FSM STATES DECLARATION
+ * Functions used in and called from all the (almost identical)
+ * "wait for start" states in particular strategies.
************************************************************************/
+/*#undef DBG_FSM_STATE
+#define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
+ fsm->debug_name, robot_current_time(), \
+ name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+
+*/
+#define HOK_MIN_MM (ROBOT_AXIS_TO_FRONT_MM - HOKUYO_CENTER_OFFSET_MM)
+#define HOK_MIN_M (HOK_MIN_MM/1000.0)
+#define HOK_MAX_MM (1000)
+
+/**
+ * Count the angle to turn by to see the minimum distance in the robot axis.
+ * @param alpha poiner to the angle to turn by
+ * @param minimum pointer to the minimum distance from hokuyo
+ */
+void get_hokuyo_min(double *alpha, double *minimum)
+{
+ double beta;
+ double min = 1000;
+ int min_i;
+
+ struct hokuyo_scan_type scan = robot.hokuyo;
+ uint16_t *data = scan.data;
+
+ for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
+ beta = HOKUYO_INDEX_TO_RAD(i);
+ if((beta < (-50.0 / 180.0 * M_PI)) || ((beta > (50.0 / 180.0 * M_PI))))
+ continue;
+
+ if(data[i] > HOK_MIN_MM && data[i] < HOK_MAX_MM) {
+ if (data[i] < min ) {
+ min = data[i];
+ min_i = i;
+ }
+ // printf("min: %f, beta: %f\n", min, beta);
+ }
+ }
+ min /= 1000.0;
+ beta = HOKUYO_INDEX_TO_RAD(min_i);
+ *alpha = atan2((min * sin(beta)), (HOKUYO_CENTER_OFFSET_M + min * cos(beta)));
+ *minimum = min - HOK_MIN_M;
+
+ printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
+}
+
+void enable_obstacle_avoidance(bool enable)
+{
+ robot.obstacle_avoidance_enabled = enable;
+ robot.ignore_hokuyo = !enable;
+ robot.check_turn_safety = enable;
+}
+struct recognize_init : TimedSimpleState<recognize_init, approach_target> {
+ typedef sc::transition<evRunSubFSM, recognize> reactions;
+};
+
+struct recognize: TimedState<recognize, approach_target> {
+ int camera_delay;
+ Timer t;
+ recognize(my_context ctx) : base_state(ctx) {
+ camera_delay = 0;
+ act_camera_on();
+#if PPC
+ runTimer(t,1000, new evTimer());
+#else
+ robot.target_valid = false;
+ robot.sched.queue_event(robot.MAIN, new evCameraDone());
+#endif
+ }
+ ~recognize() {
+ camera_delay = 0;
+ }
+ sc::result react(const evTimer &)
+ {
+ if (++camera_delay > 10) {
+ // waiting for 10 seconds now, return back
+ {
+ Guard g(robot.lock_camera);
+ robot.target_valid = false;
+ }
+ robot.sched.queue_event(robot.MAIN, new evReturn());
+ } else {
+ runTimer(t,1000, new evTimer());
+ }
+ }
+ sc::result react(const evCameraDone &) {
+ act_camera_off();
+ if (robot.target_valid) {
+ //DBG_PRINT_EVENT("camera: Target valid!");
+ return transit<get_target_turn>();
+ } else {
+ //DBG_PRINT_EVENT("camera: Target not valid!");
+ robot.target_loaded = false;
+ robot.sched.queue_event(robot.MAIN, new evReturn());
+ return discard_event();
+ }
+ }
+ typedef mpl::list<
+ sc::custom_reaction<evTimer>,
+ sc::custom_reaction<evCameraDone> > reactions;
+};
+
+struct get_target_turn : TimedSimpleState<get_target_turn, approach_target> {
+ double close;
+ double alpha, minimum;
+ double x, y, phi;
+ get_target_turn() {
+ close = 0.05;
+// DBG_PRINT_EVENT("turn");
+ enable_obstacle_avoidance(false);
+ get_hokuyo_min(&alpha, &minimum);
+ robot.get_est_pos(x, y, phi);
+ robot.move_helper.move_by(0, TURN(alpha+phi), &tcVerySlow);
+ }
+ sc::result react(const evMotionDone& ) {
+ get_hokuyo_min(&alpha, &minimum);
+ if (minimum < close) {
+ return transit<get_target_load>();
+ } else {
+ return transit<get_target_touch>();
+ }
+ }
+ sc::result react( const evMotionError& ) {
+ enable_obstacle_avoidance(true);
+ robot.sched.queue_event(robot.MAIN, new evReturn());
+ return discard_event();
+ }
+ typedef mpl::list<
+ sc::custom_reaction<evMotionDone>,
+ sc::custom_reaction<evMotionError> > reactions;
+};
+
+struct get_target_touch : TimedSimpleState<get_target_touch, approach_target> {
+ double step;
+ double x, y, phi;
+ get_target_touch() {
+ step = 0.02;
+// DBG_PRINT_EVENT("touch");
+ robot.move_helper.move_by(step, NO_TURN(), &tcVerySlow);
+ }
+ sc::result react(const evMotionError& ) {
+ enable_obstacle_avoidance(true);
+ robot.sched.queue_event(robot.MAIN, new evReturn());
+ return discard_event();
+ }
+ typedef mpl::list<
+ sc::transition<evMotionDone, get_target_turn>,
+ sc::custom_reaction<evMotionError> > reactions;
+};
-FSM_STATE_DECL(recognize);
-FSM_STATE_DECL(get_target_turn);
-FSM_STATE_DECL(get_target_touch);
-FSM_STATE_DECL(get_target_load);
-FSM_STATE_DECL(get_target_back);
+struct get_target_load : TimedSimpleState<get_target_load, approach_target> {
+ int direction;
+ Timer t;
+ get_target_load() {
+ direction = 0;
+ act_magnet(1);
+ act_crane(CRANE_DOWN);
+ }
+ sc::result react (const evTimer&) {
+ act_crane(CRANE_UP);
+ robot.target_loaded = true;
+ return transit<get_target_back>();
+ }
+ sc::result react (const evCraneDone &) {
+ if (direction == 0) {
+ direction = 1;
+ runTimer(t,1000, new evTimer());
+ }
+ return discard_event();
+ }
+ typedef mpl::list<
+ sc::custom_reaction<evTimer>,
+ sc::custom_reaction<evCraneDone> > reactions;
+};
-void get_hokuyo_min(double *alpha, double *minimum);
+struct get_target_back : TimedSimpleState<get_target_back, approach_target> {
+ get_target_back() {
+ robot.move_helper.move_by(-0.3, NO_TURN(), &tcVerySlow);
+ }
+ ~get_target_back() {
+ enable_obstacle_avoidance(true);
+ }
+ sc::result react(const evMotionError &) {
+ robot.sched.queue_event(robot.MAIN, new evReturn());
+ return discard_event();
+ }
+ sc::result react(const evMotionDone &) {
+ robot.sched.queue_event(robot.MAIN, new evReturn());
+ return discard_event();
+ }
+ typedef mpl::list<
+ sc::custom_reaction<evMotionDone>,
+ sc::custom_reaction<evMotionError> > reactions;
+};
-#endif
\ No newline at end of file
+/*
+FSM_STATE()
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_CRANE_DONE:
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+*/
\ No newline at end of file
projects / eurobot / public.git / blobdiff