void ActorHighLevel::slotUpdateWaypoints(QList< QPair<double,double> > waypoints) { // signals will be ignored, if quitting is true if (!quitting) { //wenn ein anderer Status activ ist, soll dieser nicht gestört werden switch(this->getState()) { case StatePathProcessing::STOP: case StatePathProcessing::RUNNING: { internalWP = waypoints; numWP = internalWP.length(); // Anhalten, falls es nicht genug Wegpunkte gibt if(numWP == 0) { this->setState(StatePathProcessing::STOP); enabled = true; this->startPIDController(); return; } else { this->setState(StatePathProcessing::RUNNING); } } break; default: // jeder andere Status soll beendet werden { // nichts machen! return; } // default } // switch // Ein Spline mit zwei Stützpunkten kann nicht generiert werden, schummele einen mittleren dazu. if(numWP == 1){ double midX = (internalWP.first().first + internalWP.last().first )/2; double midY = (internalWP.first().second + internalWP.last().second)/2; internalWP.insert(1, QPair<double,double> (midX, midY)); ++numWP; } // Wegpunkte Tiefpassfiltern QVector<double> wpLowPassX = lowPass(takeDimension(internalWP.toVector(), 0), config::actorWPLowPassAlpha); QVector<double> wpLowPassY = lowPass(takeDimension(internalWP.toVector(), 1), config::actorWPLowPassAlpha); // Spline Metrik erzeugen QVector<double> splineMetric(numWP); for(int i=0; i<numWP; ++i){ splineMetric[i] = static_cast<double>(i); } // Spline generieren splineX.set_points(splineMetric, wpLowPassX); splineY.set_points(splineMetric, wpLowPassY); // Send generated spline to path display tab if(PathPlanning::streamPathEnabled) { static const int numSplinePoints = 100; PathPlotData dataPacket; dataPacket.dataType = PathPlotData::SPLINE; dataPacket.splineLength = numWP; QVector<double> splineVectX(numSplinePoints), splineVectY(numSplinePoints); for(int i=0; i<numSplinePoints; i++){ double progress = static_cast<double>(i*dataPacket.splineLength)/static_cast<double>(numSplinePoints-1); splineVectX[i] = splineX(progress); splineVectY[i] = splineY(progress); } dataPacket.splineX = splineVectX; dataPacket.splineY = splineVectY; Q_EMIT signalSplinePlot(dataPacket); } // Start timer (verwendet für berechnung der I und D Anteile) elapsedTime->restart(); // PID wieder starten enabled = true; this->startPIDController(); } }
// 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); } }
void HMMWV_ReissnerTire::CreateMesh(const ChFrameMoving<>& wheel_frame, VehicleSide side) { // Create piece-wise cubic spline approximation of the tire profile. // x - radial direction // y - transversal direction ChCubicSpline splineX(m_profile_t, m_profile_x); ChCubicSpline splineY(m_profile_t, m_profile_y); // Create the mesh nodes. // The nodes are first created in the wheel local frame, assuming Y as the tire axis, // and are then transformed to the global frame. for (int i = 0; i < m_div_circumference; i++) { double phi = (CH_C_2PI * i) / m_div_circumference; ChVector<> nrm(-std::sin(phi), 0, std::cos(phi)); for (int j = 0; j <= m_div_width; j++) { double t_prf = double(j) / m_div_width; double x_prf, xp_prf, xpp_prf; double y_prf, yp_prf, ypp_prf; splineX.Evaluate(t_prf, x_prf, xp_prf, xpp_prf); splineY.Evaluate(t_prf, y_prf, yp_prf, ypp_prf); // Node position with respect to rim center double x = (m_rim_radius + x_prf) * std::cos(phi); double y = y_prf; double z = (m_rim_radius + x_prf) * std::sin(phi); // Node position in global frame (actual coordinate values) ChVector<> loc = wheel_frame.TransformPointLocalToParent(ChVector<>(x, y, z)); // Node direction ChVector<> tan_prf(std::cos(phi) * xp_prf, yp_prf, std::sin(phi) * xp_prf); ChVector<> nrm_prf = Vcross(tan_prf, nrm).GetNormalized(); ChVector<> dir = wheel_frame.TransformDirectionLocalToParent(nrm_prf); ChMatrix33<> mrot; mrot.Set_A_Xdir(tan_prf,nrm_prf); auto node = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(loc, mrot)); // Node velocity ChVector<> vel = wheel_frame.PointSpeedLocalToParent(ChVector<>(x, y, z)); node->SetPos_dt(vel); node->SetMass(0); m_mesh->AddNode(node); } } // Create the Reissner shell elements for (int i = 0; i < m_div_circumference; i++) { for (int j = 0; j < m_div_width; j++) { // Adjacent nodes int inode0, inode1, inode2, inode3; inode1 = j + i * (m_div_width + 1); inode2 = j + 1 + i * (m_div_width + 1); if (i == m_div_circumference - 1) { inode0 = j; inode3 = j + 1; } else { inode0 = j + (i + 1) * (m_div_width + 1); inode3 = j + 1 + (i + 1) * (m_div_width + 1); } auto node0 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode0)); auto node1 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode1)); auto node2 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode2)); auto node3 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode3)); // Create the element and set its nodes. auto element = std::make_shared<ChElementShellReissner4>(); element->SetNodes(node0, node1, node2, node3); // Element dimensions double len_circumference = 0.5 * ((node1->GetPos() - node0->GetPos()).Length() + (node3->GetPos() - node2->GetPos()).Length()); double len_width = (node2->GetPos() - node0->GetPos()).Length(); // Figure out the section for this element int b1 = m_num_elements_bead; int b2 = m_div_width - m_num_elements_bead; int s1 = b1 + m_num_elements_sidewall; int s2 = b2 - m_num_elements_sidewall; if (j < b1 || j >= b2) { // Bead section for (unsigned int im = 0; im < m_num_layers_bead; im++) { element->AddLayer(m_layer_thickness_bead[im], CH_C_DEG_TO_RAD * m_ply_angle_bead[im], m_materials[m_material_id_bead[im]]); } } else if (j < s1 || j >= s2) { // Sidewall section for (unsigned int im = 0; im < m_num_layers_sidewall; im++) { element->AddLayer(m_layer_thickness_sidewall[im], CH_C_DEG_TO_RAD * m_ply_angle_sidewall[im], m_materials[m_material_id_sidewall[im]]); } } else { // Tread section for (unsigned int im = 0; im < m_num_layers_tread; im++) { element->AddLayer(m_layer_thickness_tread[im], CH_C_DEG_TO_RAD * m_ply_angle_tread[im], m_materials[m_material_id_tread[im]]); } } // Set other element properties element->SetAlphaDamp(m_alpha); // Add element to mesh m_mesh->AddElement(element); } } // Switch on automatic gravity m_mesh->SetAutomaticGravity(true); }