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