/* pitstop callback */
static int pitcmd(int index, tCarElt* car, tSituation *s)
{
MyCar* myc = mycar[index-1];
Pathfinder* mpf = myc->getPathfinderPtr();
car->_pitFuel = MAX(MIN((car->_remainingLaps+1.0)*myc->fuelperlap - car->_fuel, car->_tank - car->_fuel), 0.0);
myc->lastpitfuel = MAX(car->_pitFuel, 0.0);
car->_pitRepair = car->_dammage;
mpf->setPitStop(false, myc->getCurrentSegId());
myc->loadBehaviour(myc->START);
myc->startmode = true;
myc->trtime = 0.0;
return ROB_PIT_IM; /* return immediately */
}
// Pitstop callback.
static int pitcmd(int index, tCarElt* car, tSituation *s)
{
MyCar* myc = mycar[index-1];
Pathfinder* mpf = myc->getPathfinderPtr();
float fullracedist = (myTrackDesc->getTorcsTrack()->length*s->_totLaps);
float remaininglaps = (fullracedist - car->_distRaced)/myTrackDesc->getTorcsTrack()->length;
car->_pitFuel = MAX(MIN(myc->fuelperlap*(remaininglaps+myc->FUEL_SAFETY_MARGIN) - car->_fuel, car->_tank - car->_fuel), 0.0);
myc->lastpitfuel = MAX(car->_pitFuel, 0.0);
car->_pitRepair = car->_dammage;
mpf->setPitStop(false, myc->getCurrentSegId());
myc->loadBehaviour(myc->START);
myc->startmode = true;
myc->trtime = 0.0;
return ROB_PIT_IM; // Return immediately.
}
// Controls the car.
static void drive(int index, tCarElt* car, tSituation *situation)
{
tdble angle;
tdble brake;
tdble b1; // Brake value in case we are to fast HERE and NOW.
tdble b2; // Brake value for some brake point in front of us.
tdble b3; // Brake value for control (avoid loosing control).
tdble b4; // Brake value for avoiding high angle of attack.
tdble b5; // Brake for the pit;
tdble steer, targetAngle, shiftaccel;
MyCar* myc = mycar[index-1];
Pathfinder* mpf = myc->getPathfinderPtr();
b1 = b2 = b3 = b4 = b5 = 0.0;
shiftaccel = 0.0;
// Update some values needed.
myc->update(myTrackDesc, car, situation);
// Decide how we want to drive, choose a behaviour.
if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) {
myc->loadBehaviour(myc->NORMAL);
} else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) {
myc->loadBehaviour(myc->CAREFUL);
} else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) {
myc->loadBehaviour(myc->SLOW);
}
// Update the other cars just once.
if (currenttime != situation->currentTime) {
currenttime = situation->currentTime;
for (int i = 0; i < situation->_ncars; i++) ocar[i].update();
}
// Startmode.
if (myc->trtime < 5.0 && myc->bmode != myc->START) {
myc->loadBehaviour(myc->START);
myc->startmode = true;
}
if (myc->startmode && myc->trtime > 5.0) {
myc->startmode = false;
myc->loadBehaviour(myc->NORMAL);
}
// Compute path according to the situation.
mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar);
// Clear ctrl structure with zeros and set the current gear.
memset(&car->ctrl, 0, sizeof(tCarCtrl));
car->_gearCmd = car->_gear;
// Uncommenting the following line causes pitstop on every lap.
//if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId());
// Compute fuel consumption.
if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) {
if (car->race.laps > 0) {
myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel));
}
myc->lastfuel = car->priv.fuel;
myc->lastpitfuel = 0.0;
myc->fuelchecked = true;
} else if (myc->getCurrentSegId() > 5) {
myc->fuelchecked = false;
}
// Decide if we need a pit stop.
if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE ||
(car->priv.fuel < (myc->fuelperlap*(1.0+myc->FUEL_SAFETY_MARGIN)) &&
car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap)))
{
mpf->setPitStop(true, myc->getCurrentSegId());
}
if (mpf->getPitStop()) {
car->_raceCmd = RM_CMD_PIT_ASKED;
}
// Steer toward the next target point.
targetAngle = atan2(myc->dynpath->getLoc(myc->destpathsegid)->y - car->_pos_Y, myc->dynpath->getLoc(myc->destpathsegid)->x - car->_pos_X);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
steer = targetAngle / car->_steerLock;
// Steer P (proportional) controller. We add a steer correction proportional to the distance error
// to the path.
// Steer angle has usual meaning, therefore + is to left (CCW) and - to right (CW).
// derror sign is + to right and - to left.
if (!mpf->getPitStop()) {
steer = steer + MIN(myc->STEER_P_CONTROLLER_MAX, myc->derror*myc->STEER_P_CONTROLLER_GAIN)*myc->getErrorSgn();
if (fabs(steer) > 1.0) {
steer/=fabs(steer);
}
} else {
// Check if we are almost in the pit to set brake to the max to avoid overrun.
tdble dl, dw;
RtDistToPit(car, myTrackDesc->getTorcsTrack(), &dl, &dw);
if (dl < 1.0f) {
b5 = 1.0f;
}
}
// Try to control angular velocity with a D (differential) controller.
double omega = myc->getSpeed()/myc->dynpath->getRadius(myc->currentpathsegid);
steer += myc->STEER_D_CONTROLLER_GAIN*(omega - myc->getCarPtr()->_yaw_rate);
// Brakes.
tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION);
tdble brakespeed, brakedist;
tdble lookahead = 0.0;
int i = myc->getCurrentSegId();
brake = 0.0;
while (lookahead < brakecoeff * myc->getSpeedSqr()) {
lookahead += myc->dynpath->getLength(i);
brakespeed = myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(i);
if (brakespeed > 0.0) {
tdble gm, qb, qs;
gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha();
qs = myc->dynpath->getSpeedsqr(i);
brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw)));
if (brakedist > lookahead - myc->getWheelTrack()) {
qb = brakespeed*brakecoeff/brakedist;
if (qb > b2) {
b2 = qb;
}
}
}
i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg();
}
if (myc->getSpeedSqr() > myc->dynpath->getSpeedsqr(myc->currentpathsegid)) {
b1 = (myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(myc->currentpathsegid)) / (myc->getSpeedSqr());
}
// Try to avoid flying.
if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) {
b4 = 1.0;
}
// Check if we are on the way.
if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) {
if (myc->derror > myc->PATHERR) {
vec2d *cd = myc->getDir();
vec2d *pd = myc->dynpath->getDir(myc->currentpathsegid);
float z = cd->x*pd->y - cd->y*pd->x;
// If the car points away from the path brake.
if (z*myc->getErrorSgn() >= 0.0) {
targetAngle = atan2(myc->dynpath->getDir(myc->currentpathsegid)->y, myc->dynpath->getDir(myc->currentpathsegid)->x);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos())));
b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder;
}
}
}
// Anti wheel locking and brake code.
if (b1 > b2) brake = b1; else brake = b2;
if (brake < b3) brake = b3;
if (brake < b4) {
brake = MIN(1.0, b4);
tdble abs_mean;
abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed();
abs_mean /= 2.0;
brake = brake * abs_mean;
} else {
brake = MIN(1.0, brake);
tdble abs_min = 1.0;
for (int i = 0; i < 4; i++) {
tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed();
if (slip < abs_min) abs_min = slip;
}
brake = brake * abs_min;
}
// Reduce brake value to the approximate normal force available on the wheels.
float weight = myc->mass*G;
float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED;
float force = weight + myc->ca*myc->getSpeedSqr();
brake = brake*MIN(1.0, force/maxForce);
if (b5 > 0.0f) {
brake = b5;
}
// Gear changing.
if (myc->tr_mode == 0) {
if (car->_gear <= 0) {
car->_gearCmd = 1;
} else {
float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
float omega = car->_enginerpmRedLine/gr_up;
float wr = car->_wheelRadius(2);
if (omega*wr*myc->SHIFT < car->_speed_x) {
car->_gearCmd++;
} else {
float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
omega = car->_enginerpmRedLine/gr_down;
if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) {
car->_gearCmd--;
}
}
}
}
// Acceleration / brake execution.
tdble cerror, cerrorh;
cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y);
if (cerrorh > myc->TURNSPEED) {
cerror = fabs(car->_speed_x)/cerrorh;
} else {
cerror = 1.0;
}
if (myc->tr_mode == 0) {
if (brake > 0.0) {
myc->accel = 0.0;
car->_accelCmd = myc->accel;
car->_brakeCmd = brake*cerror;
} else {
if (myc->getSpeedSqr() < myc->dynpath->getSpeedsqr(myc->getCurrentSegId())) {
if (myc->accel < myc->ACCELLIMIT) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel/cerror;
} else {
if (myc->accel > 0.0) {
myc->accel -= myc->ACCELINC;
}
// TODO: shiftaccel always 0 at the moment...
car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror);
}
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed > myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE);
}
}
}
// Check if we are stuck, try to get unstuck.
tdble bx = myc->getDir()->x, by = myc->getDir()->y;
tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y;
tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by));
if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) {
myc->turnaround += situation->deltaTime;
} else myc->turnaround = 0.0;
if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) {
if (myc->tr_mode == 0) {
myc->tr_mode = 1;
}
if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) {
car->_accelCmd = 0.0;
if (myc->tr_mode == 1) {
car->_gearCmd--;
}
car->_brakeCmd = 1.0;
} else {
myc->tr_mode = 2;
if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) {
angle = queryAngleToTrack(car);
car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0;
car->_brakeCmd = 0.0;
if (myc->accel < 1.0) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel;
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed < -myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE);
}
} else {
if (myc->getSpeed() < 1.0) {
myc->turnaround = 0;
myc->tr_mode = 0;
myc->loadBehaviour(myc->START);
myc->startmode = true;
myc->trtime = 0.0;
}
car->_brakeCmd = 1.0;
car->_steerCmd = 0.0;
car->_accelCmd = 0.0;
}
}
}
if (myc->tr_mode == 0) car->_steerCmd = steer;
car->_clutchCmd = getClutch(myc, car);
}
/* controls the car */
static void drive(int index, tCarElt* car, tSituation *situation)
{
tdble angle;
tdble brake;
tdble b1; /* brake value in case we are to fast HERE and NOW */
tdble b2; /* brake value for some brake point in front of us */
tdble b3; /* brake value for control (avoid loosing control) */
tdble b4; /* brake value for avoiding high angle of attack */
tdble steer, targetAngle, shiftaccel;
MyCar* myc = mycar[index-1];
Pathfinder* mpf = myc->getPathfinderPtr();
b1 = b2 = b3 = b4 = 0.0;
shiftaccel = 0.0;
/* update some values needed */
myc->update(myTrackDesc, car, situation);
/* decide how we want to drive */
if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) {
myc->loadBehaviour(myc->NORMAL);
} else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) {
myc->loadBehaviour(myc->CAREFUL);
} else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) {
myc->loadBehaviour(myc->SLOW);
}
/* update the other cars just once */
if (currenttime != situation->currentTime) {
currenttime = situation->currentTime;
for (int i = 0; i < situation->_ncars; i++) ocar[i].update();
}
/* startmode */
if (myc->trtime < 5.0 && myc->bmode != myc->START) {
myc->loadBehaviour(myc->START);
myc->startmode = true;
}
if (myc->startmode && myc->trtime > 5.0) {
myc->startmode = false;
myc->loadBehaviour(myc->NORMAL);
}
/* compute path according to the situation */
mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar);
/* clear ctrl structure with zeros and set the current gear */
memset(&car->ctrl, 0, sizeof(tCarCtrl));
car->_gearCmd = car->_gear;
/* uncommenting the following line causes pitstop on every lap */
//if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId());
/* compute fuel consumption */
if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) {
if (car->race.laps > 0) {
myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel));
}
myc->lastfuel = car->priv.fuel;
myc->lastpitfuel = 0.0;
myc->fuelchecked = true;
} else if (myc->getCurrentSegId() > 5) {
myc->fuelchecked = false;
}
/* decide if we need a pit stop */
if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE ||
(car->priv.fuel < 1.5*myc->fuelperlap &&
car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap)))
{
mpf->setPitStop(true, myc->getCurrentSegId());
}
if (mpf->getPitStop()) {
car->_raceCmd = RM_CMD_PIT_ASKED;
}
/* steer to next target point */
targetAngle = atan2(myc->destpathseg->getLoc()->y - car->_pos_Y, myc->destpathseg->getLoc()->x - car->_pos_X);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
steer = targetAngle / car->_steerLock;
/* brakes */
tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION);
tdble brakespeed, brakedist;
tdble lookahead = 0.0;
int i = myc->getCurrentSegId();
brake = 0.0;
while (lookahead < brakecoeff * myc->getSpeedSqr()) {
lookahead += mpf->getPathSeg(i)->getLength();
brakespeed = myc->getSpeedSqr() - mpf->getPathSeg(i)->getSpeedsqr();
if (brakespeed > 0.0) {
tdble gm, qb, qs;
gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha();
qs = mpf->getPathSeg(i)->getSpeedsqr();
brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw)));
if (brakedist > lookahead - myc->getWheelTrack()) {
qb = brakespeed*brakecoeff/brakedist;
if (qb > b2) {
b2 = qb;
}
}
}
i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg();
}
if (myc->getSpeedSqr() > myc->currentpathseg->getSpeedsqr()) {
b1 = (myc->getSpeedSqr() - myc->currentpathseg->getSpeedsqr()) / (myc->getSpeedSqr());
}
/* try to avoid flying */
if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) {
b4 = 1.0;
}
if (!mpf->getPitStop()) {
steer = steer + MIN(0.1, myc->derror*0.02)*myc->getErrorSgn();
if (fabs(steer) > 1.0) steer/=fabs(steer);
}
/* check if we are on the way */
if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) {
if (myc->derror > myc->PATHERR) {
v3d r;
myc->getDir()->crossProduct(myc->currentpathseg->getDir(), &r);
if (r.z*myc->getErrorSgn() >= 0.0) {
targetAngle = atan2(myc->currentpathseg->getDir()->y, myc->currentpathseg->getDir()->x);
targetAngle -= car->_yaw;
NORM_PI_PI(targetAngle);
double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos())));
b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder;
}
}
}
/* try to control angular velocity */
double omega = myc->getSpeed()/myc->currentpathseg->getRadius();
steer += 0.1*(omega - myc->getCarPtr()->_yaw_rate);
/* anti blocking and brake code */
if (b1 > b2) brake = b1; else brake = b2;
if (brake < b3) brake = b3;
if (brake < b4) {
brake = MIN(1.0, b4);
tdble abs_mean;
abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed();
abs_mean /= 2.0;
brake = brake * abs_mean;
} else {
brake = MIN(1.0, brake);
tdble abs_min = 1.0;
for (int i = 0; i < 4; i++) {
tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed();
if (slip < abs_min) abs_min = slip;
}
brake = brake * abs_min;
}
float weight = myc->mass*G;
float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED;
float force = weight + myc->ca*myc->getSpeedSqr();
brake = brake*MIN(1.0, force/maxForce);
// Gear changing.
if (myc->tr_mode == 0) {
if (car->_gear <= 0) {
car->_gearCmd = 1;
} else {
float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
float omega = car->_enginerpmRedLine/gr_up;
float wr = car->_wheelRadius(2);
if (omega*wr*myc->SHIFT < car->_speed_x) {
car->_gearCmd++;
} else {
float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
omega = car->_enginerpmRedLine/gr_down;
if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) {
car->_gearCmd--;
}
}
}
}
tdble cerror, cerrorh;
cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y);
if (cerrorh > myc->TURNSPEED) cerror = fabs(car->_speed_x)/cerrorh; else cerror = 1.0;
/* acceleration / brake execution */
if (myc->tr_mode == 0) {
if (brake > 0.0) {
myc->accel = 0.0;
car->_accelCmd = myc->accel;
car->_brakeCmd = brake*cerror;
} else {
if (myc->getSpeedSqr() < mpf->getPathSeg(myc->getCurrentSegId())->getSpeedsqr()) {
if (myc->accel < myc->ACCELLIMIT) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel/cerror;
} else {
if (myc->accel > 0.0) {
myc->accel -= myc->ACCELINC;
}
car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror);
}
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed > myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE);
}
}
}
/* check if we are stuck, try to get unstuck */
tdble bx = myc->getDir()->x, by = myc->getDir()->y;
tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y;
tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by));
if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), mpf->getPathSeg(myc->getCurrentSegId())->getLoc()) > myc->TURNTOL)) {
myc->turnaround += situation->deltaTime;
} else myc->turnaround = 0.0;
if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) {
if (myc->tr_mode == 0) {
myc->tr_mode = 1;
}
if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) {
car->_accelCmd = 0.0;
if (myc->tr_mode == 1) {
car->_gearCmd--;
}
car->_brakeCmd = 1.0;
} else {
myc->tr_mode = 2;
if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), mpf->getPathSeg(myc->getCurrentSegId())->getLoc()) > myc->TURNTOL)) {
angle = queryAngleToTrack(car);
car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0;
car->_brakeCmd = 0.0;
if (myc->accel < 1.0) {
myc->accel += myc->ACCELINC;
}
car->_accelCmd = myc->accel;
tdble slipspeed = myc->querySlipSpeed(car);
if (slipspeed < -myc->TCL_SLIP) {
car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE);
}
} else {
if (myc->getSpeed() < 1.0) {
myc->turnaround = 0;
myc->tr_mode = 0;
myc->loadBehaviour(myc->START);
myc->startmode = true;
myc->trtime = 0.0;
}
car->_brakeCmd = 1.0;
car->_steerCmd = 0.0;
car->_accelCmd = 0.0;
}
}
}
if (myc->tr_mode == 0) car->_steerCmd = steer;
timeSinceLastUpdate[index - 1] += situation->deltaTime;
//Only update once per second
if(timeSinceLastUpdate[index - 1] >= 0.1)
{
/* steer to next target point */
float targetAngleOut = atan2(myc->destpathseg->getLoc()->y - car->_pos_Y, myc->destpathseg->getLoc()->x - car->_pos_X);
targetAngleOut -= car->_yaw;
NORM_PI_PI(targetAngleOut);
timeSinceLastUpdate[index - 1] = 0.0;
sensors[index - 1]->sensors_update();
//Write training data
outputFiles[index - 1]<<"DATA"<<std::endl;
//INPUT
outputFiles[index - 1]<<"speed "<<myc->getSpeed()<<std::endl;
outputFiles[index - 1]<<"angle "<<myc->getDeltaPitch()<<std::endl;
outputFiles[index - 1]<<"targetAngle "<<targetAngleOut<<std::endl;
//Distance sensors
outputFiles[index - 1]<<"distR "<<sensors[index - 1]->getSensorOut(0)<<std::endl;
outputFiles[index - 1]<<"distFR "<<sensors[index - 1]->getSensorOut(1)<<std::endl;
outputFiles[index - 1]<<"distFFR "<<sensors[index - 1]->getSensorOut(2)<<std::endl;
outputFiles[index - 1]<<"distF "<<sensors[index - 1]->getSensorOut(3)<<std::endl;
outputFiles[index - 1]<<"distFFL "<<sensors[index - 1]->getSensorOut(4)<<std::endl;
outputFiles[index - 1]<<"distFL "<<sensors[index - 1]->getSensorOut(5)<<std::endl;
outputFiles[index - 1]<<"distL "<<sensors[index - 1]->getSensorOut(6)<<std::endl;
//OUTPUT
outputFiles[index - 1]<<"steer "<<car->ctrl.steer<<std::endl;
outputFiles[index - 1]<<"accel "<<car->ctrl.accelCmd<<std::endl;
outputFiles[index - 1]<<"brake "<<car->ctrl.brakeCmd<<std::endl;
outputFiles[index - 1]<<"gear "<<car->ctrl.gear<<std::endl;
outputFiles[index - 1]<<"clutch "<<car->ctrl.clutchCmd<<std::endl;
//Write training data to console
printf_s("-----------------------------\n");
printf_s("Speed: %f\n", myc->getSpeed());
printf_s("Steering: %f\n", car->ctrl.steer);
printf_s("Acceleration: %f\n", car->ctrl.accelCmd);
printf_s("Brake: %f\n", car->ctrl.brakeCmd);
printf_s("Gear: %f\n", car->ctrl.gear);
printf_s("Clutch: %f\n", car->ctrl.clutchCmd);
printf_s("Angle: %f\n", myc->getDeltaPitch());
printf_s("Target Angle: %f\n\n", targetAngleOut);
printf_s("distR %f\n", sensors[index - 1]->getSensorOut(0));
printf_s("distFR %f\n", sensors[index - 1]->getSensorOut(1));
printf_s("distFFR %f\n", sensors[index - 1]->getSensorOut(2));
printf_s("distF %f\n", sensors[index - 1]->getSensorOut(3));
printf_s("distFFL %f\n", sensors[index - 1]->getSensorOut(4));
printf_s("distFL %f\n", sensors[index - 1]->getSensorOut(5));
printf_s("distL %f\n", sensors[index - 1]->getSensorOut(6));
}
}