WebSVN - shark - Rev 1655 - /unsupported/branches/unsupported/auto/control.c

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

Subversion Repositories shark

(root)/unsupported/branches/unsupported/auto/control.c @ 1684 - Rev 1655