testmove.cc

#include "robot.h"
#include "robofsm.h"
#include "movefsm.h"
#include "trgen.h"
#include <orte.h>

static int wait_move(struct robo_fsm *fsm, int ret);
static int go2(struct robo_fsm *fsm, int ret);
static int go1(struct robo_fsm *fsm, int ret); 
static int test_move(struct robo_fsm *fsm, int ret);

static int wait_move(struct robo_fsm *fsm, int ret) 
{
        DBG_STATE();
        switch (fsm->event) {
                case EV_TRAJECTORY_DONE:
                        DBG("Trajectory done\n");
                        FSM_TIMEOUT(3000);
                        return RC_WAIT;
                        break;
                case EV_TRAJECTORY_DONE_AND_CLOSE:
                        DBG("Trajectory done and close\n");
                        FSM_TIMEOUT(3000);
                        return RC_WAIT;
                        break;
                case EV_TRAJECTORY_LOST:
                        DBG("Trajectory lost\n");
                        FSM_TIMEOUT(3000);
                        return RC_WAIT;
                        break;
                case EV_TIMEOUT:
                        DBG("Timeout\n");
                        FRET();
                        return RC_PROC;
                        break;
                default:
                        break;
        }
        return RC_WAIT;
}

#define SPEED_RATIO 3

static int go2(struct robo_fsm *fsm, int ret) 
{
        DBG_STATE();
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
        tc.maxv /= SPEED_RATIO;
        tc.maxomega /= SPEED_RATIO;
        tc.maxacc /= SPEED_RATIO;
        robot_trajectory_new(&tc, -180);
        robot_trajectory_add_point(0.5, 0);
        robot_trajectory_add_point(0.5, 0.5);
        robot_trajectory_add_point(0, 0.5);
        robot_trajectory_add_point(0, 0);
        fsm_signal(FSM(MOT), EV_NEW_TRAJECTORY);
        FCALL2(wait_move, go1);
        return RC_WAIT;
}

static int go1(struct robo_fsm *fsm, int ret) 
{
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
        tc.maxv /= SPEED_RATIO;
        tc.maxomega /= SPEED_RATIO;
        tc.maxacc /= SPEED_RATIO;
        DBG_STATE();
        robot_trajectory_new(&tc, 180);
        robot_trajectory_add_point(0, 0.5);
        robot_trajectory_add_point(0.5, 0.5);
        robot_trajectory_add_point(0.5, 0);
        robot_trajectory_add_point(0, 0);
        //robot_trajectory_add_point(1, 1);
        fsm_signal(FSM(MOT), EV_NEW_TRAJECTORY);
        FCALL2(wait_move, go2);
        return RC_WAIT;
}

static int test_move(struct robo_fsm *fsm, int ret) 
{
        DBG_STATE();
        switch (fsm->event) {
                case EV_INIT:
                        FSM_TIMEOUT(500);
                        return RC_WAIT;
                case EV_TIMEOUT:
                        FNEXT(go1);
                        return RC_PROC;
                default:
                        break;
        }
        return RC_WAIT;
}
   
int main() 
{
        /* robot initialization */
        robot_init();

        //ORTEVerbositySetOptions("ALL,5");

        FSM(MAIN)->fnc_act = test_move;
        FSM(MAIN)->debug_events = 1;
        FSM(MOT)->fnc_act = fsm_move_init;
        FSM(MOT)->debug_events = 1;

        robot_start();

//      int i;
//      for (i=-20; i<20; i+=3) {
//              double s = (double)i/100;
//              printf("%g\n", s);
//              send_speed(s, -s);
//              sleep(1);
//      }
        
//      send_speed(0, 0);
    
        robot_wait();
        robot_destroy();

        return 0;
}

---