src/robofsm/test/turn.cc

   1 /*
   2  * turn.cc                      07/08/01
   3  *
   4  * Movement test: test accurancy of turning at one point.
   5  *
   6  * Copyright: (c) 2007 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #define FSM_MAIN
  12 #include <robot.h>
  13 #include <movehelper.h>
  14 #include <trgen.h>
  15 #include <robomath.h>
  16 #include <string.h>
  17 #include <error.h>
  18
  19 FSM_STATE_DECL(line);
  20 FSM_STATE_DECL(turn);
  21 FSM_STATE_DECL(wait);
  22
  23 FSM_STATE(init) {
  24         /* Where we are at the begining? */
  25         robot_set_est_pos_trans(1, 0.5, DEG2RAD(0));
  26         FSM_TRANSITION(line);
  27 }
  28
  29 void follow_line()
  30 {
  31 }
  32
  33 FSM_STATE(line)
  34 {
  35         switch (FSM_EVENT) {
  36                 case EV_ENTRY: {
  37                         static bool backward = false;
  38                         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
  39 //                      tc.maxv *= 2;
  40 //                      tc.maxacc *= 4;
  41 //                      tc.maxangacc /= 5;
  42 //                      tc.maxomega /= 5;
  43
  44                         /* Allocate new trajectory */
  45                         if (!backward) {
  46                                 robot_move_by(0/*m*/, TURN_CCW(DEG2RAD(180)), &tc);
  47                         } else {
  48                                 robot_move_by(0/*m*/, TURN_CW(DEG2RAD(0)), &tc);
  49                         }
  50                         backward = !backward;
  51                         break;
  52                 }
  53                 case EV_MOTION_DONE:
  54                         FSM_TRANSITION(wait);
  55                         break;
  56                 default:
  57                         DBG_PRINT_EVENT("unhandled event");
  58                         break;
  59         }
  60 }
  61
  62 FSM_STATE(wait)
  63 {
  64         FSM_TIMER(5000);
  65         switch (FSM_EVENT) {
  66                 case EV_TIMER:
  67                         FSM_TRANSITION(line);
  68                         break;
  69                 default:
  70                         break;
  71         }
  72 }
  73
  74 int main()
  75 {
  76         int rv;
  77
  78         rv = robot_init();
  79         if (rv) error(1, errno, "robot_init() returned %d\n", rv);
  80
  81         robot.fsm.main.debug_states = 1;
  82
  83         robot.fsm.main.state = &fsm_state_main_init;
  84
  85         rv = robot_start();
  86         if (rv) error(1, errno, "robot_start() returned %d\n", rv);
  87
  88         robot_destroy();
  89
  90         return 0;
  91 }