bool Stuck::seemsStuck(CarState &cs) {
cs.getSpeedX()<STUCK_SPEED?slowSpeedTicks++:slowSpeedTicks = 0;
if(notStuckAnymore(cs.getTrackPos(), cs.getAngle())){
slowSpeedTicks=0;
}
return (slowSpeedTicks>MAX_SLOW_SPEED_TICKS?1:0);
}
float
SimpleDriver::filterABS(CarState &cs,float brake)
{
// convert speed to m/s
float speed = cs.getSpeedX() / 3.6;
// when spedd lower than min speed for abs do nothing
if (speed < absMinSpeed)
return brake;
// compute the speed of wheels in m/s
float slip = 0.0f;
for (int i = 0; i < 4; i++)
{
slip += cs.getWheelSpinVel(i) * wheelRadius[i];
}
// slip is the difference between actual speed of car and average speed of wheels
slip = speed - slip/4.0f;
// when slip too high applu ABS
if (slip > absSlip)
{
brake = brake - (slip - absSlip)/absRange;
}
// check brake is not negative, otherwise set it to zero
if (brake<0)
return 0;
else
return brake;
}
/** The transition choose the most fitted state at the moment of the race. */
void
FSMDriver3::transition(CarState &cs) {
DrivingState *state = current_state;
if(gameTicks <= 10000){
distRaced = cs.getDistRaced();
ticks_on_inside_track = inside_track->get_ticks_in_state();
ticks_on_stuck = stuck->get_ticks_in_state();
ticks_on_out_of_track = out_of_track->get_ticks_in_state();
damage = cs.getDamage();
gameTicks += 1;
}/*
else if(gameTicks == 10000){
distRaced = cs.getDistRaced();
gameTicks+=1;
ticks_on_inside_track = inside_track->get_ticks_in_state();
ticks_on_stuck = stuck->get_ticks_in_state();
ticks_on_out_of_track = out_of_track->get_ticks_in_state();
damage = cs.getDamage();
}*/
setTrackType();
if(stuck->isStuck(cs)) {
state = stuck;
} else {
if (cs.getTrack(1) > 0)
state = inside_track;
else {
state = out_of_track;
}
}
if (current_state != state) changeTo(state);
}
int
getGear(CarState & cs) {
int current_gear = cs.getGear();
if(!current_gear) return 1;
if(cs.getRpm() > 8000) ++current_gear;
else if(current_gear > 1 && cs.getRpm() < 5000) --current_gear;
return current_gear;
}
float ApproachingCurve::getBrake(CarState &cs) {
float brake = 0;
float brakeFactor = 0.02;
float diff = cs.getSpeedX() - targetSpeed;
//if (fabs(cs.getSpeedX()) < 2) return 1;
if (cs.getSpeedX() < 0) return 1;
if (diff > 0) brake = brakeFactor * diff;
return brake;
}
int
InsideTrackA::get_gear(CarState &cs) {
int gear = cs.getGear();
if(gear <= 0) return start_gear;
int rpm = cs.getRpm();
if(shouldIncreaseGear(gear, rpm)) ++gear;
else if(shouldDecreaseGear(gear, rpm)) --gear;
return gear;
}
float
SimpleDriver::getSteer(CarState &cs)
{
// steering angle is compute by correcting the actual car angle w.r.t. to track
// axis [cs.getAngle()] and to adjust car position w.r.t to middle of track [cs.getTrackPos()*0.5]
float targetAngle=(cs.getAngle()-cs.getTrackPos()*0.5);
// at high speed reduce the steering command to avoid loosing the control
if (cs.getSpeedX() > steerSensitivityOffset)
return targetAngle/(steerLock*(cs.getSpeedX()-steerSensitivityOffset)*wheelSensitivityCoeff);
else
return (targetAngle)/steerLock;
}
CarControl Stuck::drive(FSMDriver5 *fsmdriver5, CarState &cs) {
++elapsedTicks;
trackInitialPos = getInitialPos(cs);
if(notStuckAnymore(cs.getTrackPos(), cs.getAngle()) || hasBeenStuckLongEnough()){
elapsedTicks = 0;
slowSpeedTicks = 0;
trackInitialPos = 0;
}
const float accel = 1, brake = 0, clutch = 0;
const int gear = -1, focus = 0, meta = 0;
float steer = getSteer(trackInitialPos, cs);
return CarControl(accel, brake, gear, steer, clutch, focus, meta);
}
/**************************************************************************
* Modularization*/
float
Stuck::get_steer(CarState &cs) {
if(abs(cs.getAngle()) > M_PI) // around 180 graus
return (getInitialPos(cs) > 0 ? -1 : 1);
return (getInitialPos(cs) > 0 ? 1 : -1);
}
float
Stuck::auxSteer(float track_initial_pos, CarState &cs){
if(abs(cs.getAngle()) > M_PI) // around 180 graus
return (track_initial_pos > 0 ? -1 : 1);
return (track_initial_pos > 0 ? 1 : -1);
}
float Stuck::getSteer(float trackInitialPos, CarState &cs){
//return (trackInitialPos > 0 ? 1 : -1);
if(abs(cs.getAngle()) > 1.557){// around 180 graus
return (trackInitialPos > 0 ? -1 : 1);
}else{
return (trackInitialPos > 0 ? 1 : -1);
}
}
float
getSteering(CarState & cs) {
// based on Loiacono's SimpleDriver
const float
steerLock = 0.366519;
float
targetAngle = (cs.getAngle() - cs.getTrackPos() * 0.5) / steerLock;
// normalize steering
if(targetAngle < -1)
targetAngle = -1;
else if(targetAngle > 1)
targetAngle = 1;
return targetAngle;
}
float
ApproachingCurve::getSteering(CarState &cs) {
if(r_sensor == l_sensor) return 0;
float angle = cs.getAngle();
// If the controller is not in a pre-defined region amongst the inside limits of the track (between 0.7 and 0.9 with the current
// set of values, normalized), than it will be adjusted to do so
bool adjustedToCurve = ((fabs(cs.getTrackPos()) - target_pos >= 0) && (fabs(cs.getTrackPos()) - target_pos < 0.2));
if(!adjustedToCurve) {
if(approachingRightTurn())
angle = max_steering - angle;
else
angle -= max_steering;
}
return angle;
}
void
ApproachingCurve::updateSensors(CarState &cs) {
float speedFactor = 5000; // The target speed is obtained through a constant factor
if (cs.getFocus(2) == -1) { // Focus sensors are available only once per second
r_sensor = cs.getTrack(10); // Use track sensors
c_sensor = cs.getTrack(9);
l_sensor = cs.getTrack(8);
}
else {
r_sensor = cs.getFocus(3); // Use focus sensors
c_sensor = cs.getFocus(2);
l_sensor = cs.getFocus(1);
}
target_speed = base_speed + speedFactor / fabs(l_sensor - r_sensor);
sensors_are_updated = true;
}
float
InsideTrackA::get_accel(CarState &cs) {
setTargetSpeed(cs);
float Front, max10, max20;
Front = cs.getTrack(10);
max10 = max(cs.getTrack(9), cs.getTrack(11));
max20 = max(cs.getTrack(8), cs.getTrack(12));
float accel = (cs.getSpeedX() > target_speed ? 0 : (Front+max10+max20)/(3*200));
if(Front >= 70) accel = 1;
if(Front <= 20 && cs.getSpeedX() <= 30) accel = 1; /*Resolve o caso em que o carro está preso com a frente voltada para a borda da pista*/
//printf("%.0f, %.0f, %.0f --> accel: %.2f\n", Front, max10, max20, accel);
return accel ;
}
void ApproachingCurve::updateSensors(CarState &cs) {
float speedFactor = 5000; //The target speed is obtained through a constant factor
if (cs.getFocus(2) == -1) { //Focus sensors are available only once per second
// cout << "FOCUS MISS!" << endl;
rSensor = cs.getTrack(10); //Use track sensors
cSensor = cs.getTrack(9);
lSensor = cs.getTrack(8);
}
else {
// cout << "FOCUS HIT!" << endl;
rSensor = cs.getFocus(3); //Use focus sensors
cSensor = cs.getFocus(2);
lSensor = cs.getFocus(1);
}
targetSpeed = BASE_SPEED + speedFactor / fabs(lSensor - rSensor);
sensorsAreUpdated = true;
}
float ApproachingCurve::getSteering(CarState &cs) {
if(rSensor == lSensor) return 0;
float angle = cs.getAngle();
//If the controller is not in a pre-defined region amongst the inside limits of the track (between 0.7 and 0.9 with the current
//set of values, normalized), than it will be adjusted to do so
bool adjustedToCurve = ((fabs(cs.getTrackPos()) - TARGET_POS >= 0) && (fabs(cs.getTrackPos()) - TARGET_POS < 0.2));
//Previous conditions: // 0.2 is an arbitrary margin
//bool adjustedToCurve = (cs.getTrackPos() <= TARGET_POS);
if(!adjustedToCurve) {
if(approachingRightTurn())
angle = MAX_STEERING - angle;
else
angle -= MAX_STEERING;
}
return angle;
}
float
ApproachingCurve::getBrake(CarState &cs) {
float brake = 0;
float brake_factor = 0.02;
float diff = cs.getSpeedX() - target_speed;
if (isGoingWrongWay(cs)) return 1;
if (diff > 0) brake = brake_factor * diff;
return brake;
}
CarControl ApproachingCurve::drive(FSMDriver5 *FSMDriver5, CarState &cs) {
if(!sensorsAreUpdated) /*@todo Só atualiza na 1a vez mesmo? */
updateSensors(cs);
const int focus = 0, meta = 0;
const float clutch = 0;
return CarControl(getAccel(cs), getBrake(cs), getGear(cs), cs.getAngle(), clutch, focus, meta);
//Use the line below if the behavior of adjusting the car to the curve ahead is desired (not fully functional):
//return CarControl(getAccel(cs), getBrake(cs), getGear(cs), getSteering(cs), clutch, focus, meta);
}
bool
Stuck::seemsStuck(CarState &cs) {
if(cs.getSpeedX() < stuck_speed)
++slow_speed_ticks;
else
slow_speed_ticks = 0;
if(notStuckAnymore(cs))
slow_speed_ticks = 0;
return (slow_speed_ticks > maximum_number_of_ticks_in_slow_speed);
}
float Curve::findFarthestDirection(CarState &cs) {
float farthestSensor = -INFINITY;
float farthestDirection = 0;
for (int i = 0; i < 19; i++) {
if (farthestSensor < cs.getTrack(i)) {
farthestSensor = cs.getTrack(i);
farthestDirection = i;
}
}
farthestDirection = -M_PI/2 + farthestDirection*M_PI/18;
return normalizeSteer(-farthestDirection);
}
int
SimpleDriver::getGear(CarState &cs)
{
int gear = cs.getGear();
int rpm = cs.getRpm();
// if gear is 0 (N) or -1 (R) just return 1
if (gear<1)
return 1;
// check if the RPM value of car is greater than the one suggested
// to shift up the gear from the current one
if (gear <6 && rpm >= gearUp[gear-1])
return gear + 1;
else
// check if the RPM value of car is lower than the one suggested
// to shift down the gear from the current one
if (gear > 1 && rpm <= gearDown[gear-1])
return gear - 1;
else // otherwhise keep current gear
return gear;
}
void
SimpleDriver::clutching(CarState &cs, float &clutch)
{
double maxClutch = clutchMax;
// Check if the current situation is the race start
if (cs.getCurLapTime()<clutchDeltaTime && stage==RACE && cs.getDistRaced()<clutchDeltaRaced)
clutch = maxClutch;
// Adjust the current value of the clutch
if(clutch > 0)
{
double delta = clutchDelta;
if (cs.getGear() < 2)
{
// Apply a stronger clutch output when the gear is one and the race is just started
delta /= 2;
maxClutch *= clutchMaxModifier;
if (cs.getCurLapTime() < clutchMaxTime)
clutch = maxClutch;
}
// check clutch is not bigger than maximum values
clutch = min(maxClutch,double(clutch));
// if clutch is not at max value decrease it quite quickly
if (clutch!=maxClutch)
{
clutch -= delta;
clutch = max(0.0,double(clutch));
}
// if clutch is at max value decrease it very slowly
else
clutch -= clutchDec;
}
}
float
SimpleDriver::getAccel(CarState &cs)
{
// checks if car is out of track
if (cs.getTrackPos() < 1 && cs.getTrackPos() > -1)
{
// reading of sensor at +5 degree w.r.t. car axis
float rxSensor=cs.getTrack(10);
// reading of sensor parallel to car axis
float cSensor=cs.getTrack(9);
// reading of sensor at -5 degree w.r.t. car axis
float sxSensor=cs.getTrack(8);
float targetSpeed;
// track is straight and enough far from a turn so goes to max speed
if (cSensor>maxSpeedDist || (cSensor>=rxSensor && cSensor >= sxSensor))
targetSpeed = maxSpeed;
else
{
// approaching a turn on right
if(rxSensor>sxSensor)
{
// computing approximately the "angle" of turn
float h = cSensor*sin5;
float b = rxSensor - cSensor*cos5;
float sinAngle = b*b/(h*h+b*b);
// estimate the target speed depending on turn and on how close it is
targetSpeed = maxSpeed*(cSensor*sinAngle/maxSpeedDist);
}
// approaching a turn on left
else
{
// computing approximately the "angle" of turn
float h = cSensor*sin5;
float b = sxSensor - cSensor*cos5;
float sinAngle = b*b/(h*h+b*b);
// estimate the target speed depending on turn and on how close it is
targetSpeed = maxSpeed*(cSensor*sinAngle/maxSpeedDist);
}
}
// accel/brake command is expontially scaled w.r.t. the difference between target speed and current one
return 2/(1+exp(cs.getSpeedX() - targetSpeed)) - 1;
}
else
return 0.3; // when out of track returns a moderate acceleration command
}
bool Stuck::justStartedRace(CarState &cs) {
return (cs.getDistRaced() <= MIN_RACED_DISTANCE);
}
float
Stuck::getInitialPos(CarState &cs) {
return (track_initial_pos == 0 ? cs.getTrackPos() : track_initial_pos);
}
bool
Stuck::notStuckAnymore(CarState &cs) {
return onRightWay(cs.getTrackPos(), cs.getAngle());
}
bool
Stuck::justStartedRace(CarState &cs) {
return (cs.getDistRaced() <= minimum_distance_raced);
}
CarControl MyDriver::wDrive(CarState cs){
// Update clock variables for graphing over time
_timePrevious = _timeCurrent;
_timeCurrent = Clock::now();
_dt = _timeCurrent - _timePrevious;
_runtime += _dt;
if (_logging)
Renderer::get().setWindowTitle(std::to_string(_dt.count()));
// Temporarily copy old sensors to delta and reset sensors with max distance
memcpy(&_driving.delta[0], &_driving.sensors[0], sizeof(_driving.sensors));
std::fill_n(_driving.sensors, ALL_SENSORS, 200.f);
// Directly mapping track sensors to front 19 sensors
int zero = 0;
for (int i = 0; i < HALF_SENSORS; i++){
float distance = cs.getTrack(i);
if (distance <= 0.f)
zero++;
if (distance < _driving.sensors[i + QUART_SENSORS])
_driving.sensors[i + QUART_SENSORS] = distance;
}
if (zero == 19)
_driving.crashed = true;
else if (_driving.crashed)
_driving.crashed = false;
// Iterate through opponent sensors
for (int i = 0; i < ALL_SENSORS; i++){
//std::cout << "i - " << i << "\n";
float distance = cs.getOpponents(i);
// If no opponent, skip sensor
if (distance >= _awarnessOpponent * 200.f)
continue;
// Calculate amount of track sensors to block, and round to nearest even integer
float raw = distance / (_awarnessOpponent * 200.f);
float block = ((1.f - raw) * (float)((_maxBlock - 2) + 2));
block = glm::roundEven(block);
// Apply smaller distances to sensor array
for (int x = block / 2; x >= -(block / 2 - 1); x--){
int sensor = (i + x) % ALL_SENSORS;
float dropOff = glm::abs(changeRange(block / 2, -(block / 2 - 1), -1.f, 1.f, x));
if (_driving.sensors[sensor] > distance + dropOff * _blockDropOff)
_driving.sensors[sensor] = distance + dropOff * _blockDropOff;
}
}
// Calculate difference using old sensors stored in delta
for (int i = 0; i < ALL_SENSORS; i++){
_driving.delta[i] = _driving.delta[i] - _driving.sensors[i];
if (_driving.delta[i] < 0.f)
_driving.delta[i] = glm::abs(_driving.delta[i]);
else
_driving.delta[i] = 0.f;
if (_driving.delta[i] > 0.25f)
_driving.delta[i] = 0.f;
}
// Decreasing sensors around furthest ray
// if furthest ray is on left, decrease right sensors
// if furthest ray is on right, decrease left sensors
// if furthest ray is in the middle, decrease both sides sensors
if (_driving.furthestRay <= QUART_SENSORS){
for (int i = _driving.furthestRay; i < HALF_SENSORS; i++){
_driving.sensors[QUART_SENSORS + i] *= changeRange(_driving.furthestRay, 18, 1, 0, i);
}
}
if (_driving.furthestRay >= QUART_SENSORS){
for (int i = _driving.furthestRay; i >= 0; i--){
_driving.sensors[QUART_SENSORS + i] *= changeRange(_driving.furthestRay, 0, 1, 0, i);
}
}
// Choosing which ray to steer towards, and how fast
float maxDistance = _driving.sensors[_driving.furthestRay + QUART_SENSORS];
for (int i = 0; i < HALF_SENSORS; i++){
float distance = _driving.sensors[i + QUART_SENSORS];
if (maxDistance < distance){
maxDistance = distance;
_driving.furthestRay = i;
}
}
// P - PID
float proportional = (changeRange(0.f, 18.f, 1.5f, -1.5f, _driving.furthestRay) - (cs.getTrackPos() / 500.f)) - cs.getTrackPos() * _middleDrift;
proportional *= _p;
// I - PID
while (_driving.steerHistory.size() > _historySteerLength)
_driving.steerHistory.erase(_driving.steerHistory.begin());
float integral = 0.f;
for (float i : _driving.steerHistory)
integral += i;
integral *= _i;
// D - PID
float derivative = 0.f;
if (_driving.steerHistory.size() >= 2)
derivative = _driving.steerHistory[_driving.steerHistory.size() - 1] - _driving.steerHistory[_driving.steerHistory.size() - 2];
derivative *= _d;
// P + I + D
float oldSteer = _driving.steer;
_driving.steer = proportional + integral + derivative;
_driving.steerHistory.push_back(_driving.steer);
_driving.steer = glm::mix(oldSteer, _driving.steer, _easeSteer);
// Speed control
if (!_driving.crashed){
float speed = _driving.sensors[_driving.furthestRay + QUART_SENSORS] / (200.f * _awarnessTrack) + _driving.delta[_driving.furthestRay + QUART_SENSORS] * 8.f;
speed *= (1.f - _speedRestrict);
_driving.speed = glm::mix(_driving.speed, speed, _easeAccel); //*(1.f - cs.getTrack(QUART_SENSORS)) * brake;
float brake = (cs.getSpeedX() / 200.f) - _driving.speed;
if (brake > 0.f)
_driving.brake = glm::mix(_driving.brake, brake, _easeBrake);
else
_driving.brake = 0.f;
}
// Changing gears
int gear = cs.getGear();
int rpm = cs.getRpm();
if (gear < 1)
_driving.gear = 1;
if (gear < 6 && rpm >= _gearUp[gear - 1])
_driving.gear = gear + 1;
else
if (gear > 1 && rpm <= _gearDown[gear - 1])
_driving.gear = gear - 1;
// If logging, render sensors
if (_logging){
Renderer::get().drawGraph({ _runtime.count() / 100.f, _driving.steer }, 0);
Renderer::get().drawGraph({ _runtime.count() / 100.f, _driving.speed }, 1);
Renderer::get().drawGraph({ _runtime.count() / 100.f, _driving.brake }, 2);
for (int i = 0; i < ALL_SENSORS; i++){
float distance = _driving.sensors[i];
if (distance < 0.1f)
continue;
float radians = glm::radians(i * 10.f);
glm::vec2 point = { -glm::sin(radians), -glm::cos(radians) };
glm::vec3 colour = { 1.f, 1.f, 1.f };
if (_driving.furthestRay + QUART_SENSORS == i)
colour = { 0.f, 1.f, 0.f };
float zoom = 10.f;
Renderer::get().drawLine({ 0, 0 }, point * distance * zoom, colour);
}
if (_driving.crashed)
std::cout << "Crashed!\n";
// Un-comment for pointless cool effects
Renderer::get().setRotation(cs.getAngle() * 90.f);
Renderer::get().setZoom(1.f + cs.getSpeedX() / 200.f);
}
// Applying to controller
CarControl cc;
cc.setGear(_driving.gear);
cc.setAccel(_driving.speed);
cc.setSteer(_driving.steer);
cc.setBrake(_driving.brake);
return cc;
}
CarControl
SimpleDriver::wDrive(CarState cs)
{
// check if car is currently stuck
if ( fabs(cs.getAngle()) > stuckAngle )
{
// update stuck counter
stuck++;
}
else
{
// if not stuck reset stuck counter
stuck = 0;
}
// after car is stuck for a while apply recovering policy
if (stuck > stuckTime)
{
/* set gear and sterring command assuming car is
* pointing in a direction out of track */
// to bring car parallel to track axis
float steer = - cs.getAngle() / steerLock;
int gear=-1; // gear R
// if car is pointing in the correct direction revert gear and steer
if (cs.getAngle()*cs.getTrackPos()>0)
{
gear = 1;
steer = -steer;
}
// Calculate clutching
clutching(cs,clutch);
// build a CarControl variable and return it
CarControl cc (1.0,0.0,gear,steer,clutch);
return cc;
}
else // car is not stuck
{
// compute accel/brake command
float accel_and_brake = getAccel(cs);
// compute gear
int gear = getGear(cs);
// compute steering
float steer = getSteer(cs);
// normalize steering
if (steer < -1)
steer = -1;
if (steer > 1)
steer = 1;
// set accel and brake from the joint accel/brake command
float accel,brake;
if (accel_and_brake>0)
{
accel = accel_and_brake;
brake = 0;
}
else
{
accel = 0;
// apply ABS to brake
brake = filterABS(cs,-accel_and_brake);
}
// Calculate clutching
clutching(cs,clutch);
cout << "Steer: "<< steer << endl;
cout << "Accel: :"<< accel << endl;
// build a CarControl variable and return it
CarControl cc(accel,brake,gear,steer,clutch);
return cc;
}
}
