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 void fill_in_known_areas_in_map()
  67 {
  68         /* Do not plan path close to edges */
  69 /*      ShmapSetRectangleFlag(0.0, 0.0, 0.199, 2.1, MAP_FLAG_WALL, 0); */
  70 /*      ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0); */
  71 /*      ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0); */
  72 /*      ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); */
  73
  74
  75         ShmapSetRectangleFlag(0, 1.25, 0.2, 1.45, MAP_FLAG_IGNORE_OBST, 0); /* Left white vert. dispenser */
  76         ShmapSetRectangleFlag(2.8, 1.25, 3, 1.45, MAP_FLAG_IGNORE_OBST, 0); /* Right white vert. dispenser */
  77
  78         ShmapSetRectangleFlag(0.65, 1.9, 0.85, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* Blue vert. dispenser */
  79         ShmapSetRectangleFlag(2.30, 1.9, 2.55, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* Red vert. dispenser */
  80
  81         /* Ignore other obstacles at edges */
  82         ShmapSetRectangleFlag(0.0, 0.0, 0.09, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* left */
  83         ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.09, MAP_FLAG_IGNORE_OBST, 0); /* bottom */
  84         ShmapSetRectangleFlag(0.0, 2.01, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* top */
  85         ShmapSetRectangleFlag(2.91, 0.0, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* right */
  86 }
  87
  88 /**
  89  * Initializes the robot.
  90  * Setup fields in robot structure, initializes FSMs and ORTE.
  91  *
  92  * @return 0
  93  */
  94 int robot_init()
  95 {
  96         int i;
  97
  98         /* robot and competition configuration */
  99         robot.mode = ROBO_TESTING;
 100         robot.team_color = BLUE;
 101         robot.beacon_color = BEACON_BLUE;
 102
 103         pthread_mutex_init(&robot.lock, NULL);
 104         pthread_mutex_init(&robot.lock_ref_pos, NULL);
 105         pthread_mutex_init(&robot.lock_est_pos, NULL);
 106         pthread_mutex_init(&robot.lock_meas_angles, NULL);
 107         pthread_mutex_init(&robot.lock_joy_data, NULL);
 108         pthread_mutex_init(&robot.lock_disp, NULL);
 109
 110         /* FSM initialization */
 111         for (i=0; i<FSM_CNT; i++)
 112                 fsm_init(&robot.fsm[i], fsm_name[i]);
 113
 114         /* FIXME: there should be team color initilization */
 115 #ifdef WE_ARE_RED
 116         robot.team_color = RED;
 117 #endif
 118 #ifdef WE_ARE_BLUE
 119         robot.team_color = BLUE;
 120 #endif
 121
 122         if (robot.team_color == RED) {
 123                 printf("We are RED!\n");
 124         } else {
 125                 printf("We are BLUE!\n");
 126         }
 127 #ifdef CONFIG_OPEN_LOOP
 128         printf("OPEN_LOOP enabled\n");
 129 #endif
 130
 131         robot.map = ShmapInit(1);
 132         fill_in_known_areas_in_map();
 133
 134         signal(SIGINT, int_handler);
 135         block_signals();
 136
 137         /* initial values */
 138         robot.gorte.motion_speed.left = 0;
 139         robot.gorte.motion_speed.right = 0;
 140
 141         robot.gorte.pwr_ctrl.voltage33 = 1;
 142         robot.gorte.pwr_ctrl.voltage50 = 1;
 143         robot.gorte.pwr_ctrl.voltage80 = 1;
 144
 145         robot.orte.servos.brush_left = BRUSH_LEFT_CLOSE;
 146         robot.orte.servos.brush_right = BRUSH_RIGHT_CLOSE;
 147         robot.orte.servos.door_bottom = BOTTOM_DOOR_CLOSE;
 148         robot.orte.servos.door_top = TOP_DOOR_CLOSE;
 149         robot.orte.servos.door_back = BACK_DOOR_CLOSE;
 150
 151         robot.orte.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
 152         robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
 153         robot.orte.drives.bagr = BAGR_DRIVE_OFF;
 154
 155         /* MCL initialization */
 156         robot.laser.width = PLAYGROUND_WIDTH_M; /* in meters */
 157         robot.laser.height = PLAYGROUND_WIDTH_M; /* in meters */
 158         /* the noises */
 159         robot.laser.pred_dnoise = 0.001;
 160         robot.laser.pred_anoise = DEG2RAD(2);
 161         robot.laser.aeval_sigma = DEG2RAD(4);
 162         robot.mcl = mcl_laser_init(&robot.laser, 1000);
 163
 164         robot.fsm[FSM_ID_MOTION].state = &fsm_state_motion_init;
 165         robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
 166
 167         /* Only activate display if it is configured */
 168         robot.sercom = uoled_init(serial_comm);
 169         if (strcmp(robot.sercom->devname, "/dev/null") != 0)
 170                 robot.fsm[FSM_ID_DISP].state = &fsm_state_disp_init;
 171
 172         robot.obstacle_avoidance_enabled = true;
 173         robot.laser_enabled = true;
 174
 175         /* init ORTE domain, create publishers, subscribers, .. */
 176         robot_init_orte();
 177
 178
 179         return 0;
 180 }
 181 /**
 182  * Starts the robot threads.
 183  * @todo Don't exit() in case of error.
 184  * @return 0
 185  */
 186 int robot_start()
 187 {
 188         int i;
 189
 190         /* start FSM threads */
 191         for(i=0; i<FSM_CNT; i++) {
 192                 pthread_attr_t attr;
 193                 struct sched_param param;
 194
 195                 pthread_attr_init(&attr);
 196
 197                 param.sched_priority = thread_priorities[i];
 198                 pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
 199                 if (pthread_attr_setschedparam(&attr, &param)) {
 200                         perror("pthread_attr_setschedparam");
 201                         exit(1);
 202                 }
 203
 204
 205                 if (fsm_start(&robot.fsm[i], &attr) != 0) {
 206                         perror("fsm_start");
 207                         exit(1);
 208                 }
 209         }
 210
 211         pthread_attr_t tattr;
 212         struct sched_param param;
 213         pthread_t thr_obstacle_forgeting;
 214         int ret;
 215
 216         // Trajectory follower thread
 217         pthread_attr_init (&tattr);
 218         pthread_attr_getschedparam (&tattr, &param);
 219         param.sched_priority = OBST_FORGETING_PRIO;
 220         ret = pthread_attr_setschedparam (&tattr, &param);
 221         if(ret) {
 222                 perror("robot_start: pthread_attr_setschedparam()");
 223                 exit(1);
 224         }
 225         ret = pthread_create(&thr_obstacle_forgeting, &tattr, thread_obstacle_forgeting, NULL);
 226         if(ret) {
 227                 perror("robot_start: pthread_create");
 228                 exit(1);
 229         }
 230
 231         return 0;
 232 }
 233
 234 /**
 235  * Signals all the robot threads to finish.
 236  * @return 0
 237  */
 238 int robot_exit()
 239 {
 240         int i;
 241
 242         /* stop FSM threads */
 243         for(i=0; i<FSM_CNT; i++) {
 244                 fsm_event e = { EV_EXIT, NULL };
 245                 __fsm_signal(&robot.fsm[i], e, true);
 246         }
 247
 248         robot.orte.laser_cmd.speed = LASER_DRIVE_OFF;
 249         ORTEPublicationSend(robot.orte.publication_laser_cmd);
 250         ORTEPublicationSend(robot.orte.publication_laser_cmd);
 251
 252         open_back_door();
 253         open_bottom_door();
 254         off_bagr();
 255         off_brush_left();
 256         off_brush_right();
 257         robot.gorte.motion_speed.right = 0;
 258         robot.gorte.motion_speed.left = 0;
 259         ORTEPublicationSend(robot.gorte.publication_motion_speed);
 260
 261         eb2008_roboorte_destroy(&robot.orte);
 262         generic_roboorte_destroy(&robot.gorte);
 263
 264         return 0;
 265 }
 266
 267 /**
 268  * Wait for finishing the robot threads after calling robot_exit(). *
 269  *
 270  * @return 0
 271  */
 272 int robot_wait()
 273 {
 274         int i;
 275
 276         /* wait for threads */
 277         for (i=0; i<FSM_CNT; i++) {
 278                 pthread_join(robot.fsm[i].threadid, NULL);
 279         }
 280
 281         return 0;
 282 }
 283
 284 /**
 285  * Stops the robot. All resources alocated by robot_init() or
 286  * robot_start() are dealocated here.
 287  * @return 0
 288  */
 289 int robot_destroy()
 290 {
 291         int i;
 292
 293         for (i=0; i<FSM_CNT; i++)
 294                 fsm_destroy(&robot.fsm[i]);
 295         DBG("robofsm: stop.\n");
 296
 297         return 0;
 298 }