void DistanceToObjectsReport::report(const odcore::wrapper::Time &t) {
            cerr << "Call to DistanceToObjectsReport for t = " << t.getSeconds() << "." << t.getPartialMicroseconds() << ", containing " << getFIFO().getSize() << " containers." << endl;

            // Get last EgoState.
            KeyValueDataStore &kvds = getKeyValueDataStore();
            Container c = kvds.get(opendlv::data::environment::EgoState::ID());
            EgoState es = c.getData<EgoState>();

            const uint32_t SIZE = getFIFO().getSize();
            for (uint32_t i = 0; i < SIZE; i++) {
                c = getFIFO().leave();
                cerr << "Received: " << c.toString() << endl;

                if (c.getDataType() == opendlv::data::environment::Obstacle::ID()) {
                    Obstacle o = c.getData<Obstacle>();

                    const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));
                    cerr << "DistanceToObjectsReport: Distance to object: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;

                    // Continuously check distance.
                    m_correctDistance &= (DISTANCE > m_threshold);

                    vector<Point3> shape = o.getPolygon().getVertices();
                    Point3 head = shape.front();
                    shape.push_back(head);
                    const uint32_t NUMVERTICES = shape.size();
                    for(uint32_t j = 1; j < NUMVERTICES; j++) {
                        Point3 pA = shape.at(j-1);
                        Point3 pB = shape.at(j);

                        // TODO: Check polygonal data as well as perpendicular to all sides.
                        // Create line.
                        Line l(pA, pB);

                        // Compute perpendicular point.
                        Point3 perpendicularPoint = l.getPerpendicularPoint(es.getPosition());

                        // Compute distance between current position and perpendicular point.
                        const float DISTANCE_PP = (es.getPosition().getDistanceTo(perpendicularPoint));

                        cerr << "DistanceToObjectsReport: Distance to object's shape: " << DISTANCE_PP << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << ", perpendicular point:" << perpendicularPoint.toString() << endl;

                        // Continuously check distance.
                        m_correctDistance &= (DISTANCE > m_threshold);
                    }
                }

                if (c.getDataType() == opendlv::data::environment::OtherVehicleState::ID()) {
                    OtherVehicleState o = c.getData<OtherVehicleState>();

                    const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));

                    // Compute distance between current position and perpendicular point.
                    cerr << "DistanceToObjectsReport: Distance to other vehicle: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;

                    // Continuously check distance.
                    m_correctDistance &= (DISTANCE > m_threshold);
                }
            }
        }
示例#2
0
void FCScene::doFrame(void)
{
	FCFighter* fighter = (FCFighter*)_player.get();
	//_camNode->setPosition(fighter->getPosition() + fighter->getAxis()*Ogre::Vector3(-0.5, 3, -1));
	////_camNode->setDirection(fighter->getAxis()*Ogre::Vector3::UNIT_Z, Ogre::Node::TS_WORLD);
	//_camNode->setOrientation(fighter->getNode()->getOrientation()*Ogre::Quaternion(0, 0, 1, 0));
	//FCKnowledge::getSingleton().setPlayerPosition(fighter->getPosition());
	DataLogger::getSingleton().getPlayerData(fighter->getID(), fighter->getPosition().ptr(), fighter->getOrientation().ptr(), fighter->getHealthPoint());
	for(unsigned int i = 0; i < fighter->getBulletList().size(); i++)
	{
		FCBullet* bullet = (FCBullet*)fighter->getBulletList()[i].get();
		if(bullet->isActive())
			DataLogger::getSingleton().getPlayerProjectileData(bullet->getID(), bullet->getPosition().ptr(), bullet->getOrientation().ptr());
	}
	for(unsigned int i = 0; i < _enemies.size(); i++)
	{
		FCFighter* enemy = (FCFighter*)_enemies[i].get();
		DataLogger::getSingleton().getBotData(enemy->getID(), enemy->getPosition().ptr(), enemy->getOrientation().ptr(), enemy->getHealthPoint());
		for(unsigned int i = 0; i < enemy->getBulletList().size(); i++)
		{
			FCBullet* bullet = (FCBullet*)enemy->getBulletList()[i].get();
			if(bullet->isActive())
				DataLogger::getSingleton().getBotProjectileData(bullet->getID(), enemy->getID(), bullet->getPosition().ptr(), bullet->getOrientation().ptr());
		}
	}
	Obstacle* obs = (Obstacle*)_obstacle1.get();
	DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale());
	
	obs = (Obstacle*)_obstacle2.get();
	DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale());
	
	obs = (Obstacle*)_obstacle3.get();
	DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale());

	DataLogger::getSingleton().writeTest();
}
示例#3
0
    Matrix getVLIM(const Obstacle &obstacle, const Matrix &v_lim)
    {
        Vector xo=obstacle.getPosition();
        deque<Vector> ctrlPoints=getCtrlPointsPosition();

        Matrix VLIM=v_lim;
        for (size_t i=0; i<ctrlPoints.size(); i++)
        {
            Vector dist=xo-ctrlPoints[i];
            double d=norm(dist);
            if (d>=obstacle.radius)
            {
                dist*=1.0-obstacle.radius/d;
                d=norm(dist);
            }
            else
            {
                dist=0.0;
                d=0.0;
            }
            
            double f=1.0/(1.0+exp((d*(2.0/rho)-1.0)*alpha));
            Matrix J=chainCtrlPoints[i]->GeoJacobian().submatrix(0,2,0,chainCtrlPoints[i]->getDOF()-1);
            Vector s=J.transposed()*dist;

            double red=1.0-f;
            for (size_t j=0; j<s.length(); j++)
            {
                if (s[j]>=0.0)
                {
                    double tmp=v_lim(j,1)*red;
                    VLIM(j,1)=std::min(VLIM(j,1),tmp);
                    VLIM(j,0)=std::min(VLIM(j,0),VLIM(j,1));
                }
                else
                {
                    double tmp=v_lim(j,0)*red;
                    VLIM(j,0)=std::max(VLIM(j,0),tmp);
                    VLIM(j,1)=std::max(VLIM(j,0),VLIM(j,1));
                }
            }
        }

        return VLIM;
    }
示例#4
0
float			RadarRenderer::transScale(const Obstacle& o)
{
  float scaleColor;
  const LocalPlayer* myTank = LocalPlayer::getMyTank();

  // Scale color so that objects that are close to tank's level are opaque
  const float zTank = myTank->getPosition()[2];
  const float zObstacle = o.getPosition()[2];
  const float hObstacle = o.getHeight();
  if (zTank >= (zObstacle + hObstacle))
    scaleColor = 1.0f - (zTank - (zObstacle + hObstacle)) / colorFactor;
  else if (zTank <= zObstacle)
    scaleColor = 1.0f - (zObstacle - zTank) / colorFactor;
  else
    scaleColor = 1.0f;

  if (scaleColor < 0.5f)
    scaleColor = 0.5f;

  return scaleColor;
}
示例#5
0
    Matrix getVLIM(const Obstacle &obstacle, const Matrix &v_lim)
    {
        Vector xo=obstacle.getPosition();
        deque<Vector> ctrlPoints=getCtrlPointsPosition();

        Matrix VLIM=v_lim;
        for (size_t i=0; i<ctrlPoints.size(); i++)
        {
            Vector dist=xo-ctrlPoints[i];
            double d=norm(dist);
            if (d>=obstacle.radius)
                continue;
            
            double f=k*(obstacle.radius-d);
            Matrix J=chainCtrlPoints[i]->GeoJacobian().submatrix(0,2,0,chainCtrlPoints[i]->getDOF()-1);
            Vector s=(-f/d)*(J.transposed()*dist);
            
            for (size_t j=0; j<s.length(); j++)
            {
                if (s[j]>=0.0)
                {
                    s[j]=std::min(v_lim(j,1),s[j]);
                    VLIM(j,0)=std::max(VLIM(j,0),s[j]);
                    VLIM(j,1)=std::max(VLIM(j,0),VLIM(j,1));
                }
                else
                {
                    s[j]=std::max(v_lim(j,0),s[j]);
                    VLIM(j,1)=std::min(VLIM(j,1),s[j]);
                    VLIM(j,0)=std::min(VLIM(j,0),VLIM(j,1));
                }
            }
        }

        return VLIM;
    }
示例#6
0
    Matrix getVLIM(const Obstacle &obstacle, const Matrix &v_lim)
    {
        Vector xo=obstacle.getPosition();
        deque<Vector> ctrlPoints=getCtrlPointsPosition();
        double sNorm = 1.0;
        double sScalingGain = 4.0;

        Matrix VLIM=v_lim;
        //yDebug("AvoidanceHandlerVisuo::getVLIM: adapting VLIM: \n");
        for (size_t i=0; i<ctrlPoints.size(); i++)
        {
            Vector dist=xo-ctrlPoints[i];
            Vector distNormalized(3,0.0);
            double d_norm=norm(dist);
            if (d_norm>=obstacle.radius)
            {
                dist*=1.0-obstacle.radius/d_norm; //the distance vector is refactored to account for the real distance between the control point and the obstacle surface
                d_norm=norm(dist);
                distNormalized = dist / d_norm;
            }
            else
            {
                dist=0.0;
                distNormalized = 0.0;
                d_norm=0.0;
            }
            
            double f=1.0/(1.0+exp((d_norm*(2.0/rho)-1.0)*alpha));
            Matrix J=chainCtrlPoints[i]->GeoJacobian().submatrix(0,2,0,chainCtrlPoints[i]->getDOF()-1);
            Vector s=J.transposed()*(distNormalized); //Matej: adding the normalization of distance- Eq. 13 in Flacco

            yAssert((f>=0.0) && (f<=1.0));
            //printf("    control point %d:  collision risk (f): %f, \n   J: \n %s \ns = J.transposed * distNormalized\n   (%s)T = \n (%s) * \n (%s)T\n",i,f,J.toString(3,3).c_str(),s.toString(3,3).c_str(),J.transposed().toString(3,3).c_str(),distNormalized.toString(3,3).c_str());
            Vector rowNorms(J.rows(),0.0);
        
            if(scalingBySnorm){
                sNorm = norm(s);
                //printf("norm s: %f \n",norm(s));
            }
            for (size_t j=0; j<s.length(); j++)
            {
                //printf("        Joint: %d, s[j]: %f, limits before: Min: %f, Max: %f\n",j,s[j],VLIM(j,0),VLIM(j,1));
                if (s[j]>=0.0)
                {
                    if(scalingBySnorm){
                        sScaling = std::min(1.0,abs(s[j]) / sNorm * sScalingGain);
                        //printf("            s>=0 clause, scaling of f term min(1, abs(s[j])/sNorm*sScalingGain)  = %f = min(1.0, %f * %f) = min(1.0,%f)\n",sScaling,abs(s[j])/sNorm,sScalingGain,abs(s[j])/sNorm*sScalingGain);
                    }
                    double tmp=v_lim(j,1)*(1.0 - f*sScaling);
                    //printf("        New max limit candidate: %f = %f * (1 - %f * %f) = %f * (1-%f),",tmp,v_lim(j,1),f,sScaling,v_lim(j,1),f*sScaling);
                    VLIM(j,1)=std::min(VLIM(j,1),tmp);
                    VLIM(j,0)=std::min(VLIM(j,0),VLIM(j,1));
                    //printf("            s>=0 clause, limits after: Min: %f, Max: %f\n",VLIM(j,0),VLIM(j,1));
                }
                else
                {
                    if(scalingBySnorm){
                        sScaling = std::min(1.0,abs(s[j]) / sNorm * sScalingGain);
                        //printf("        s<0 clause, scaling of f term min(1, abs(s[j])/sNorm*sScalingGain)  = %f = min(1.0, %f * %f) = min(1.0,%f)\n",sScaling,abs(s[j])/sNorm,sScalingGain,abs(s[j])/sNorm*sScalingGain);
                    }
                    double tmp=v_lim(j,0)*(1.0 - f*sScaling);
                    //printf("            New min limit candidate: %f = %f * (1 - %f * %f) = %f * (1-%f),",tmp,v_lim(j,0),f,sScaling,v_lim(j,0),f*sScaling);
                    VLIM(j,0)=std::max(VLIM(j,0),tmp);
                    VLIM(j,1)=std::max(VLIM(j,0),VLIM(j,1));
                    //printf("            s<0 clause, limits after: Min: %f, Max: %f\n",VLIM(j,0),VLIM(j,1));
                }
            }
        }

        return VLIM;
    }
// Is not called periodically from a timer slot
void ActorHighLevel::startPIDController()
{
    while(enabled)
    {
        // Roboterposition abspeichern
        robotPosition = MapData::getRobotPosition(ObstacleType::ME);

        // we dont have a position right now -> try again in 50ms
        if(robotPosition.certainty() == 0.0) {
            QThread::msleep(50);
            // Process Qt Event Queue and wait a little
            QCoreApplication::processEvents();
            QCoreApplication::sendPostedEvents();

            continue;
        }

        // Zeit seit letzter Berechnung bestimmen, um korrekt zu integrieren / differenzieren
        double elapsedMs = elapsedTime->restart();
        double timeSinceStart = (QDateTime::currentMSecsSinceEpoch() / 1000.0) - timeOfStart;

        switch (this->getState()) {
        case StatePathProcessing::GATHER_PUCK:
        {
            double deltaX = releasePuckOrigin.x()- robotPosition.x();
            double deltaY = releasePuckOrigin.y()- robotPosition.y();
            double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz
            double deltaL = config::actorGatherPuckDistance - dist; // Distanz zur Sollposition

            // Sind wir weit genug gefahren?
            if(dist >= config::actorGatherPuckDistance) {
                if(config::enableDebugActorHighLevel)
                    qDebug() << "Puck gathered, weil dist == " << dist << " >= config::actorGatherPuckDistance == " << config::actorGatherPuckDistance;
                resetPIDtempVars();
                this->setState(StatePathProcessing::RUNNING);
                Q_EMIT signalSendRobotControlParams(0, 0);
                Q_EMIT signalPuckDone(); // Signal an KI, dass Gathering fertig ist
                break;
            }

            // PID Regler Abstand
            double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
            lastDeltaL = deltaL;
            iDeltaL += deltaL * elapsedMs;
            iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
            // Sollgeschwindigkeit
            double robotV = +1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL);

            // Daten zu history hinzufuegen fuer spaeteren plot
            if(ActorHighLevel::streamPIDEnabled) {
                mutexPidHist->lock();
                pidHistTime.append(timeSinceStart);
                pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
                pidHistWinkelSoll.append(0.0);
                pidHistDistIst.append(dist);
                pidHistDistSoll.append(deltaL);
                mutexPidHist->unlock();
            }

            // Emit the calculated control values to the low level actor module
            Q_EMIT signalSendRobotControlParams(robotV, 0.0);

            break;

        }
        case StatePathProcessing::PUSH_AND_RELEASE_PUCK:
        {
            double deltaX = releasePuckOrigin.x() - robotPosition.x();
            double deltaY = releasePuckOrigin.y() - robotPosition.y();
            double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz

            // 25cm (NEIN 20cm) minus das bereits gefahrene
            ///\laurenz hier sollte doch 20cm unten und oben stehen?
            double deltaL = /*config::actorReleasePuckDistance*/
                            config::actorPushPuckDistance
                            - dist; // Distanz zur Sollposition

            // hier wird gefragt ob wir mehr als 20cm gefahren sind

            // Sind wir weit genug gefahren?
            if(dist >= config::actorPushPuckDistance) {
                if(config::enableDebugActorHighLevel)
                    qDebug() << "Puck pushed, weil dist =" << dist;

                resetPIDtempVars();
                // nehme wieder die tatsächlich aktuelle position
                releasePuckOrigin = MapData::getRobotPosition(ObstacleType::ME);
                this->setState(StatePathProcessing::RELEASE_PUCK_FROM_PUSH);

                if(config::enableDebugActorHighLevel)
                    qDebug() << "Starting Puck release";

                Q_EMIT signalSendRobotControlParams(0, 0);
                break; // springe raus
            }

            // PID Regler Abstand
            double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
            lastDeltaL = deltaL;
            iDeltaL += deltaL * elapsedMs;
            iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
            double robotV = +1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL); // Sollgeschwindigkeit

            // Daten zu history hinzufuegen fuer spaeteren plot
            if(ActorHighLevel::streamPIDEnabled) {
                mutexPidHist->lock();
                pidHistTime.append(timeSinceStart);
                pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
                pidHistWinkelSoll.append(0.0);
                pidHistDistIst.append(dist);
                pidHistDistSoll.append(deltaL);
                mutexPidHist->unlock();
            }

            // Emit the calculated control values to the low level actor module
            Q_EMIT signalSendRobotControlParams(robotV, 0.0);

            break;
        }
        case StatePathProcessing::RELEASE_PUCK_FROM_PUSH:
        {
            double deltaX = releasePuckOrigin.x() - robotPosition.x();
            double deltaY = releasePuckOrigin.y() - robotPosition.y();
            double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz

            // (additional distance is 20 or 0) + 25cm + 2,5 cm
            // Carlo: habe die unterscheidung eingebaut um den puck correct ins tor zu legen
            // und den beim DUMP trotzdem weit genug zurück zu fahren
            double distanceToGoBack = additionalReleasePuckDistance +
                                      config::actorReleasePuckDistance +
                                      config::actorPushAndReleaseAdditionalReverseDist;

            // 25cm + 20cm + 2,5 cm
            double deltaL = distanceToGoBack - dist; // Distanz zur Sollposition

            // Sind wir weit genug gefahren?
            if(dist >= distanceToGoBack)
            {
                if(config::enableDebugActorHighLevel)
                    qDebug() << "Puck released, weil dist =" << dist;
                resetPIDtempVars();
                this->setState(StatePathProcessing::RUNNING);
                Q_EMIT signalSendRobotControlParams(0, 0);
                Q_EMIT signalPuckDone(); // Signal an KI, dass Release fertig ist
                break;
            }

            // PID Regler Abstand
            double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
            lastDeltaL = deltaL;
            iDeltaL += deltaL * elapsedMs;
            iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);

            // Sollgeschwindigkeit
            double robotV = -1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL);

            // Daten zu history hinzufuegen fuer spaeteren plot
            if(ActorHighLevel::streamPIDEnabled) {
                mutexPidHist->lock();
                pidHistTime.append(timeSinceStart);
                pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
                pidHistWinkelSoll.append(0.0);
                pidHistDistIst.append(dist);
                pidHistDistSoll.append(deltaL);
                mutexPidHist->unlock();
            }

            // Emit the calculated control values to the low level actor module
            Q_EMIT signalSendRobotControlParams(robotV, 0.0);

            break;
        }
        case StatePathProcessing::RELEASE_PUCK:
        {
            double deltaX = releasePuckOrigin.x() - robotPosition.x();
            double deltaY = releasePuckOrigin.y() - robotPosition.y();
            double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz
            double deltaL = config::actorReleasePuckDistance - dist; // Distanz zur Sollposition

            // Sind wir weit genug gefahren?
            if(dist >= config::actorReleasePuckDistance) {
                if(config::enableDebugActorHighLevel)
                    qDebug() << "Puck released, weil dist =" << dist;

                resetPIDtempVars();
                this->setState(StatePathProcessing::RUNNING);
                Q_EMIT signalSendRobotControlParams(0, 0);
                Q_EMIT signalPuckDone(); // Signal an KI, dass Release fertig ist
                break;
            }

            // PID Regler Abstand
            double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
            lastDeltaL = deltaL;
            iDeltaL += deltaL * elapsedMs;
            iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);

            // Sollgeschwindigkeit
            double robotV = -1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL);

            // Daten zu history hinzufuegen fuer spaeteren plot
            if(ActorHighLevel::streamPIDEnabled) {
                mutexPidHist->lock();
                pidHistTime.append(timeSinceStart);
                pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
                pidHistWinkelSoll.append(0.0);
                pidHistDistIst.append(dist);
                pidHistDistSoll.append(deltaL);
                mutexPidHist->unlock();
            }

            // Emit the calculated control values to the low level actor module
            Q_EMIT signalSendRobotControlParams(robotV, 0.0);

            break;
        }
        case StatePathProcessing::RUNNING:
        {
            // Wenn es keine Wegpunkte gibt
            if(internalWP.isEmpty() || MapData::getObstacle(ObstacleType::TARGET).isEmpty()) {
                // Leave the state
                this->setState(StatePathProcessing::STOP);
                break;
            }

            // Turn off primitive collision avoidance
            MapData::setDisableEmergency(true);

            // Wenn der Roboter auf dem letzten Wegpunkt steht
            Obstacle target = MapData::getFirstTarget();

            bool targetHasOrientation = !std::isnan(target.getOrientation()) && !std::isinf(target.getOrientation());
            double targetDist = robotPosition.getDistanceTo(target.getPosition());
            double targetDistDiff = targetDist - targetDistLast;
            targetDistLast = targetDist;
            if(targetDistDiff == 0) targetDistDiff = targetDistDiffLast;
            targetDistDiffLast = targetDistDiff;
            bool targetSeemsReached = targetDistDiff > config::actorWaypointReachedDiffChange && targetDist <= config::actorWaypointReachedDistance;
            if( positionWasReached || targetSeemsReached ) {
                // Roboter ist/war an Position

                if(positionWasReached == false){
                    qDebug() << "Ziel erreicht mit Abstand" << targetDist << " - Stelle jetzt Winkel perfekt ein";
                }

                positionWasReached = true; // Roboter hat das Ziel erreicht. Wenn das Ziel davonwackelt, ist es immernoch erreicht.

                // Abweichung vom einzustellenden Winkel (kann undefinierte Werte annehmen, falls das Target keinen Winkel besitzt)
                double angleDeviation = fabs(target.getOrientation() - robotPosition.rot());
                if(std::isnan(angleDeviation)) angleDeviation = 0;
                while(angleDeviation > M_PI) angleDeviation -= M_PI; // Die Winkelabweichung darf nie über 180° sein

                if(!targetHasOrientation || (targetHasOrientation && angleDeviation <= config::actorWaypointMaxAngleDeviation ) ) {
                    // Winkel ist OK
                    if(config::enableDebugActorHighLevel)
                        qDebug() << "Ziel erreicht mit Winkelabweichung" << (angleDeviation/M_PI*180.0) << "°";

                    positionWasReached = false; // Zurücksetzen, weil als nächstes ein neues Ziel kommt
                    MapData::deleteFirstTarget(); // Delete this target
                    this->setState(StatePathProcessing::STOP); // Leave the state
                    Q_EMIT signalSendRobotControlParams(0.0, 0.0); // Zur Sicherheit Stop senden
                    break;
                } else {
                    if(config::enableDebugActorHighLevel)
                        qDebug() << "Winkel noch nicht OK - Abweichung " << (angleDeviation/M_PI*180.0) << "°";
                }
            }

            // Nähesten Punkt auf dem Spline finden.
            double minP=0, minL=9999;
            for(double p = 0; p<numWP; p+=0.001){ // Sicher wär ne Newton Iteration oder so besser, aber so gehts auch.
                double deltaX = splineX(p) - robotPosition.x();
                double deltaY = splineY(p) - robotPosition.y();
                double deltaL = sqrt(deltaX*deltaX+deltaY*deltaY); // Distanz des Roboters zu diesem Punkt auf dem Spline
                if(deltaL < minL){
                    minP = p;
                    minL = deltaL;
                }
            }

            // Die Sollposition ist vom nähesten Spline Punkt 0.5m weiter auf dem Spline entfernt, jedoch nie weiter weg als das Ziel selbst
            double progressOnSpline = minP + config::actorDistanceOfTargetOnSpline;
            double sollX, sollY;// Sollposition
            if(progressOnSpline <= numWP) {
                sollX = splineX(progressOnSpline);
                sollY = splineY(progressOnSpline);
            } else {
                sollX = target.getCoords().first;
                sollY = target.getCoords().second;
            }
            double deltaX = sollX - robotPosition.x(); // Abweichung von der Sollposition
            double deltaY = sollY - robotPosition.y(); // Abweichung von der Sollposition
            double deltaL = sqrt(deltaX*deltaX+deltaY*deltaY); // Distanz zur Sollposition

            double sollA;
            if(targetHasOrientation && deltaL < config::actorWaypointReachedDistance) { // Roboter ist im Prinzip schon da und muss sich noch vernünftig ausrichten
                sollA = /*constrainAngle*/(target.getOrientation()); // Vorgegebener Anfahrtswinkel
            } else { // Roboter muss beim Fahren in Fahrtrichtung gucken
                sollA = /*constrainAngle*/(atan2(deltaY, deltaX)); // Winkel zur Sollposition
            }
            double deltaA = constrainAngle(sollA) - constrainAngle(robotPosition.rot()); // Abweichung vom Winkel zur Sollposition
             deltaA = constrainAngle(deltaA);

            // PID Regler Rotation(sgeschwindigkeit)
//            deltaA = SensorLowLevel::angleWeightedAverage(deltaA, config::actorLowPass, lastDeltaA, 1-config::actorLowPass); // Tiefpassfilter
            double dDeltaA = (elapsedMs)? (deltaA-lastDeltaA)/elapsedMs : 0; // Differenzialanteil
            lastDeltaA = deltaA;
            iDeltaA += deltaA * elapsedMs; // Integralanteil
            iDeltaA = qMin(qMax(iDeltaA, -config::actorMaxI), config::actorMaxI); // Begrenzen auf -10...+10
            double robotR = PID_A_P * deltaA + PID_A_I * iDeltaA + PID_A_D * dDeltaA; // Solldrehgeschwindigkeit

            if(config::enableDebugActorHighLevel)
                qDebug() << "deltaA="<<deltaA << ", iDeltaA=" << iDeltaA << ", dDeltaA=" << dDeltaA << "robotR=" << robotR;

            // PID Regler Abstand
            double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
            lastDeltaL = deltaL;
            iDeltaL += deltaL * elapsedMs;
            iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
            double robotV = PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL; // Sollgeschwindigkeit

            // Vorwärtsgeschwindigkeit reduzieren, wenn Winkelabweichung zu groß
            double angleLimiter = 1;
            if(fabs(deltaA) > config::actorMinAngleLimiter && fabs(deltaA) < config::actorMaxAngleLimiter) {
                angleLimiter = 1 - (fabs(deltaA) - config::actorMinAngleLimiter) / (config::actorMaxAngleLimiter - config::actorMinAngleLimiter); // 0...1
            } else if ( fabs(deltaA) > config::actorMaxAngleLimiter ) {
                angleLimiter = 0;
            }
            robotV = robotV * angleLimiter;

            // Wenn es nur noch um die Drehung geht, gar nicht vorwärts fahren
            if(positionWasReached){
                robotV = 0.0;
            }

            // if enemy position is next to target -> wait
            Position enemyPosition = MapData::getRobotPosition(ObstacleType::OPPONENT);

            if(enemyPosition.isConsimilarTo(target.getPosition(), 2.5 * config::SENSOR_RADIUS_ROBOT))
            {
                robotV = 0;
                robotR = 0;

                resetPIDtempVars();
            }

            // Emit the calculated control values to the low level actor module
            Q_EMIT signalSendRobotControlParams(robotV,robotR);

            // Daten zu history hinzufuegen fuer spaeteren plot
            if(ActorHighLevel::streamPIDEnabled) {
                mutexPidHist->lock();
                pidHistTime.append(timeSinceStart);
                pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
                pidHistWinkelSoll.append(sollA);
                pidHistDistIst.append(robotV);
                pidHistDistSoll.append(deltaL);
                mutexPidHist->unlock();
            }
            break;
        }
        case StatePathProcessing::STOP:
        {
            // Clear remaining waypoints
            internalWP.clear();

            // Stop robot motion
            Q_EMIT signalSendRobotControlParams(0.0, 0.0);

            // (Re-)Enable primitive collision avoidance in manual control mode
            MapData::setDisableEmergency(false);

            // avoid looping in here again all the time
            enabled = false;

            break;
        }
        } // switch

        // Process Qt Event Queue and wait a little
        QCoreApplication::processEvents();
        QCoreApplication::sendPostedEvents();

        // we dont want 100% cpu time for the PID
        QThread::msleep(10);
    }
}