Subversion Repositories shark

Rev

Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*
 * Project: S.Ha.R.K.
 *
 * Coordinators:
 *   Giorgio Buttazzo    <giorgio@sssup.it>
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *
 * Authors     :
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *   Massimiliano Giorgi <massy@gandalf.sssup.it>
 *   Luca Abeni          <luca@gandalf.sssup.it>
 *   (see the web pages for full authors list)
 *
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
 *
 * http://www.sssup.it
 * http://retis.sssup.it
 * http://shark.sssup.it
 */



/**
 ------------
 CVS :        $Id: control.c,v 1.1.1.1 2004-05-24 18:03:39 giacomo Exp $

 File:        $File$
 Revision:    $Revision: 1.1.1.1 $
 Last update: $Date: 2004-05-24 18:03:39 $
 ------------
**/


/*
 * Copyright (C) 2000 Marco Dallera and Marco Fiocca
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */


/*
 *              AUTO
 *
 * Another Unuseful Track simulatOr
 *
 * Authors: Marco Dallera
 *          Marco Fiocca
 *
 */


/* ------------- */
/*  Car control  */
/* ------------- */

#include "include/auto.h"
#include "include/const.h"
#include "include/utils.h"

#define TURN_QUANTUM    2
#define KM     18.0             // pixel
#define HOUR 3600.0             // seconds
#define KPH  (KM/HOUR)          // 0.005 pixel/second
#define HIGHWAY_LIMIT  6
#define MAZE_LIMIT    45
#define CURVE_LIMIT     25      // il limite della curva per cui imposta la traiettoria
#define BORDER_LIMIT  ((1.0 / params.rage) * 2.0)
#define STEERING_LIMIT  3       // limite di sterzata

/* Road state */
#define HIGHWAY         1
#define MIXED           2
#define MAZE            3
#define CENTER     10
#define TOO_LEFT   11
#define TOO_RIGHT  12

/* Opponents relationship */
#define ATTACK                  20
#define DEFENSE                 21
#define SUPERATTACK             22

/* Car parameters */
#define MAX_SPEED       (300 * KPH)
#define MIN_SPEED       (10 * KPH)
#define MIN_ACC         (-0.1)
#define MAX_ACC      (0.02)

extern sem_t grx_mutex;

char track_img[TRACK_WIDTH*TRACK_HEIGHT];
char sprite[18][CAR_WIDTH*CAR_HEIGHT];

/* Display functions */
void put_transimage(int x1, int y1, int x2, int y2, BYTE *img,int car_color, int color);
void draw_car(point_f old_pos, car_status cs, int car_color, int fumo);
void put_trackbox(int x1, int y1, int x2, int y2);
void show_display(int, int, float, float, float, float);
void car_crash(PID sp, PID cp, point_f old_pos, int free);

/* ------------------------------------------------------------------ */

TASK control(int index) {
  int i = 0;                            // general purpose counter
  car_status old_car_status;            // the previous car state
  car_status actual_car_status;         // the actual car state
  road_info actual_road_info;           // the actual road informations
  int road_state;                       // the state of the road: HIGHWAY, MIXED; MAZE
  int car_align;                        // the car alignment with respect to road limits
  int opps_state;                       // the relationship with other cars
  float old_speed = 0;                  // the previous car speed value
  float actual_speed = 0;               // the actual car speed value
  float old_acc = 0;                    // the previous acceleration value
  float acc = 0;                        // the new acceleration
  point polepos;

  // Variabili che dipendono dal tipo di macchina
  float max_speed;                      // maximum car speed
  float min_acc;                        // minimum acceleration
  float max_acc;                        // maximum acceleration

  int killed = 0;                       // flag for killed cars
  int crashed = 0;                      // flag for crashed cars

  /* Timing variables */
  float t = 0;
  float dt = (float)CONTROL_PERIOD/100000.0;  // time quantum
  time ts = int2time(t);
  float crash_t = 0;
  time crash_ts = int2time(crash_t);
  float bestlap = 999999999.9;
  time bestlap_s = int2time(bestlap);
  int bestlap_done = 0;
  int laps = 0;

  /* Display vars */
  point spd_p, acc_p, time_p, drv_p;
  char *txt;

  /* Car data */
  car_params params;    // car data structure

  /* ----------------------------------------------------- */

  txt = (char *)malloc(15*sizeof(char));

  params = cars[index];

  max_speed = params.max_speed * MAX_SPEED;             // maximum car speed
  min_acc   = params.min_acc * MIN_ACC;                 // minimum acceleration
  max_acc   = params.max_acc * MAX_ACC;                 // maximum acceleration

  /* Get track data */
  while (track_list[i].selected == 0)
    i++;

  /* Same pole for all */
  polepos.x = track_list[i].pole_pos.x;
  polepos.y = track_list[i].pole_pos.y;
  actual_car_status.pos.x  = polepos.x;
  actual_car_status.pos.y  = polepos.y;
  actual_car_status.orient = track_list[i].pole_orient;
  set_car_status(actual_car_status, params.car_status_cab);

  /* Initialize old values */
  old_car_status = actual_car_status;
  old_acc = acc;
  old_speed = actual_speed;

  /* POWER IS NOTHING WITHOUT CONTROL */
  while(1) {

    if (crashed) {
      crash_t += dt;
      crash_ts = int2time(crash_t);
      if (crash_ts.sec > 2) {
        car_crash(params.sensor_pid,params.control_pid,
                  old_car_status.pos, 1);
        killed=1;
      }
    }

    else {

      /* Reads road informations */
      actual_road_info = get_road_info(params.road_status_cab);

      /* Defines the state of the road */
      if (fabs(actual_road_info.curve) < HIGHWAY_LIMIT  ||
          actual_road_info.distance > 0.9*TEL_HEIGHT) {
        road_state = HIGHWAY;
      } else if (fabs(actual_road_info.curve) > MAZE_LIMIT ||
                 actual_road_info.distance < 0.1*TEL_HEIGHT) {
        road_state = MAZE;
      } else {
        road_state = MIXED;
      }

      /* Defines the car position whitin road borders */
      if (actual_road_info.left_border < BORDER_LIMIT)
        car_align = TOO_LEFT;
      else if (actual_road_info.right_border < BORDER_LIMIT)
        car_align = TOO_RIGHT;
      else car_align = CENTER;

      /* Sets the power percentage */
      switch (road_state)
        {
        case HIGHWAY:
          acc = max_acc;                // tarella!!!!
          break;
        case MIXED:
          actual_speed = params.rage * (((float)actual_road_info.distance/(float)TEL_HEIGHT) /
                                        (fabs(actual_road_info.curve)/90.0)) * max_speed;
          acc = (actual_speed - old_speed) / dt;
          break;
        case MAZE:
          actual_speed = params.rage * (1.0 - (fabs(actual_road_info.curve)/90.0)) * max_speed;
          acc = (actual_speed - old_speed) / dt;
          break;
        default:
          acc = 0.0;
        }

      /* Can't go off road */
      /* When car is near a border, it steers to come in */
      switch (car_align)
        {
        case TOO_LEFT:
          actual_car_status.orient -= TURN_QUANTUM;
          break;
        case TOO_RIGHT:
          actual_car_status.orient += TURN_QUANTUM;
          break;
        }

      if (road_state != HIGHWAY) {
        // C'e' una curva vicina.
        // Se la curva e' a destra si tiene vicino al bordo di sinistra
        // per impostare la traiettoria migliore e viceversa
        if (actual_road_info.curve < 0 && actual_road_info.left_border > BORDER_LIMIT
            && fabs(actual_road_info.curve) > CURVE_LIMIT)
          actual_car_status.orient += TURN_QUANTUM;
        else if (actual_road_info.curve > 0 && actual_road_info.right_border > BORDER_LIMIT
                 && fabs(actual_road_info.curve) > CURVE_LIMIT)
          actual_car_status.orient -= TURN_QUANTUM;

        // Fa la curva dolcemente
        if (actual_road_info.distance < ((1.0/params.rage) * (TEL_HEIGHT * 0.25))) {
          actual_car_status.orient += actual_road_info.curve * (params.rage/8.0);
                                //actual_car_status.orient += actual_road_info.curve / 4.0;
        }
      }

      /* Defines the state of interactions with other cars */
      if (actual_road_info.opps_number > 0)
        {
          if (road_state == HIGHWAY && actual_road_info.opps_list[0].y > (TEL_HEIGHT/3.0))
            opps_state = ATTACK;
          else if (road_state == MAZE && actual_road_info.opps_list[0].y < (TEL_HEIGHT/3.0))
            opps_state = DEFENSE;
          else if (actual_road_info.opps_list[0].y < (TEL_HEIGHT/6.0) &&
                   actual_road_info.opps_list[0].x != 0 &&
                   (abs(actual_road_info.opps_list[0].x) < CAR_W/2)) {
            // Defends and steers to avoid opponents
            opps_state = DEFENSE;
            acc *= -((TEL_HEIGHT/6.0) / (actual_road_info.opps_list[0].y));
            actual_car_status.orient -= ((actual_road_info.opps_list[0].x) /
                                         abs(actual_road_info.opps_list[0].x)) * TURN_QUANTUM/2;
          }
          else if (actual_road_info.opps_list[0].y < (TEL_HEIGHT/3.0) &&
                   actual_road_info.opps_list[0].x != 0 &&
                   abs(actual_road_info.opps_list[0].x) < CAR_W/2) {
            // Attacks and steers to override the opponent
            opps_state = ATTACK;
            actual_car_status.orient -= ((actual_road_info.opps_list[0].x) /
                                         abs(actual_road_info.opps_list[0].x)) * TURN_QUANTUM;
          }
          else
            opps_state = DEFENSE;

          // Response to opponents positions
          if (opps_state == ATTACK)
            acc = max_acc;
          else if (opps_state == DEFENSE)
            acc *= 0.5;
        }


      /* Checking car position: road/offroad */
      if (actual_road_info.left_border * actual_road_info.right_border < 0)
        acc = 0.5 * min_acc;

      if (actual_road_info.flag != ROAD_OK) {
        switch (actual_road_info.flag) {
        case LEFT_ONLY:
          if (actual_road_info.left_border < 0)
            actual_car_status.orient -= TURN_QUANTUM;
          acc = 0.5 * max_acc;
          break;
        case RIGHT_ONLY:
          if (actual_road_info.right_border < 0)
            actual_car_status.orient += TURN_QUANTUM;
          acc = 0.5 * max_acc;
          break;
        case NO_ROAD:
          crashed = 1;
          car_crash(params.sensor_pid,params.control_pid,
                    old_car_status.pos, 0);
          break;
        }
      }

      /* Checking collisions... */
      if (actual_road_info.collision != NO_COLL) {
        switch(actual_road_info.collision) {
        case COLLISION_LEFT:
          actual_car_status.orient -= TURN_QUANTUM;
          acc *= (0.5 * params.rage);
          break;
        case COLLISION_RIGHT:
          actual_car_status.orient += TURN_QUANTUM;
          acc *= (0.5 * params.rage);
          break;
        }
      }

      /* Checking acceleration range... */
      acc = max(min(max_acc,acc),min_acc);
      actual_speed = old_speed + acc*dt;

      /* Checking speed range... */
      actual_speed = min(max(actual_speed,MIN_SPEED),max_speed);

      /* Checking steering angle ... */
      if (fabs(actual_car_status.orient - old_car_status.orient) >
          STEERING_LIMIT) {
        if ((actual_car_status.orient - old_car_status.orient) > 0)
          actual_car_status.orient = old_car_status.orient + STEERING_LIMIT;
        else
          actual_car_status.orient = old_car_status.orient - STEERING_LIMIT;
      }

      /* Setting new car position... */
      actual_car_status.pos.x += actual_speed * cos(degree_to_rad(actual_car_status.orient));
      actual_car_status.pos.y -= actual_speed * sin(degree_to_rad(actual_car_status.orient));

      /* Checking track limits... */
      if (actual_car_status.pos.x < MIN_CAR_X)
        actual_car_status.pos.x = MIN_CAR_X;
      if (actual_car_status.pos.x > MAX_CAR_X)
        actual_car_status.pos.x = MAX_CAR_X;
      if (actual_car_status.pos.y < MIN_CAR_Y)
        actual_car_status.pos.y = MIN_CAR_Y;
      if (actual_car_status.pos.y > MAX_CAR_Y)
        actual_car_status.pos.y = MAX_CAR_Y;

      /* Sends new status to car_status_cab */
      set_car_status(actual_car_status, params.car_status_cab);

      if (!crashed) {
        /* Updates car */
        if (acc == min_acc)
          draw_car(old_car_status.pos,actual_car_status, params.color, 1);
        else
          draw_car(old_car_status.pos,actual_car_status, params.color, 0);
      }

      /* Sets display points */
      spd_p.x = TRACK_X1 + TRACK_WIDTH + TEL_HEIGHT + TEL_WIDTH + 9;
      spd_p.y = TEL_HEIGHT*index + 1 + index + 1;
      bar_display(spd_p, 70, 4, GREEN, actual_speed, max_speed, LIGHTGRAY, "Spd");

      acc_p.x = spd_p.x;
      acc_p.y = spd_p.y + 24;
      bidir_bar_display(acc_p, 70, 4, GREEN, acc, max(fabs(min_acc), max_acc), LIGHTGRAY, "Acc");

      sem_wait(&grx_mutex);

      /* Displays driver's name */
      drv_p.x = spd_p.x + 80;
      drv_p.y = spd_p.y;
      grx_text(params.driver, drv_p.x, drv_p.y, params.color, BLACK);

      /* Displays timer */
      time_p.x = spd_p.x + 80;
      time_p.y = spd_p.y + 8;
      if( (!bestlap_done) || (bestlap_done && t > 50))
        {
          /* Mins */
          txt = itoa(ts.min, txt);
          grx_text(txt, time_p.x,     time_p.y, WHITE, BLACK);
          grx_text(":", time_p.x + 8, time_p.y, WHITE, BLACK);
          /* Secs */
          txt = itoa(ts.sec, txt);
          if (ts.sec < 10) {
            grx_text("0", time_p.x+16, time_p.y, WHITE, BLACK);
            grx_text(txt, time_p.x+24, time_p.y, WHITE, BLACK);
          }
          else
            grx_text(txt, time_p.x+16, time_p.y, WHITE, BLACK);
          /* Decs */
          grx_text(":", time_p.x+32, time_p.y, WHITE, BLACK);
          txt = itoa(ts.dec, txt);
          grx_text(txt, time_p.x+40, time_p.y, WHITE, BLACK);
        }

      /* Displays the best lap */
      if(bestlap_done == 1)
        {
          time_p.x = spd_p.x + 80;
          time_p.y = spd_p.y + 16;
          /* Mins */
          txt = itoa(bestlap_s.min, txt);
          grx_text(txt, time_p.x,     time_p.y, YELLOW, BLACK);
          grx_text(":", time_p.x + 8, time_p.y, YELLOW, BLACK);
          /* Secs */
          txt = itoa(bestlap_s.sec, txt);
          if (bestlap_s.sec < 10) {
            grx_text("0", time_p.x+16, time_p.y, YELLOW, BLACK);
            grx_text(txt, time_p.x+24, time_p.y, YELLOW, BLACK);
          }
          else
            grx_text(txt, time_p.x+16, time_p.y, YELLOW, BLACK);
          /* Decs */
          grx_text(":", time_p.x+32, time_p.y, YELLOW, BLACK);
          txt = itoa(bestlap_s.dec, txt);
          grx_text(txt, time_p.x+40, time_p.y, YELLOW, BLACK);
        }

      /* Displays lap number */
      time_p.x = spd_p.x + 80;
      time_p.y = spd_p.y + 24;
      grx_text("Lap ", time_p.x, time_p.y, LIGHTGRAY, BLACK);
      txt = itoa(laps, txt);
      grx_text(txt, time_p.x+32, time_p.y, LIGHTGRAY, BLACK);

      sem_post(&grx_mutex);

      /* Control the best lap */
      t += dt;
      ts = int2time(t);

      /* Finish line */
      if ((track_list[i].lap == CLOCK &&
           (actual_car_status.pos.x >= polepos.x) && (old_car_status.pos.x < polepos.x)
           && (actual_car_status.pos.y > polepos.y-20) && (actual_car_status.pos.y < polepos.y+20)) ||
          (track_list[i].lap == ANTICLOCK &&
           (actual_car_status.pos.x <= polepos.x) && (old_car_status.pos.x > polepos.x)
           && (actual_car_status.pos.y > polepos.y-20) && (actual_car_status.pos.y < polepos.y+20))) {
        laps++;         // increases lap number

        bestlap = min(bestlap,t);
        bestlap_s = int2time(bestlap);
        bestlap_done = 1;
        t = 0;
        ts = int2time(t);
      }

      /* Updates old values */
      old_car_status = actual_car_status;
      old_acc = acc;
      old_speed = actual_speed;

    }  // end crashed

    task_endcycle();

    // Esce se e' morto
    if (killed) return 0;
  }
}

/* ------------------------------------------------------------------ */

void car_crash(PID sp, PID cp, point_f old_pos, int free)
{
  point p1, p2;
  p1.x = round(old_pos.x) - CAR_WIDTH/2 - 1;
  p1.y = round(old_pos.y) - CAR_HEIGHT/2 - 1;
  p2.x = p1.x + CAR_WIDTH + 1;
  p2.y = p1.y + CAR_HEIGHT + 1;

  sem_wait(&grx_mutex);
  /* Find the track box to redraw */
  put_trackbox(p1.x,p1.y,p2.x,p2.y);

  if (free == 0) {
    // Draw the explosion
    put_transimage(p1.x+1,p1.y+1,p2.x-1,p2.y-1,&sprite[16][0],0,0);
  }
  else if (free == 1) {
    // Delete the explosion
    put_trackbox(p1.x,p1.y,p2.x,p2.y);
    task_kill(sp);
    task_kill(cp);
  }
  sem_post(&grx_mutex);
}

/* ------------------------------------------------------------------ */

void draw_car(point_f old_pos, car_status cs, int car_color, int fumo)
{
  int index;
  point p1, p2;
  p1.x = round(old_pos.x) - CAR_WIDTH/2 - 1;
  p1.y = round(old_pos.y) - CAR_HEIGHT/2 - 1;
  p2.x = p1.x + CAR_WIDTH + 1;
  p2.y = p1.y + CAR_HEIGHT + 1;

  sem_wait(&grx_mutex);
  /* Find the track box to redraw */
  put_trackbox(p1.x,p1.y,p2.x,p2.y);


  p1.x = round(cs.pos.x) + (CAR_WIDTH/2  * cos(degree_to_rad(cs.orient)));
  p1.y = round(cs.pos.y) - (CAR_HEIGHT/2 * sin(degree_to_rad(cs.orient)));
  p2.x = round(cs.pos.x) - (CAR_WIDTH/2  * cos(degree_to_rad(cs.orient)));
  p2.y = round(cs.pos.y) + (CAR_HEIGHT/2 * sin(degree_to_rad(cs.orient)));

  index = ((((int)cs.orient%360)+360)%360);
  index = (int)(index + 11.25)%360;
  index /= 22.5;

  put_transimage(round(cs.pos.x) - CAR_WIDTH/2 + 1,round(cs.pos.y) - CAR_HEIGHT/2 + 1,
                 round(cs.pos.x) + CAR_WIDTH/2 ,round(cs.pos.y) + CAR_HEIGHT/2,
                 &sprite[index][0], car_color, 0);
  if (fumo)
    put_transimage(round(cs.pos.x) - CAR_WIDTH/2 + 1,round(cs.pos.y) - CAR_HEIGHT/2 + 1,
                   round(cs.pos.x) + CAR_WIDTH/2 ,round(cs.pos.y) + CAR_HEIGHT/2,
                   &sprite[17][0], car_color, 0);
  sem_post(&grx_mutex);
}

/* ------------------------------------------------------------------ */

/* Copy a box of track_image to screen */
void put_trackbox(int x1, int y1, int x2, int y2)
{
  int y;
  int tx1,ty1,tx2,ty2;
  BYTE *addr;

  tx1 = x1-TRACK_X1+1;
  ty1 = y1-TRACK_Y1;
  tx2 = x2-TRACK_X1+1;
  ty2 = y2-TRACK_Y1;

  if (tx1 < 0)  tx1 = 0;
  if (ty1 < 0)  ty1 = 0;
  if (tx2 > TRACK_WIDTH-1)      tx2 = TRACK_WIDTH-1;
  if (ty2 > TRACK_HEIGHT-1)     ty2 = TRACK_HEIGHT-1;

  tx1 = max(tx1,0);
  ty1 = max(ty1,0);
  tx2 = min(tx2,TRACK_WIDTH-1);
  ty2 = min(ty2,TRACK_HEIGHT-1);

  addr = &track_img[0] + tx1 + TRACK_WIDTH * ty1;

  x1 = max(0,x1);
  y1 = max(0,y1);

  for (y = y1; y <= y2; y++) {
    grx_putimage(x1+1, y, x2, y, addr);
    addr += TRACK_WIDTH;
  }
}

/* ------------------------------------------------------------------ */

void put_transimage(int x1, int y1, int x2, int y2, BYTE *img, int car_color,
                    int color)
{
  int x,y;
  BYTE *addr;
  addr = img;


  for (y = y1; y < y2; y++) {
    for ( x = x1; x < x2; x++, addr++) {
      if (*addr != color) {
        if (*addr == 26)
          grx_plot(x,y,car_color);
        else
          grx_plot(x,y,*addr);

      }
    }
    addr++;
  }
}

/* ------------------------------------------------------------------ */

void show_display(int rs, int ca, float speed, float max_speed,
                  float acc, float max_acc)
{
  point hw, mx, mz;     // three road states
  point tl, ct, tr;     // three car alignments
  point sp1, sp2;               // speed display coordinates
  point ap1, ap2;               // acceleration display coordinates
  int s_amp = 0;
  int a_amp = 0;

  /* Leds coordinates */
  hw.x = 10;
  hw.y = SCREEN_HEIGHT-50;
  mx.x = 25;
  mx.y = SCREEN_HEIGHT-50;
  mz.x = 40;
  mz.y = SCREEN_HEIGHT-50;
  tl.x = 10;
  tl.y = SCREEN_HEIGHT-35;
  ct.x = 25;
  ct.y = SCREEN_HEIGHT-35;
  tr.x = 40;
  tr.y = SCREEN_HEIGHT-35;
  sp1.x = 10;
  sp1.y = SCREEN_HEIGHT-75;
  sp2.x = 110;
  sp2.y = SCREEN_HEIGHT-65;
  ap1.x = 10;
  ap1.y = SCREEN_HEIGHT-90;
  ap2.x = 110;
  ap2.y = SCREEN_HEIGHT-80;

  sem_wait(&grx_mutex);

  /* Writes out controller titles */
  grx_text("Road",  mz.x+15, mz.y, YELLOW, BLACK);
  grx_text("Curve", tr.x+15, tr.y, YELLOW, BLACK);
  switch (rs)
    {
    case HIGHWAY:
      grx_box(hw.x, hw.y, hw.x+10, hw.y+10, GREEN);
      grx_box(mx.x, mx.y, mx.x+10, mx.y+10, BLACK);
      grx_box(mz.x, mz.y, mz.x+10, mz.y+10, BLACK);
      break;
    case MIXED:
      grx_box(hw.x, hw.y, hw.x+10, hw.y+10, BLACK);
      grx_box(mx.x, mx.y, mx.x+10, mx.y+10, YELLOW);
      grx_box(mz.x, mz.y, mz.x+10, mz.y+10, BLACK);
      break;
    case MAZE:
      grx_box(hw.x, hw.y, hw.x+10, hw.y+10, BLACK);
      grx_box(mx.x, mx.y, mx.x+10, mx.y+10, BLACK);
      grx_box(mz.x, mz.y, mz.x+10, mz.y+10, RED);
      break;
    }

  switch (ca)
    {
    case TOO_LEFT:
      grx_box(tl.x, tl.y, tl.x+10, tl.y+10, RED);
      grx_box(ct.x, ct.y, ct.x+10, ct.y+10, BLACK);
      grx_box(tr.x, tr.y, tr.x+10, tr.y+10, BLACK);
      break;
    case TOO_RIGHT:
      grx_box(tl.x, tl.y, tl.x+10, tl.y+10, BLACK);
      grx_box(ct.x, ct.y, ct.x+10, ct.y+10, BLACK);
      grx_box(tr.x, tr.y, tr.x+10, tr.y+10, BLUE);
      break;
    default:
      grx_box(tl.x, tl.y, tl.x+10, tl.y+10, BLACK);
      grx_box(ct.x, ct.y, ct.x+10, ct.y+10, WHITE);
      grx_box(tr.x, tr.y, tr.x+10, tr.y+10, BLACK);
    }

  /* Draws speed display */
  grx_text("Speed", sp1.x+120, sp1.y, YELLOW, BLACK);
  grx_rect(sp1.x-1, sp1.y-1, sp2.x+1, sp2.y+1, GREEN);
  /* Updates speed display */
  grx_box(sp1.x, sp1.y, sp2.x, sp2.y, BLACK);
  s_amp = round( (speed * (float)(sp2.x-sp1.x)) / max_speed );
  grx_box(sp1.x, sp1.y, sp1.x+s_amp, sp2.y, LIGHTGRAY);

        /* Draws acceleration display */
  grx_text("Acceleration", ap1.x+120, ap1.y, YELLOW, BLACK);
  grx_rect(ap1.x-1, ap1.y-1, ap2.x+1, ap2.y+1, GREEN);
  /* Updates acceleration display */
  grx_box(ap1.x, ap1.y, ap2.x, ap2.y, BLACK);
  a_amp = round( (float)acc * (float)((ap2.x-ap1.x)/2) / (float)max_acc );
  if (a_amp >= 0)
    grx_box((ap1.x+ap2.x)/2, ap1.y, ((ap1.x+ap2.x)/2)+a_amp, ap2.y, LIGHTGRAY);
  else
    grx_box((ap1.x+ap2.x)/2+a_amp, ap1.y, ((ap1.x+ap2.x)/2), ap2.y, LIGHTGRAY);
  grx_line((ap1.x+ap2.x)/2, ap1.y-2, (ap1.x+ap2.x)/2, ap2.y+2, WHITE);
  sem_post(&grx_mutex);
}