+++ /dev/null
-#define FSM_MAIN
-#include <robot.h>
-#include <fsm.h>
-#include <unistd.h>
-#include <math.h>
-#include <movehelper.h>
-#include <map.h>
-#include <sharp.h>
-#include <robomath.h>
-#include <string.h>
-#include <robodim.h>
-#include <error.h>
-#include <trgen.h>
-#include <stdbool.h>
-#include <ul_log.h>
-#include <shape_detect.h>
-
-#include "robodata.h"
-#include "actuators.h"
-#include "match-timing.h"
-#include "common-states.h"
-#include "sub-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.
- ************************************************************************/
-
-#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)
-
-/************************************************************************
- * Trajectory constraints used; They are initialized in the main() function in competition.cc
- ************************************************************************/
-
-struct TrajectoryConstraints tcSlow, tcVerySlow;
-
-/**
- * Vector where all absolute positions of all detected targets are stored.
- */
-std::vector<robot_pos_type> detected_target;
-
-/**
- * Safe distance for target recognition
- */
-const double approach_radius = TARGET_RADIUS_M + 3.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
-
-void set_initial_position()
-{
- robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
-}
-
-void actuators_home()
-{
- act_crane(CRANE_UP);
- act_magnet(0);
-}
-
-/* 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)
-{
- const double close = 0.5;
- double x, y, phi;
- robot_get_est_pos(&x, &y, &phi);
-
- if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
- return true;
- else
- return false;
-}
-
-/**
- * Take data from hokuyo and run shape detection on it.
- *
- * Absolute positions of all detected targets centers are stored in alobal variable (vector).
- *
- * @return True if at least one target detected, else false.
- */
-static bool detect_target()
-{
- struct hokuyo_scan_type hokuyo = robot.hokuyo;
-
- Shape_detect sd;
- std::vector<Shape_detect::Arc> arcs;
- sd.prepare(hokuyo.data);
- sd.arc_detect(arcs);
-
- // clear old targets
- detected_target.clear();
-
- if (arcs.size() > 0) {
- robot_pos_type e, target, hok;
-
- robot_get_est_pos(&e.x, &e.y, &e.phi);
-
- double sinus = sin(e.phi);
- double cosinus = cos(e.phi);
-
- // save absolute positions of all detected targets
- for (int i = 0; i < arcs.size(); i++) {
- Shape_detect::Arc *a = &arcs[i];
-
- hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
- hok.y = (double)a->center.y / 1000.0;
-
- /* transform target position which is relative to Hokuyo
- center to absolute position in space */
- target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
- target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
-
- // filter those targets not in playground range
- if (goal_is_in_playground(target.x, target.y))
- detected_target.push_back(target);
- }
- }
- return detected_target.size();
-}
-
-/**
- * Calculates point to approach the target.
- *
- * @param target Position of the center of the target.
- * @param approach Pointer to the the intersection point of circle around
- * the target and line between robot center and target.
- */
-void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
-{
- double xrobot, yrobot, phi;
- double delta;
-
- robot_get_est_pos(&xrobot, &yrobot, &phi);
-
- delta = distance(xrobot, yrobot, xtarget, ytarget);
-
- *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
- *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
-
- *phi_approach = get_approach_angle(xtarget, ytarget);
-}
-
-/**
- * Calculates point to approach the target.
- *
- * @param target Position of the center of the target.
- * @return Angle to approach the target form.
- */
-double get_approach_angle(double xtarget, double ytarget)
-{
- double xrobot, yrobot,phi;
-
- robot_get_est_pos(&xrobot, &yrobot, &phi);
-
- return atan2((ytarget - yrobot), (xtarget - xrobot));
-}
-
-
-/**
- * FSM state for neighborhood observation.
- *
- * Detect targets using shape_detect.
- * If no target detected, turn 120deg and try again.
- * Scan all 360deg and then go back to move_around state.
- *
- * If target detected, go to approach_target state.
- */
-FSM_STATE(survey)
-{
- static char turn_cntr = 0;
- double x, y, phi;
-
- switch(FSM_EVENT) {
- case EV_ENTRY:
- DBG_PRINT_EVENT("survey");
-#if 1 // FIXME just for test
- if (detect_target()) {
-#else
- if (turn_cntr > 1) {
- robot_pos_type target;
- detected_target.clear();
- for (double i = 1; i < 5; i++) {
- target.x = i;
- target.y = i/2.0;
- detected_target.push_back(target);
- }
-#endif
- // target detected, go to the target
- FSM_TRANSITION(approach_target);
- DBG_PRINT_EVENT("Target detected!");
- } else {
- // no target detected in this heading, turn 120°
- robot_get_est_pos(&x, &y, &phi);
- robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
- turn_cntr++;
- DBG_PRINT_EVENT("no target");
- }
- break;
- case EV_TIMER:
- if (turn_cntr > 2) {
- FSM_TRANSITION(move_around);
- } else {
- FSM_TRANSITION(survey);
- }
- break;
- case EV_MOTION_DONE:
- FSM_TIMER(1000);
- break;
- case EV_MOTION_ERROR:
- FSM_TRANSITION(move_around);
- break;
- case EV_EXIT:
- turn_cntr = 0;
- break;
- }
-}
-
-/**
- * FSM state for approaching all detected targets.
- *
- * Try to approach target.
- * If approach OK - go to subautomaton and do target recognition, touch and load.
- * On subautomaton return check if target loaded/valid.
- *
- * If target loaded, go home.
- * If target not valid, try next target if any.
- * If approach not succesfull - go to move_around state.
- */
-FSM_STATE(approach_target)
-{
- static int target_cntr = 0;
- int max_target = detected_target.size();
- double x_target, y_tatget;
- double x_approach, y_approach, phi_approach;
-
- switch(FSM_EVENT) {
- case EV_ENTRY:
- DBG_PRINT_EVENT("approaching target");
- x_target = detected_target[target_cntr].x;
- y_tatget = detected_target[target_cntr].y;
- target_cntr++;
-
- printf("target %d / %d\n", target_cntr, max_target);
-
- get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
- robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.1), &tcSlow);
- break;
- case EV_MOTION_DONE:
- DBG_PRINT_EVENT("target approached");
- SUBFSM_TRANSITION(recognize, NULL);
- break;
- case EV_RETURN:
- if (robot.target_loaded) {
- FSM_TRANSITION(go_home);
- } else if (robot.target_valid) {
- //FIXME target is valid but not loaded - try another approach direction
-
- } else if (!robot.target_valid && (target_cntr < max_target)) {
- // go for next target if any
- FSM_TRANSITION(approach_target);
- } else {
- // go to new point and survey
- FSM_TRANSITION(move_around);
- }
- break;
- case EV_MOTION_ERROR:
- DBG_PRINT_EVENT("can not approach target");
- if (target_cntr < max_target) {
- FSM_TRANSITION(approach_target);
- } else {
- FSM_TRANSITION(move_around);
- }
- break;
- case EV_EXIT:
- target_cntr = 0;
- break;
- }
-}
-
-FSM_STATE(move_around)
-{
- double goalx, goaly;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- do {
- goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
- goaly += ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
- } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
-
- robot_goto_notrans(goalx, goaly, NO_TURN(), &tcSlow);
- DBG_PRINT_EVENT("new survey point");
- break;
- case EV_MOTION_ERROR:
- DBG_PRINT_EVENT("can not access survey point");
- case EV_MOTION_DONE:
- FSM_TRANSITION(survey);
- break;
- case EV_TIMER:
- break;
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(go_home)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- DBG_PRINT_EVENT("homing");
- robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
- break;
- case EV_MOTION_ERROR:
- DBG_PRINT_EVENT("ERROR: home position is not reachable!");
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(go_home);
- break;
- case EV_MOTION_DONE:
- case EV_EXIT:
- DBG_PRINT_EVENT("Mission completed!");
- robot_exit();
- break;
- }
-}
-
-/*
-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