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