6 #include <movehelper.h>
18 #include "actuators.h"
19 #include "match-timing.h"
20 #include "common-states.h"
21 #include "sub-states.h"
23 UL_LOG_CUST(ulogd_sub_states); /* Log domain name = ulogd + name of the file */
25 extern struct TrajectoryConstraints tcSlow, tcVerySlow;
27 /************************************************************************
28 * Functions used in and called from all the (almost identical)
29 * "wait for start" states in particular strategies.
30 ************************************************************************/
33 #define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
34 fsm->debug_name, robot_current_time(), \
35 name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
38 #define HOK_MIN_MM (ROBOT_AXIS_TO_FRONT_MM - HOKUYO_CENTER_OFFSET_MM)
39 #define HOK_MIN_M (HOK_MIN_MM/1000.0)
40 #define HOK_MAX_MM (1000)
43 * Count the angle to turn by to see the minimum distance in the robot axis.
44 * @param alpha poiner to the angle to turn by
45 * @param minimum pointer to the minimum distance from hokuyo
47 void get_hokuyo_min(double *alpha, double *minimum)
53 struct lidar_scan_type scan = robot.hokuyo;
54 uint16_t *data = scan.data;
56 for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
57 beta = HOKUYO_INDEX_TO_RAD(i);
58 if((beta < (-50.0 / 180.0 * M_PI)) || ((beta > (50.0 / 180.0 * M_PI))))
61 if(data[i] > HOK_MIN_MM && data[i] < HOK_MAX_MM) {
66 // printf("min: %f, beta: %f\n", min, beta);
70 beta = HOKUYO_INDEX_TO_RAD(min_i);
71 *alpha = atan2((min * sin(beta)), (HOKUYO_CENTER_OFFSET_M + min * cos(beta)));
72 *minimum = min - HOK_MIN_M;
74 printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
77 void get_camera_ang(double *alpha)
79 *alpha = DEG2RAD(robot.target_ang);
82 void enable_obstacle_avoidance(bool enable)
84 robot.obstacle_avoidance_enabled = enable;
85 robot.ignore_hokuyo = !enable;
86 robot.check_turn_safety = enable;
91 static int camera_delay = 0;
95 act_camera_recognize(robot.target_color);
96 DBG_PRINT_EVENT("recognition");
100 if (++camera_delay > 10) {
101 // waiting for 10 seconds now, return back
102 DBG_PRINT_EVENT("camera: No response!");
103 ROBOT_LOCK(camera_result);
104 robot.target_valid = false;
105 ROBOT_UNLOCK(camera_result);
113 if (robot.target_valid) {
114 robot.target_valid = false;
115 DBG_PRINT_EVENT("camera: Target valid!");
116 FSM_TRANSITION(get_target_turn);
118 DBG_PRINT_EVENT("camera: Target not valid!");
119 robot.target_loaded = false;
129 FSM_STATE(get_target_turn)
131 const double close = 0.05;
132 double alpha, minimum;
134 static char first = 1;
138 DBG_PRINT_EVENT("turn");
139 //enable_obstacle_avoidance(false);
142 // use angle from camera on first run
143 get_camera_ang(&alpha);
147 // than navigate using hokuyo
148 get_hokuyo_min(&alpha, &minimum);
151 robot_get_est_pos(&x, &y, &phi);
152 robot_move_by(0, TURN(alpha+phi), &tcSlow);
157 get_hokuyo_min(&alpha, &minimum);
158 if (minimum < close) {
159 FSM_TRANSITION(get_target_load);
161 FSM_TRANSITION(get_target_touch);
164 case EV_MOTION_ERROR:
165 enable_obstacle_avoidance(true);
173 FSM_STATE(get_target_touch)
175 const double step = 0.02;
180 DBG_PRINT_EVENT("touch");
181 robot_move_by(step, NO_TURN(), &tcSlow);
186 FSM_TRANSITION(get_target_turn);
188 case EV_MOTION_ERROR:
189 enable_obstacle_avoidance(true);
197 FSM_STATE(get_target_load)
199 static char delay = 0;
206 // target was not confirmed
207 DBG_PRINT_EVENT("OMRON: Target NOT valid!");
208 robot.target_loaded = false;
209 robot.target_valid = false;
210 FSM_TRANSITION(get_target_unload);
216 // target confirmed as valid by OMRON sensor
217 // return to main automaton and transport target to storage
218 DBG_PRINT_EVENT("OMRON: Target valid!");
219 robot.target_loaded = true;
220 robot.target_valid = true;
228 /* unload target if not valid = no color match using OMRON */
229 FSM_STATE(get_target_unload)
235 robot.target_loaded = true;
236 FSM_TRANSITION(get_target_back);
244 FSM_STATE(get_target_back)
248 robot_move_by(-0.5, NO_TURN(), &tcVerySlow);
253 case EV_MOTION_ERROR:
257 enable_obstacle_avoidance(true);
273 case EV_MOTION_ERROR:
274 case EV_SWITCH_STRATEGY:
275 DBG_PRINT_EVENT("unhandled event");