Subversion Repositories shark

/*

 * 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);

}