robofsm: Correct handling of back switch

- taken into account only after 5 sec after start
- react on first switch
- react on continuous switch only when moving backwards

diff --git a/src/robofsm/fsmmain.c b/src/robofsm/fsmmain.c
index 62efebb46d453bc9938725b25a8053e266feab15..410d818bf1a33a0d8adc43ddfaf9ea8a9d927a46 100644 (file)
--- a/src/robofsm/fsmmain.c
+++ b/src/robofsm/fsmmain.c
@@ -177,23 +177,30 @@ ROBOT:
  * Competition timer. Stop robot when the timer exceeds.
  *
  */
-void *wait_for_end(void *arg)
+void *timing_thread(void *arg)
 {
-       sleep(COMPETITION_TIME);
+       struct timespec start;
+
+       clock_gettime(CLOCK_MONOTONIC, &start);
+#define WAIT(sec)                                                      \
+       do {                                                            \
+               struct timespec t;                                      \
+               t.tv_sec = start.tv_sec+sec;                            \
+               t.tv_nsec = start.tv_nsec;                              \
+               clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); \
+       } while(0)
+
+       WAIT(5);
+       robot.use_back_switch = true;
+       printf("Back switch not ignored\n");
+
+       WAIT(TIME_TO_DEPOSITE);
+       FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
+
+       WAIT(COMPETITION_TIME);
        printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
        robot_exit();
-       return NULL;
-}
 
-/**
- * Timer to go to tray.
- *
- * FIXME: rename this function? (F.J.)
- */
-void *wait_to_deposition(void *arg)
-{
-       sleep(TIME_TO_DEPOSITE);
-       FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
        return NULL;
 }
 
@@ -300,8 +307,7 @@ FSM_STATE(wait_for_start)
                case EV_START:
 #endif
                        /* start competition timer */
-                       pthread_create(&thid, NULL, wait_for_end, NULL);
-                       pthread_create(&thid, NULL, wait_to_deposition, NULL);
+                       pthread_create(&thid, NULL, timing_thread, NULL);
                        robot_set_est_pos_trans(0.16,
                                                PLAYGROUND_HEIGHT_M - 0.16,
                                                DEG2RAD(-45));

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index 42102082179bd2c7cc2cfc50117e3e9e25576314..905eccf6eceb8477be6914508ad95781dc362997 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -189,6 +189,8 @@ void robot_send_speed(double left, double right) {
        r = (int)(right * mul);
        robot.orte.motion_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
        robot.orte.motion_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
+
+       robot.moves_backward = (left < -0.01 || right < -0.01);
 //     DBG("speed l=%8.3g (%6d) r=%8.3g (%6d)", 
 //         left, l, // robot.orte.motion_speed.left, 
 //         right, r //robot.orte.motion_speed.right

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 17918b609b663561a75a5cea7d61e54502b95e88..ecc0645232c3c2ff05a588daf11f437bda353772 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -180,6 +180,7 @@ int robot_init()
                robot.fsm.display.state = &fsm_state_disp_init;
 
        robot.obstacle_avoidance_enabled = true;
+       robot.use_back_switch = false; /* Switched on sime time after start */
        robot.state = POWER_ON;
 
        /* init ORTE domain, create publishers, subscribers, .. */

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 70f4747970eee603fb7cba2719a1dc90056b12f4..196dffb2dabefaddf1a6fcbbb496f452fc023ba8 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -152,6 +152,12 @@ struct robot {
        /** True if localization data arrives correctly and therfore
         * localization runs */
        bool localization_works;
+
+       bool use_back_switch;
+
+       /** True iff at least one wheel rotates backward */
+       bool moves_backward;
+       
        /*
         * sometimes H8S does not send queried odometry
         * following flag has been added for EKF estimator,

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 25f673109a263a3470f7a279764b25e26197fab1..2735cea1453421dbe7f4edce87f726900bab67d1 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -320,11 +320,18 @@ void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
        static bool previous_switch_status;
        switch (info->status) {
        case NEW_DATA: {
-               if (robot.orte.puck_inside.status == 1) {
-                       //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
-                       FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+               if (robot.use_back_switch) {
+                       if (robot.orte.puck_inside.status & 0x01) {
+                               //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+
+                               /* React on the first switch or when moving backward. */
+                               if ((previous_switch_status & 0x01) == 0 ||
+                                   robot.moves_backward) {
+                                       FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+                               }
+                               previous_switch_status = robot.orte.puck_inside.status;
+                       }
                }
-               previous_switch_status = robot.orte.puck_inside.status;
                robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
        }
                break;