robofsm: fsmmove - logging

Prepare logging of states on terminal.

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index b4a48317a9891161d3349adec31c994fe2b18953..a06b21b6778f050638833c1dbdfa67fe562dcfc2 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -257,7 +257,7 @@ target_status recalculate_trajectory(void) {
        return ret;
 }
 
-bool back_bumpers() {
+inline bool back_bumpers() {
        return robot.moves_backward && robot.use_back_bumpers &&
                    (robot.orte.robot_bumpers.bumper_left_across ||
                    robot.orte.robot_bumpers.bumper_right_across ||
@@ -266,7 +266,7 @@ bool back_bumpers() {
 }
 
 
-bool side_bumpers() { 
+inline bool side_bumpers() { 
        return (robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
                    (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right);
 }
@@ -295,6 +295,7 @@ struct MotionBase : sc::simple_state< MotionBase, FSMMotion, wait_for_command >
 struct wait_for_command : TimedSimpleState< wait_for_command, MotionBase > {
        target_status ret;
        wait_for_command() {
+               printf("FSMMotion - wait_for_command\n");
                stop();
        }
        sc::result react(const evNewTarget &event) {
@@ -313,6 +314,9 @@ struct wait_for_command : TimedSimpleState< wait_for_command, MotionBase > {
 };
 
 struct movement : TimedSimpleState< movement, MotionBase > {
+       movement() {
+               printf("FSMMotion - movement\n");
+       }
        sc::result react(const evTrajectoryDone &) {
                robot.sched.queue_event(robot.MAIN, new evMotionDone());
                return transit<wait_for_command>();
@@ -355,6 +359,7 @@ struct movement : TimedSimpleState< movement, MotionBase > {
 struct lost : TimedState< lost, MotionBase > {
        Timer lost_tout;
        lost(my_context ctx) : base_state(ctx) {
+               printf("FSMMotion - lost\n");
                stop();
                runTimer(lost_tout, 1000, new evTimer());
        }
@@ -376,6 +381,7 @@ struct lost : TimedState< lost, MotionBase > {
 struct wait_and_try_again : TimedState< wait_and_try_again, MotionBase > {
        Timer wait_tout;
        wait_and_try_again(my_context ctx) : base_state(ctx) {
+               printf("FSMMotion - wait_and_try_again\n");
                stop();
                runTimer(wait_tout, 1000, new evTimer());
        }