C++ CarState::getAngle方法代码示例

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

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

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

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

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

CarControl
ANNdataGather::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);

        // build a CarControl variable and return it
        CarControl cc(accel,brake,gear,steer,clutch);

        // Record input - output
        ofstream myfile("ANNtraining.txt", std::ios_base::app);


        // Needs to consider standardising input values for better networks
        // Inputs from CarState
        // Check TORCS competition manual for description of all possible inputs
        // Performance is much better if you normalise as much as possible inputs (between -1 and 1, or 0 to 1)
        myfile << cs.getAngle() << " " << cs.getTrackPos() << "\n";

        // Network outputs should be between -1 and 1!
        // Outputs from CarControl
        // Check TORCS competition manual for description of all possible outputs
        myfile << accel << " " << brake << " " << steer << "\n";

        myfile.close();

        samples++; // counting the number of samples gathered in current training

        return cc;
    }
}

bool
Stuck::notStuckAnymore(CarState &cs) {
    return onRightWay(cs.getTrackPos(), cs.getAngle());
}

//.........这里部分代码省略.........
		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;
}

bool Curve::isFacingWrongWay(CarState &cs) {
    return cs.getAngle() < -M_PI/2 || cs.getAngle() > M_PI/2;
}

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

/**The steer is a essential actuator, getSteer obtain it using the track sensors to get the higher one and aplied that sensor's angle.*/
float Curve::getSteer(CarState &cs) {
    return isFacingWrongWay(cs) ? cs.getAngle() : findFarthestDirection(cs);
}