con_vec drive(situation &s) // This is the robot "driver" function:
{
con_vec result = CON_VEC_EMPTY; // 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 = 0.0; // 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;
}
con_vec drive(situation &s)
{
con_vec result = CON_VEC_EMPTY; // This is what is returned.
double alpha, vc; // components of result
static double lane = -10000; // an absurd value to show not initialized
double bias, speed, width;
if( s.starting )
{
result.fuel_amount = MAX_FUEL; // fuel when starting
}
// 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(lane < -9000) // will be true only once
lane = s.to_lft; // better not to change lanes at the start
// Set "lane" during curves. This robot sets "lane" during curves to
// try to maintain a fixed distance to the inner rail.
// For straightaways, we leave "lane" unchanged until later.
if(s.cur_rad > 0.0) // turning left
lane = DIST_FROM_INSIDE;
else if(s.cur_rad < 0.0) // turning right
lane = width - DIST_FROM_INSIDE;
// 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.
if(s.cur_rad == 0.0)
{
bias = 0.0;
speed = corn_spd(s.nex_rad + DIST_FROM_INSIDE);
}
else
{
speed = corn_spd(s.cur_rad + DIST_FROM_INSIDE);
// See initial paragraphs for discussion of this formula.
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: When nearing end of straight, change "lane" for the turn, also.
if(s.cur_rad == 0.0) // If we are on a straightaway,
{
if(s.to_end > ACCEL_FRACTION * s.cur_len) // if we are far from the end,
{
vc = s.v + 50.0; // pedal to the metal!
}
else // otherwise, adjust speed for the coming turn:
{
if(s.v > 1.02 * speed) // if we're 2% too fast,
vc = .95 * s.v; // brake hard.
else if(s.v < .98 * speed) // if we're 2% too slow,
vc = 1.05 * speed; // accelerate hard.
else // if we are very close to speed,
vc = .5 * (s.v + speed); // approach the speed gently.
// approach the lane you want for the turn:
if(s.nex_rad > 0.0)
lane = DIST_FROM_INSIDE;
else
lane = width - DIST_FROM_INSIDE;
}
}
else // This is when we are in a curve: (seek correct speed)
{
vc = .5 * (s.v + speed)/cos(alpha); // to maintain speed in corner
}
// During the acceleration portion of a straightaway, the lane variable
// is not changed by the code above. Hence the code below changes it a
// little at a time until there is no car dead_ahead. This code here has
// no affect at all in the turns, nor in the braking portion
// of the straight.
if(s.dead_ahead) // Change the lane a little if someone's
if(s.to_lft > s.to_rgt) // in your way.
lane -= DELTA_LANE; // lane must be a static variable
else
lane += DELTA_LANE;
result.vc = vc; result.alpha = alpha;
// 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;
}
