src/robofsm/eb2008/test/sharpcalib.cc

   1
   2 #define FSM_MAIN
   3 #include <robot_eb2008.h>
   4 #include <movehelper_eb2008.h>
   5 #include <trgen.h>
   6 #include <robomath.h>
   7
   8 FSM_STATE(init)
   9 {
  10         static int cm = 100;
  11         switch (FSM_EVENT) {
  12                 case EV_ENTRY:
  13                         robot_set_est_pos_trans(1, 0.5, DEG2RAD(180));
  14                         FSM_TIMER(1000);
  15                         break;
  16                 case EV_TIMER:
  17                 case EV_MOTION_DONE:
  18                         printf("%3d cm: sharp left: %5.3f right: %5.3f\n", cm, robot.sharps.front_left, robot.sharps.front_right);
  19                         robot_move_by(0.01, NO_TURN(), NULL);
  20                         cm--;
  21                         if (cm == 0)
  22                                 robot_exit();
  23                         break;
  24                 default:
  25                         break;
  26         }
  27 }
  28
  29
  30 int main()
  31 {
  32         /* robot initialization */
  33         robot_init();
  34
  35         robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
  36
  37         /* Start threads and wait */
  38         robot_start();
  39         robot_wait();
  40
  41         /* clean up */
  42         robot_destroy();
  43
  44         return 0;
  45 }