/*
TUTORIAL4.CPP (JACKBOT.CPP )
History
- yet another RARS 'bot, by M. Timin, June
'95
- Object Oriented Robot + Pitting
January 2001
This file is the seventh in our tutorial series.
It is also a working robot. The robot function name,
and its displayed name, is "JackBot". This
file may be compiled and linked just as any other RARS
driver. The main purpose of JackBot is to
present and explain some code that uses the "nearby" structure
for collision avoidance and passing. It has
been tested only on OVAL2.TRK, and it runs very well there.
However, this code is not symetrical with
respect to left and right, and will not perform well on tracks
with right turns.
This code is largely based on the first six
tutorials. I will try to explain where and why it differs
in the comments of the code.
The file CNTRL0.CPP is still useful as a reference
for some details of robot code that are not explained
below.
*/
//--------------------------------------------------------------------------
//
I N C L U D E
//--------------------------------------------------------------------------
#include <stdlib.h>
#include <math.h>
#include "car.h"
//--------------------------------------------------------------------------
//
D E F I N E S
//--------------------------------------------------------------------------
// parameters to tinker with:
// accelerations are in feet/second per
second.
// slips are in feet/second
// distances are in feet
const double CORN_MYU = 1.00;
// lateral g's expected when cornering
const double BRAKE_ACCEL = -33.0;
// acceleration when braking on straight
const double BRAKE_SLIP = 6.5;
// tire slip when braking
const double BRK_CRV_ACC = -27.0;
// acceleration when braking in curve
const double BRK_CRV_SLIP = 3.5;
// tire slip for braking in curve
const double MARGIN = 10.0;
// target distance from curve's inner rail
const double MARG2 = MARGIN+10.0;
// target for entering the curve
const double ENT_SLOPE = .33;
// slope of entrance path before the curve
const double STEER_GAIN = 1.0;
// gain of steering servo
const double DAMP_GAIN = 1.2;
// damping of steering servo
const double BIG_SLIP = 9.0;
// affects the bias of steering servo
const double CURVE_END = 4.0;
// when you are near end of curve, widths
const double TOO_FAST = 1.02;
// a ratio to determine if speed is OK in curve
const double DELTA_LANE = 2.5;
// if collision predicted, change lane by this
//--------------------------------------------------------------------------
//
Class Tutorial3
//--------------------------------------------------------------------------
class Tutorial4 : public Driver
{
public:
Tutorial4::Tutorial4()
{
m_sName = "Tuto 4";
m_sAuthor = "Mitchell
Timin";
m_iNoseColor = oBLUE;
m_iTailColor = oBLUE;
m_sBitmapName2D = "car_blue_blue";
}
double corn_spd(double radius)
// returns maximum cornering speed, fps
{
// MUST NEVER
CALL THIS ROUTINE WITH ZERO OR NEGATIVE ARGUMENT!
return sqrt(radius
* 32.2 * CORN_MYU); // compute the speed
}
// Calculates the critical distance
necessary to bring a car from speed
// v0 to speed v1 when the braking
acceleration is "a", ft per sec^2.
// Speeds are in fps. ("a"
should be negative)
double CritDist(double v0, double
v1, double a)
{
double dv;
dv = v1 - v0;
if(dv > 0.0)
// this saves having such a test in the caller
return(0.0);
return (v0 + .5 * dv)
* dv / a;
}
con_vec drive(situation &s)
// This is the robot "driver" function:
{
con_vec result;
// This is what is returned.
double alpha, vc;
// components of result
double bias;
// added to servo's alpha result when entering curve
double speed;
// target speed for curve (next curve if straightaway)
double speed_next;
// target speed for next curve when in a curve, fps.
double width;
// track width, feet
double to_end;
// distance to end of present segment in feet.
static double lane;
// target distance from left wall, feet
static double lane0;
// value of lane during early part of straightaway
static int rad_was
= 0; // 0, 1, or -1 to indicate type of previous segment
static double lane_inc
= 0.0; // an adjustment to "lane", for passing
// service routine in
the host software to handle getting unstuck from
// from crashes and
pileups:
if(stuck(s.backward,
s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
return
result;
width = s.to_lft + s.to_rgt; // compute width of track
// This is a little
trick so that the car will not try to change lanes
// during the "dragout"
at the start of the race. We set "lane" to
// whatever position
we have been placed by the host.
if(s.starting)
// will be true only once
lane =
lane0 = s.to_lft; // better not to change lanes during
"dragout"
// Set "lane" during
curves. This robot sets "lane" during curves to
// try to maintain
a small fixed distance to the inner rail.
if(s.cur_rad > 0.0)
// turning left
{
lane =
MARGIN;
rad_was
= 1;
// set this appropriate to curve.
}
else if(s.cur_rad <
0.0) // turning right
{
lane =
width - MARGIN;
rad_was
= -1;
// set this appropriate to curve.
}
else
// straightaway:
{
// We will
let the car go down the straigtaway in whatever "lane" it
// comes
out of the turn.
if(rad_was)
// If we just came out of a turn, then:
{
lane = s.to_lft; // set
"lane" where we are now.
if(lane < .5 * width) // but maybe push it a little
more to right?
lane += MARG2; //
(add MARG2 if we were to left of center)
lane0 = lane;
// save a copy of the new "lane" value.
rad_was = 0;
// set this appropriate to straightaway.
}
// This
is for the transition from straight to left turn. If we are
// in a
transition zone near the end of the straight, then set lane to
// a linear
function of s.to_end. During this zone, "lane" will change
// from
"lane0" upon entering the zone to MARG2 upon reaching the end
// of the
straightaway. ENT_SLOPE is the change in lane per change in
// s.to_end.
if(s.to_end
< (lane0 - MARG2) / ENT_SLOPE)
lane = MARG2 + ENT_SLOPE * s.to_end;
}
// set the bias:
// Bias is an additive
term in the steering servo, so that the servo
// doesn't have to
"hunt" much for the correct alpha value. It is an
// estimate of the
alpha value that would be found by the servo if there
// was plenty of settling
time. It is zero for straightaways.
// Also, for convenience,
we call the corn_spd() function here. On
// the straightaway,
we call it to find out the correct speed for the
// corner ahead, using
s.nex_rad for the radius. In the curve we of
// course use the radius
of the curve we are in. But also, we call it
// for the next segment,
to find out our target speed for the end of
// the current segment,
which we call speed_next.
if(s.cur_rad == 0.0)
{
bias =
0.0;
if(s.nex_rad
> 0.0)
speed = corn_spd(s.nex_rad + MARGIN);
else if(s.nex_rad
< 0.0)
speed = corn_spd(-s.nex_rad + MARGIN);
else
speed = 250.0; // This should not execute, for a normal track file
}
else
// we are in a curve:
{
if(s.nex_rad
== 0.0)
speed_next = 250.0;
else
speed_next = corn_spd(fabs(s.nex_rad) + MARGIN);
speed =
corn_spd(fabs(s.cur_rad) + MARGIN + fabs(lane_inc));
bias =
(s.v*s.v/(speed*speed)) * atan(BIG_SLIP / speed);
if(s.cur_rad
< 0.0) // bias must be negative for right turn
bias = -bias;
}
// set alpha:
(This line is the complete steering servo.)
alpha = STEER_GAIN
* (s.to_lft - lane)/width - DAMP_GAIN * s.vn/s.v + bias;
// set vc:
if(s.cur_rad == 0.0)
// If we are on a straightaway,
{
// if we are far from the end,
if(s.to_end
> CritDist(s.v, speed, BRAKE_ACCEL))
vc = s.v + 50.0;
// pedal to the metal!
else
// otherwise, adjust speed for the coming turn:
{
if(s.v > TOO_FAST * speed)
// if we're a little too fast,
vc = s.v - BRAKE_SLIP;
// brake hard.
else if(s.v < speed/TOO_FAST)
// if we're a little too slow,
vc = 1.1 * speed;
// accelerate hard.
else
// if we are very close to speed,
vc = .5 * (s.v + speed); // approach the speed gently.
}
}
else
// This is when we are in a curve: (seek correct speed)
{
// calculate
distance to end of curve:
if(s.cur_rad
> 0.0)
to_end = s.to_end * (s.cur_rad + MARGIN);
else
to_end = -s.to_end * (s.cur_rad - MARGIN);
// compute
required braking distance and compare:
// This
is to slow us down for then next curve, if necessary:
if(to_end
<= CritDist(s.v, speed_next, BRK_CRV_ACC))
vc = s.v - BRK_CRV_SLIP;
// but
if there is a straight, or a faster curve next, then
// we may
want to accelerate:
else if(to_end/width
< CURVE_END && speed_next > speed)
vc = .5 * (s.v + speed_next)/cos(alpha);
else
// normally, just calculate vc to maintain speed in corner
vc = .5 * (s.v + speed)/cos(alpha);
}
// Passing and anti-collision
code:
// This code first
tries to predict a collision; if no collision is
// predicted, it does
nothing. Collision prediction is approximate, and
// is based on linear
extrapolation. This can work because it is
// repeated eighteen
times per second of simulated time.
// If a collision is
predicted, then it gradually changes the
// lane_inc static
variable which changes alpha.
// The hope is to steer
around the car. When no collision is
// predicted then lane_inc
is gradually brought back to zero.
// If a crash is about
to occur, medium hard braking occurs.
double x, y, vx, vy,
dot, vsqr, c_time, y_close, x_close;
int kount;
// counts cars that are in danger of collision
kount = 0;
for(int i=0;i<3;i++)
if (s.nearby[i].who<16) // if there is a close car
{
y=s.nearby[i].rel_y;
// get forward distance (center-to-center)
x=s.nearby[i].rel_x;
// get right distance
vx=s.nearby[i].rel_xdot;
// get forward relative speed
vy=s.nearby[i].rel_ydot;
// get lateral relative speed
// if the
cars are getting closer, then the dot product of the relative
// position
and velocity vectors will be negative.
dot = x
* vx + y * vy; // compute dot product of vectors
if(dot
> -0.1)
// no action if car is not approaching.
continue;
vsqr =
vx*vx + vy*vy; // compute relative speed
squared
// Time
to closest approach is dot product divided by speed squared:
c_time
= -dot / vsqr; // compute time to closest approach
if(c_time
> 3.0) // ignore
if over three seconds
continue;
/* If the
execution gets this far, it means that there is a car
ahead of
you, and getting closer, and less than 3.0 seconds
away.
Evaluate the situation more carefully to decide if
evasive
action is warranted: */
x_close
= x + c_time * vx; // x coord at closest
approach
y_close
= y + c_time * vy; // y coord at closest
approach
/*
Due to the length of the cars, a collision will occur if
x changes sign while y is less than CARLEN. This
can happen before the center-to-center distance reaches its
point of closest approach. */
// check
if collision would occur prior to closest approach
// if so,
reduce c_time, re-calculate x_close and y_close:
if(x_close
* x < 0.0 && y < 1.1 * CARLEN)
{
c_time = (fabs(x) - CARWID) / fabs(vx);
x_close = x + c_time * vx; // x coord at
closest approach
y_close = y + c_time * vy; // y coord at
closest approach
}
// Will
it be a hit or a miss?
if(fabs(x_close)
> 2 * CARWID || fabs(y_close) > 1.25 * CARLEN)
continue;
// this when a miss is predicted
// If we
get here there is a collision predicted
++kount;
// This counts how many cars are in the way.
if(kount
> 1 || c_time < .85) // if more than one problem car, or if
vc = s.v - BRK_CRV_SLIP; // car within .85 sec of collision,
brake!
// steer
to avoid the other car:
// if there
is room, we try to pass with least x deviation
if(s.cur_rad
> 0.0)
if(x_close < 0.0 || s.to_lft < MARGIN) // avoid scraping the
inside
lane_inc += DELTA_LANE;
else
lane_inc -= DELTA_LANE;
else if(s.cur_rad
< 0.0)
if(x_close > 0.0 || s.to_rgt < MARGIN)
lane_inc -= DELTA_LANE;
else
lane_inc += DELTA_LANE;
else if(x_close
< 0.0) // on straights, pass with least
x deviation
lane_inc += DELTA_LANE;
else
lane_inc -= DELTA_LANE;
if(lane_inc
> .25 * width) // limit the lane alteration to 1/4 width:
lane_inc = .25 * width;
else if(lane_inc
< -.25 * width)
lane_inc = -.25 * width;
}
// Here we gradually
reduce lane_inc to zero if no collision is predicted:
if(!kount)
if(lane_inc
> .1)
lane_inc -= .5*DELTA_LANE;
else if(lane_inc
< -.001)
lane_inc += .5*DELTA_LANE;
// lane_inc represents
an adjustment to the lane variable. This is
// accomplished by
changing alpha an amount equal to that which the
// steering servo would
have done had lane actually been changed.
result.vc = vc;
result.alpha = alpha - STEER_GAIN * lane_inc / width;
// Pit: if the fuel
is too low
// Fuel: full
// Damage: repair
all
if( s.fuel<10.0
)
{
result.request_pit
= 1;
result.repair_amount
= s.damage;
result.fuel_amount
= MAX_FUEL;
}
return result;
}
};
Driver * getTutorial4Instance()
{
return new Tutorial4();
}