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; } }