odometry.cc

#define FSM_MAIN
#include "fsmmove.h"
#include "movehelper.h"
#include "robot.h"
#include "trgen.h"

FSM_STATE_DECL(rectangle);
FSM_STATE_DECL(turn);
FSM_STATE_DECL(wait);

FSM_STATE(main_init) {
        // Where we are at the begining?
        robot.act_pos.x = 1;
        robot.act_pos.y = 0.5;
        robot.act_pos.phi = 0*M_PI/2;
        FSM_TRANSITION(rectangle);
}


void follow_rectangle()
{
        static bool backward = false;
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
        //tc.maxv *= 1.5;

        // Allocate new trajectory
        if (!backward)
                robot_trajectory_new(&tc);
        else
                robot_trajectory_new_backward(&tc);
        backward = !backward;

        // Add random point to the trajectory.
        robot_trajectory_add_point(2, 0.5);
        robot_trajectory_add_point(2, 1.5);
        robot_trajectory_add_point(1, 1.5);
        robot_trajectory_add_final_point(1, 0.5, 90);
}

FSM_STATE(rectangle)
{
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        break;
                case EV_TRAJECTORY_ERROR:
                        FSM_TRANSITION(turn);
                        break;
                case EV_MOTION_DONE:
                        FSM_TRANSITION(turn);
                        break;
                default:
                        break;
        }
}

FSM_STATE(turn)
{
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        robot_trajectory_new(NULL);
                        robot_trajectory_add_final_point(1, 0.5, -90);
                        break;
                case EV_TRAJECTORY_ERROR:
                        FSM_TRANSITION(wait);
                        break;
                case EV_MOTION_DONE:
                        FSM_TRANSITION(wait);
                        break;
                default:
                        break;
        }
}


FSM_STATE(wait)
{
        FSM_TIMER(1000);
        switch (FSM_EVENT) {
                case EV_TIMER:
                        FSM_TRANSITION(rectangle);
                        break;
                default:
                        break;
        }
}

int main()
{
        /* robot initialization */
        robot_init();

        FSM(MAIN)->debug_states = 1;
//      FSM(MOVE)->debug_states = 1;

        robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
        robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;

        /* Start threads and wait */
        robot_start();
        robot_wait();

        /* clean up */
        robot_destroy();

        return 0;
}

---