src/robofsm/eb2008/test/odometry.cc

   1 /*
   2  * odometry.cc                  07/08/01
   3  *
   4  * Movement test: used to test the odometry.
   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 <movehelper_eb2008.h>
  13 #include <robot_eb2008.h>
  14 #include <trgen.h>
  15 #include <robomath.h>
  16
  17 FSM_STATE_DECL(rectangle);
  18 FSM_STATE_DECL(turn);
  19 FSM_STATE_DECL(wait);
  20
  21 FSM_STATE(init) {
  22         /* Where we are at the begining? */
  23         robot_set_est_pos_trans(1, 1, DEG2RAD(0));
  24         FSM_TRANSITION(rectangle);
  25 }
  26
  27
  28 void follow_rectangle()
  29 {
  30         static bool backward = false;
  31         struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
  32         /*tc.maxv *= 1.5;*/
  33
  34         /* Allocate new trajectory */
  35         if (!backward)
  36                 robot_trajectory_new(&tc);
  37         else
  38                 robot_trajectory_new_backward(&tc);
  39         backward = !backward;
  40
  41         /* Add random point to the trajectory. */
  42         robot_trajectory_add_point_trans(2, 0.5);
  43         robot_trajectory_add_point_trans(2, 1.5);
  44         robot_trajectory_add_point_trans(1, 1.5);
  45         robot_trajectory_add_final_point_trans(1, 0.5, TURN(DEG2RAD(90)));
  46 }
  47
  48 FSM_STATE(rectangle)
  49 {
  50         switch (FSM_EVENT) {
  51                 case EV_ENTRY:
  52                         break;
  53                 case EV_MOTION_DONE:
  54 //                      FSM_TRANSITION(turn);
  55                         break;
  56                 default:
  57                         break;
  58         }
  59 }
  60
  61 FSM_STATE(turn)
  62 {
  63         switch (FSM_EVENT) {
  64                 case EV_ENTRY:
  65                         robot_trajectory_new(NULL);
  66                         robot_trajectory_add_final_point_trans(1, 0.5,
  67                                                 TURN(DEG2RAD(-90)));
  68                         break;
  69                 case EV_MOTION_DONE:
  70                         FSM_TRANSITION(wait);
  71                         break;
  72                 default:
  73                         break;
  74         }
  75 }
  76
  77
  78 FSM_STATE(wait)
  79 {
  80         FSM_TIMER(1000);
  81         switch (FSM_EVENT) {
  82                 case EV_TIMER:
  83                         FSM_TRANSITION(rectangle);
  84                         break;
  85                 default:
  86                         break;
  87         }
  88 }
  89
  90 int main()
  91 {
  92         /* robot initialization */
  93         robot_init();
  94
  95         FSM(MAIN)->debug_states = 1;
  96         /*FSM(MOVE)->debug_states = 1;*/
  97
  98         robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
  99
 100         /* start threads and wait */
 101         robot_start();
 102         robot_wait();
 103
 104         /* clean up */
 105         robot_destroy();
 106
 107         return 0;
 108 }