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