/* * Adjusts the desired velocity based on output from the proximity sensor */ void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel) { // exit immediately if proximity sensor is not present if (_proximity.get_status() != AP_Proximity::Proximity_Good) { return; } // exit immediately if no desired velocity if (desired_vel.is_zero()) { return; } // normalise desired velocity vector Vector2f vel_dir = desired_vel.normalized(); // get angle of desired velocity float heading_rad = atan2f(vel_dir.y, vel_dir.x); // rotate desired velocity angle into body-frame angle float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw); // get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame) float distance_m; if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) { limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f)); } }
void RRTWidget::drawTerminalState(QPainter &painter, const Vector2f &pos, const Vector2f &vel, const QColor &color) { // draw point painter.setPen(QPen(color, 6)); QPointF rootLoc(pos.x(), pos.y()); painter.drawEllipse(rootLoc, 2, 2); Vector2f tipOffset = vel * VelocityDrawingMultiplier; Vector2f tipLocVec = pos + tipOffset; QPointF tipLoc(tipLocVec.x(), tipLocVec.y()); // draw arrow shaft painter.setPen(QPen(color, 3)); painter.drawLine(rootLoc, tipLoc); // draw arrow head Vector2f headBase = tipLocVec - tipOffset.normalized()*4; Vector2f perp = Vector2f(-tipOffset.y(), tipOffset.x()).normalized(); Vector2f tipLeftVec = headBase + perp*4; Vector2f tipRightVec = headBase - perp*4; QPointF trianglePts[] = { tipLoc, QPointF(tipLeftVec.x(), tipLeftVec.y()), QPointF(tipRightVec.x(), tipRightVec.y()) }; painter.drawPolygon(trianglePts, 3); }
// calculate vehicle stopping point using current location, velocity and maximum acceleration bool AR_WPNav::get_stopping_location(Location& stopping_loc) { Location current_loc; if (!AP::ahrs().get_position(current_loc)) { return false; } // get current velocity vector and speed const Vector2f velocity = AP::ahrs().groundspeed_vector(); const float speed = velocity.length(); // avoid divide by zero if (!is_positive(speed)) { stopping_loc = current_loc; return true; } // get stopping distance in meters const float stopping_dist = _atc.get_stopping_distance(speed); // calculate stopping position from current location in meters const Vector2f stopping_offset = velocity.normalized() * stopping_dist; stopping_loc = current_loc; stopping_loc.offset(stopping_offset.x, stopping_offset.y); return true; }
bool MidCirclePerceptor::searchWithSXAndT(MidCircle& midCircle) const { std::vector<const FieldLineIntersections::Intersection*> useSmallXIntersections; for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections) if(intersection.type == FieldLineIntersections::Intersection::X && intersection.additionalType == FieldLineIntersections::Intersection::none) useSmallXIntersections.push_back(&intersection); if(useSmallXIntersections.empty()) return false; static const float distance = theFieldDimensions.yPosLeftSideline - theFieldDimensions.centerCircleRadius ; std::vector<const FieldLineIntersections::Intersection*> useTIntersections; for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections) if(intersection.type == FieldLineIntersections::Intersection::T) useTIntersections.push_back(&intersection); for(const FieldLineIntersections::Intersection* intersectionSX : useSmallXIntersections) for(const FieldLineIntersections::Intersection* intersectionT : useTIntersections) if(intersectionT->indexDir1 == intersectionSX->indexDir1 || intersectionT->indexDir1 == intersectionSX->indexDir2) if(std::abs((intersectionT->pos - intersectionSX->pos).norm() - distance) < allowedTsXVariance) { const Vector2f dirTToX = intersectionSX->pos - intersectionT->pos; midCircle.translation = intersectionSX->pos + dirTToX.normalized(theFieldDimensions.centerCircleRadius); midCircle.rotation = Angle(dirTToX.angle() + pi_2).normalize();// OR intersectionT->dir2.angle(); midCircle.markedPoints.emplace_back(midCircle.translation, MarkedPoint::midCircle); theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionT->ownIndex, MarkedIntersection::BT), theFieldLineIntersections, theFieldLines, midCircle); return true; } return false; }
//---------------------------------------------------------------------------------------- void Entity::update(float deltaSeconds) { Vector2f velocity = m_target - m_position; if (!(velocity == Vector2f(0,0))) { velocity = velocity.normalized(); m_position = m_position + (velocity * deltaSeconds * 50.f); } }
Obstacle::Obstacle(const Matrix2f& covariance, const Vector2f& center, unsigned int lastSeen, Type type) : covariance(covariance), center(center), velocity(Vector2f::Zero()), lastSeen(lastSeen), type(type) { float radius = 55.f; if(type != Obstacle::goalpost) radius = getRobotDepth(); else if(Blackboard::getInstance().exists("FieldDimensions")) radius = static_cast<const FieldDimensions&>(Blackboard::getInstance()["FieldDimensions"]).goalPostRadius; left = right = center.normalized(radius); left.rotateLeft(); right.rotateRight(); left += center; right += center; }
MTV getMTV_circ_circ(Game_Object &circle0, Game_Object &circle1){ MTV result; float overlap = 5000.0f; Vector2f smallest; const Vector2f dist_vec = (circle0.get_position() + 0.5f * circle0.get_size()) - (circle1.get_position() + 0.5f * circle1.get_size()); const float dist2 = dist_vec * dist_vec; const float radius_sum = circle0.get_radius() + circle1.get_radius(); if (dist2 < radius_sum * radius_sum) { overlap = radius_sum - sqrt(dist2); smallest = dist_vec.normalized(); result = MTV(true, smallest, overlap); } return result; }
// calculate vehicle stopping point using current location, velocity and maximum acceleration void Mode::calc_stopping_location(Location& stopping_loc) { // default stopping location stopping_loc = rover.current_loc; // get current velocity vector and speed const Vector2f velocity = ahrs.groundspeed_vector(); const float speed = velocity.length(); // avoid divide by zero if (!is_positive(speed)) { stopping_loc = rover.current_loc; return; } // get stopping distance in meters const float stopping_dist = attitude_control.get_stopping_distance(speed); // calculate stopping position from current location in meters const Vector2f stopping_offset = velocity.normalized() * stopping_dist; location_offset(stopping_loc, stopping_offset.x, stopping_offset.y); }