src/robofsm/eb2008/robot_eb2008.c

   1 /*
   2  * robot_eb2008.c                       08/04/20
   3  *
   4  * Robot's generic initialization and clean up functions.
   5  *
   6  * Copyright: (c) 2008 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #include <map.h>
  12 #include <movehelper_eb2008.h>
  13 #include <pthread.h>
  14 #include <robomath.h>
  15 #include <robot_eb2008.h>
  16 #include <robot_orte.h>
  17 #include <servos_eb2008.h>
  18 #include <signal.h>
  19 #include <sys/time.h>
  20 #include <time.h>
  21 #include <unistd.h>
  22 #include "map_handling.h"
  23 #include <uoled.h>
  24 #include <string.h>
  25
  26 static int thread_priorities[] = {
  27         [FSM_ID_MAIN] = THREAD_PRIO_MAIN,
  28         [FSM_ID_MOTION] = THREAD_PRIO_MOTION,
  29         [FSM_ID_LOC] = THREAD_PRIO_LOC,
  30         [FSM_ID_JOY] = THREAD_PRIO_JOY,
  31         [FSM_ID_DISP] = THREAD_PRIO_DISP,
  32 };
  33
  34 static char *fsm_name[] = {
  35         [FSM_ID_MAIN] = "main",
  36         [FSM_ID_MOTION] = "motion",
  37         [FSM_ID_LOC] = "localization",
  38         [FSM_ID_JOY] = "joystick",
  39         [FSM_ID_DISP] = "display",
  40 };
  41
  42 struct robot_eb2008 robot;
  43
  44 #ifdef CONFIG_LOCK_CHECKING
  45 struct lock_log robot_lock_log;
  46 #endif
  47
  48
  49 static void block_signals()
  50 {
  51         sigset_t sigset;
  52         int i;
  53
  54         sigemptyset(&sigset);
  55         for (i=0; i<7; i++)
  56                 sigaddset(&sigset, SIGRTMIN+i);
  57
  58         pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
  59 }
  60
  61 static void int_handler(int sig)
  62 {
  63         robot_exit();
  64 }
  65
  66 /**
  67  * Initializes the robot.
  68  * Setup fields in robot structure, initializes FSMs and ORTE.
  69  *
  70  * @return 0
  71  */
  72 int robot_init()
  73 {
  74         int i;
  75
  76         /* robot and competition configuration */
  77         robot.mode = ROBO_TESTING;
  78         robot.team_color = BLUE;
  79         robot.beacon_color = BEACON_BLUE;
  80
  81         pthread_mutex_init(&robot.lock, NULL);
  82         pthread_mutex_init(&robot.lock_ref_pos, NULL);
  83         pthread_mutex_init(&robot.lock_est_pos, NULL);
  84         pthread_mutex_init(&robot.lock_meas_angles, NULL);
  85         pthread_mutex_init(&robot.lock_joy_data, NULL);
  86         pthread_mutex_init(&robot.lock_disp, NULL);
  87
  88         /* FSM initialization */
  89         for (i=0; i<FSM_CNT; i++)
  90                 fsm_init(&robot.fsm[i], fsm_name[i]);
  91
  92         /* FIXME: there should be team color initilization */
  93 #ifdef WE_ARE_RED
  94         robot.team_color = RED;
  95 #endif
  96 #ifdef WE_ARE_BLUE
  97         robot.team_color = BLUE;
  98 #endif
  99
 100         if (robot.team_color == RED) {
 101                 printf("We are RED!\n");
 102         } else {
 103                 printf("We are BLUE!\n");
 104         }
 105
 106         robot.map = ShmapInit(1);
 107
 108         signal(SIGINT, int_handler);
 109         block_signals();
 110
 111         /* initial values */
 112         robot.gorte.motion_speed.left = 0;
 113         robot.gorte.motion_speed.right = 0;
 114
 115         robot.gorte.pwr_ctrl.voltage33 = 1;
 116         robot.gorte.pwr_ctrl.voltage50 = 1;
 117         robot.gorte.pwr_ctrl.voltage80 = 1;
 118
 119         robot.orte.servos.brush_left = BRUSH_LEFT_CLOSE;
 120         robot.orte.servos.brush_right = BRUSH_RIGHT_CLOSE;
 121         robot.orte.servos.door_bottom = BOTTOM_DOOR_CLOSE;
 122         robot.orte.servos.door_top = TOP_DOOR_CLOSE;
 123         robot.orte.servos.door_back = BACK_DOOR_CLOSE;
 124
 125         robot.orte.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
 126         robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
 127         robot.orte.drives.bagr = BAGR_DRIVE_OFF;
 128
 129         /* MCL initialization */
 130         robot.laser.width = PLAYGROUND_WIDTH_M; /* in meters */
 131         robot.laser.height = PLAYGROUND_WIDTH_M; /* in meters */
 132         /* the noises */
 133         robot.laser.pred_dnoise = 0.001;
 134         robot.laser.pred_anoise = DEG2RAD(2);
 135         robot.laser.aeval_sigma = DEG2RAD(4);
 136         robot.mcl = mcl_laser_init(&robot.laser, 1000);
 137
 138         robot.fsm[FSM_ID_MOTION].state = &fsm_state_motion_init;
 139         robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
 140
 141         /* Only activate display if it is configured */
 142         robot.sercom = uoled_init(serial_comm);
 143         if (strcmp(robot.sercom->devname, "/dev/null") != 0)
 144                 robot.fsm[FSM_ID_DISP].state = &fsm_state_disp_init;
 145
 146         /* init ORTE domain, create publishers, subscribers, .. */
 147         robot_init_orte();
 148
 149         robot.obstacle_avoidance_enabled = false;
 150
 151         return 0;
 152 }
 153 /**
 154  * Starts the robot threads.
 155  * @todo Don't exit() in case of error.
 156  * @return 0
 157  */
 158 int robot_start()
 159 {
 160         int i;
 161
 162         /* start FSM threads */
 163         for(i=0; i<FSM_CNT; i++) {
 164                 pthread_attr_t attr;
 165                 struct sched_param param;
 166
 167                 pthread_attr_init(&attr);
 168
 169                 param.sched_priority = thread_priorities[i];
 170                 pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
 171                 if (pthread_attr_setschedparam(&attr, &param)) {
 172                         perror("pthread_attr_setschedparam");
 173                         exit(1);
 174                 }
 175
 176
 177                 if (fsm_start(&robot.fsm[i], &attr) != 0) {
 178                         perror("fsm_start");
 179                         exit(1);
 180                 }
 181         }
 182
 183         pthread_attr_t tattr;
 184         struct sched_param param;
 185         pthread_t thr_obstacle_forgeting;
 186         int ret;
 187
 188         // Trajectory follower thread
 189         pthread_attr_init (&tattr);
 190         pthread_attr_getschedparam (&tattr, &param);
 191         param.sched_priority = OBST_FORGETING_PRIO;
 192         ret = pthread_attr_setschedparam (&tattr, &param);
 193         if(ret) {
 194                 perror("robot_start: pthread_attr_setschedparam()");
 195                 exit(1);
 196         }
 197         ret = pthread_create(&thr_obstacle_forgeting, &tattr, thread_obstacle_forgeting, NULL);
 198         if(ret) {
 199                 perror("robot_start: pthread_create");
 200                 exit(1);
 201         }
 202
 203         return 0;
 204 }
 205
 206 /**
 207  * Signals all the robot threads to finish.
 208  * @return 0
 209  */
 210 int robot_exit()
 211 {
 212         int i;
 213
 214         /* stop FSM threads */
 215         for(i=0; i<FSM_CNT; i++) {
 216                 fsm_event e = { EV_EXIT, NULL };
 217                 __fsm_signal(&robot.fsm[i], e, true);
 218         }
 219
 220         open_back_door();
 221         open_bottom_door();
 222         off_bagr();
 223         off_brush_left();
 224         off_brush_right();
 225         robot.gorte.motion_speed.right = 0;
 226         robot.gorte.motion_speed.left = 0;
 227         ORTEPublicationSend(robot.gorte.publication_motion_speed);
 228
 229         eb2008_roboorte_destroy(&robot.orte);
 230         generic_roboorte_destroy(&robot.gorte);
 231
 232         return 0;
 233 }
 234
 235 /**
 236  * Wait for finishing the robot threads after calling robot_exit(). *
 237  *
 238  * @return 0
 239  */
 240 int robot_wait()
 241 {
 242         int i;
 243
 244         /* wait for threads */
 245         for (i=0; i<FSM_CNT; i++) {
 246                 pthread_join(robot.fsm[i].threadid, NULL);
 247         }
 248
 249         return 0;
 250 }
 251
 252 /**
 253  * Stops the robot. All resources alocated by robot_init() or
 254  * robot_start() are dealocated here.
 255  * @return 0
 256  */
 257 int robot_destroy()
 258 {
 259         int i;
 260
 261         for (i=0; i<FSM_CNT; i++)
 262                 fsm_destroy(&robot.fsm[i]);
 263         DBG("robofsm: stop.\n");
 264
 265         return 0;
 266 }