src/robofsm/test/sharpcalib.cc

   1
   2 #define FSM_MAIN
   3 #include <robot.h>
   4 #include <movehelper.h>
   5 #include <trgen.h>
   6 #include <robomath.h>
   7
   8 FSM_STATE_DECL(measure);
   9 FSM_STATE_DECL(move);
  10
  11
  12 FSM_STATE(init)
  13 {
  14         switch (FSM_EVENT) {
  15                 case EV_ENTRY:
  16 //                      brushes_drives_in();
  17 //                      on_bagr();
  18                         robot_set_est_pos_trans(1, 0.5, DEG2RAD(180));
  19                         FSM_TIMER(1000);
  20                         break;
  21                 case EV_TIMER:
  22                         FSM_TRANSITION(measure);
  23                         break;
  24                 default:
  25                         break;
  26         }
  27 }
  28
  29 int cm = 100;
  30
  31 FSM_STATE(move)
  32 {
  33         switch (FSM_EVENT) {
  34                 case EV_ENTRY:
  35                         if (cm <= 5)
  36                                 robot_exit();
  37                         robot_move_by(0.01, NO_TURN(), NULL);
  38                         break;
  39                 case EV_MOTION_DONE:
  40                         cm--;
  41                         FSM_TRANSITION(measure);
  42                         break;
  43                 default:
  44                         break;
  45         }
  46 }
  47
  48 FSM_STATE(measure)
  49 {
  50         static int count;
  51         FSM_TIMER(50);
  52         switch (FSM_EVENT) {
  53                 case EV_ENTRY:
  54                         count = 0;
  55                 case EV_TIMER:
  56                         printf("%3d %9.3f %9.3f\n", cm, robot.sharps.front_left, robot.sharps.front_right);
  57                         count++;
  58                         if (count >= 20)
  59                                 FSM_TRANSITION(move);
  60                         break;
  61                 default:
  62                         break;
  63         }
  64 }
  65
  66
  67 int main()
  68 {
  69         int rv;
  70
  71         /* robot initialization */
  72         rv = robot_init();
  73         if (rv) {
  74                 printf("robot_init(): terminated with errors.\n");
  75                 exit(1);
  76         }
  77
  78         robot.laser_enabled = false;
  79
  80         robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
  81
  82         /* Start threads and wait */
  83         rv = robot_start();
  84         if (rv) {
  85                 printf("robot_start(): terminated with errors.\n");
  86                 exit(1);
  87         }
  88
  89         robot_wait();
  90
  91         /* clean up */
  92         robot_destroy();
  93
  94         return 0;
  95 }