void CalibratedServo::setTemperature(uint8_t temperature) { static uint8_t lastTemp = 0; static unsigned long detatchTime = 0; if (temperature > getMaxTemperature()) temperature = getMaxTemperature(); if (temperature < getMinTemperature()) temperature = getMinTemperature(); lastTemperature = temperature; int deltaTemp = getMaxTemperature() - getMinTemperature(); int deltaAngle = getMaxAngle() - getMinAngle(); if (deltaTemp == 0) return; int angle = ((((int)temperature - (int)getMinTemperature()) * (int)deltaAngle) / (int)deltaTemp) + getMinAngle(); if (temperature != lastTemp) { detatchTime = millis() + 1500ul; lastTemp = temperature; attach(pin); } else if (attached() && (millis() > detatchTime)) { detach(); } if (angle <= getMaxAngle() && angle >= getMinAngle()) write(180-angle); }
void AngleRadiusWidget::setMaxAngle(double angle) { if (maxAngle_ != angle) { maxAngle_ = angle; // Update angle to be within bounds setAngle(std::min(getAngle(), getMaxAngle())); update(); emit angleMinMaxChanged(); } }
void AngleRadiusWidget::setMinMaxAngle(double minAngle, double maxAngle) { if (minAngle_ != minAngle || maxAngle_ != maxAngle) { minAngle_ = minAngle; maxAngle_ = maxAngle; // Update radius to be within bounds setAngle(glm::clamp(getAngle(), getMinAngle(), getMaxAngle())); update(); emit angleMinMaxChanged(); } }
/* * Subdivide one triangle to four triangles when it has angles to the center are larger than 45.0 dereees. */ vector< vector<cv::Point3d> > Triangle::subdivide(vector<cv::Point3d> vertices){ double max_angle = getMaxAngle(vertices); vector < vector<cv::Point3d> > triangle_list; cv::Point3d p01, p12, p02; if(max_angle > m_MAX_ANGLE){ if(vertices.size() == 3){ p01 = midPoint(vertices[0], vertices[1]); p12 = midPoint(vertices[1], vertices[2]); p02 = midPoint(vertices[0], vertices[2]); } // Lower left sub-triangle vector<cv::Point3d> tri0; tri0.push_back(vertices[0]); tri0.push_back(p01); tri0.push_back(p02); triangle_list.push_back(tri0); // top sub-triangle vector<cv::Point3d> tri1; tri1.push_back(p01); tri1.push_back(vertices[1]); tri1.push_back(p12); triangle_list.push_back(tri1); // Lower right sub-triangle vector<cv::Point3d> tri2; tri2.push_back(p02); tri2.push_back(p12); tri2.push_back(vertices[2]); triangle_list.push_back(tri2); // center sub-triangle vector<cv::Point3d> tri3; tri3.push_back(p01); tri3.push_back(p12); tri3.push_back(p02); triangle_list.push_back(tri3); }else{ triangle_list.push_back(vertices); } return triangle_list; }
/* * Obtain the field of the view in the perspective camera * points: vertices in 3D unit sphere * vd: viewing direction of the camera pointing to the center of the vertices. * * h_fov, v_fov: field of view * return: adjuested new viewing direction. */ ViewDirection Triangle::getFOV(vector<Point3d> points, ViewDirection vd, Point3d center, double &h_fov, double &v_fov){ // measure panning by vector angle. bool debug_flag = true; double max_theta = getMaxAngle(points); if(debug_flag){ if(max_theta > m_MAX_ANGLE){ double theta_deg = max_theta/M_PI*180.0; cout << "Triangle is too big: " << theta_deg << " degrees.\n"; } } h_fov = max_theta * 3.0; v_fov = max_theta * 3.0; }
void AngleRadiusWidget::paintEvent(QPaintEvent *) { const int referenceRadius = getMaxPixelSpaceRadius(); const double pixelSpaceRadius = referenceRadius*(getRadius()/getMaxRadius()); const qreal angleIndicatorCircleDiameter = 6.; QPen angleIndicatorPen(palette().midlight(), 2, Qt::SolidLine, Qt::SquareCap, Qt::MiterJoin); QPen coordinateSystemPen(palette().alternateBase(), 1, Qt::SolidLine, Qt::SquareCap, Qt::MiterJoin); QPen circleBoundsPen(palette().alternateBase(), 1, Qt::DashLine, Qt::SquareCap, Qt::MiterJoin); // x and y in pixel coordinates auto x = pixelSpaceRadius*std::cos(getAngle()); auto y = -pixelSpaceRadius*std::sin(getAngle()); int side = qMin(width(), height()); QStylePainter painter(this); painter.setRenderHint(QPainter::Antialiasing); painter.translate(width() / 2, height() / 2); painter.scale(side / (qreal)100., side / (qreal)100.); painter.setPen(coordinateSystemPen); // Draw axes and bounds circle(s) for (int i = 0; i < 4; ++i) { painter.drawLine(0, 0, referenceRadius+5, 0); painter.rotate(90.0); } painter.setPen(circleBoundsPen); // Display angle bounds by drawing a pie (pacman) if min/max is not 0/2pi int innerBoundsCircleRadius = static_cast<int>(static_cast<double>(referenceRadius)*(getMinRadius()/getMaxRadius())); if (getMinAngle() > 0. || getMaxAngle() < 2.*M_PI) { // drawPie wants 16*degrees int pieStart = static_cast<int>( 16*glm::degrees(getMinAngle()) ); int pieEnd = static_cast<int>( 16*(glm::degrees(getMaxAngle())-glm::degrees(getMinAngle())) ); painter.drawPie(-referenceRadius, -referenceRadius, 2*referenceRadius, 2*referenceRadius, pieStart, pieEnd); if (minRadius_ > 0.) { painter.drawPie(-innerBoundsCircleRadius, -innerBoundsCircleRadius, 2*innerBoundsCircleRadius, 2*innerBoundsCircleRadius, pieStart, pieEnd); } } else { painter.drawEllipse(-referenceRadius, -referenceRadius, 2*referenceRadius, 2*referenceRadius); if (minRadius_ > 0.) { painter.drawEllipse(-innerBoundsCircleRadius, -innerBoundsCircleRadius, 2*innerBoundsCircleRadius, 2*innerBoundsCircleRadius); } } // Draw angle and radius painter.setPen(QPen(palette().text().color())); QPainterPath anglePath; // Make sure that angle goes from 0 to 2*pi double sweepAngle = getAngle() < 0. ? 2.*M_PI+getAngle() : getAngle(); // Draw angle arc and text anglePath.arcTo(-10., -10., 20., 20., 0., glm::degrees(sweepAngle)); painter.drawPath(anglePath); std::stringstream angleStream; angleStream.precision(1); angleStream << std::fixed << glm::degrees(sweepAngle); int anglePosX = 5; int anglePosY = -10; QFontMetrics fm(painter.font()); QString angleText = QString::fromStdString(angleStream.str())+ QChar(0260); int angleTextWidth = fm.width(angleText); if (anglePosX+angleTextWidth > referenceRadius) { anglePosX = referenceRadius+2; anglePosY = -2; } // 0260 is the degree symbol painter.drawText(anglePosX, anglePosY, angleText); // Draw radius text std::stringstream radiusStream; radiusStream.precision(2); radiusStream << getRadius(); painter.drawText(static_cast<int>(x+angleIndicatorCircleDiameter+angleIndicatorPen.width()), static_cast<int>(y), QString::fromStdString(radiusStream.str())); // Rotated line and circle painter.setPen(angleIndicatorPen); painter.drawLine(QLineF(QPointF(0., 0.), QPointF(x, y))); painter.setBrush(QBrush(palette().shadow().color(), Qt::SolidPattern)); painter.drawEllipse(QPointF(x, y), angleIndicatorCircleDiameter, angleIndicatorCircleDiameter); }
double Triangle::getMaxAngle(Vec9f triangle){ vector<Point3d> vertices = convVec9fToPoint3d(triangle); return getMaxAngle(vertices); }
/** * Given 2 lists of matched triangles, subdivides the triangles that are too big to make smaller * matched triangles */ void Triangle::subdivideMatched(vector<Vec9f> &triangle_list1, vector<Vec9f> &triangle_list2){ vector<Vec9f> new_list1, new_list2, tmp_new1, tmp_new2; // for(int k = 0;k < n;k++){ // repeat n times. // bool found_flag = false; new_list1 = triangle_list1; new_list2 = triangle_list2; bool exists_one = true; while(exists_one){ exists_one = false; vector<Vec9f> tmp_new_list1, tmp_new_list2; for(int j=0;j<new_list1.size();j++){ Vec9f vec1 = new_list1[j]; Vec9f vec2 = new_list2[j]; double max_angle1 = getMaxAngle(vec1); double max_angle2 = getMaxAngle(vec2); //Make a list of all points vector<Point3d> points; for(int n=0;n<9;n++){ Point3d p1,p2,p3; p1.x = vec1[0]; p1.y = vec1[1]; p1.z = vec1[2]; p2.x = vec1[3]; p2.y = vec1[4]; p2.z = vec1[5]; p3.x = vec1[6]; p3.y = vec1[7]; p3.z = vec1[8]; points.push_back(p1);points.push_back(p2);points.push_back(p3); p1.x = vec2[0]; p1.y = vec2[1]; p1.z = vec2[2]; p2.x = vec2[3]; p2.y = vec2[4]; p2.z = vec2[5]; p3.x = vec2[6]; p3.y = vec2[7]; p3.z = vec2[8]; points.push_back(p1);points.push_back(p2);points.push_back(p3); } double max_both = nPointsMaxAngle(points); if(max_angle1>m_MAX_ANGLE || max_angle2>m_MAX_ANGLE || max_both > m_MAX_ANGLE){ exists_one = true; tmp_new1 = forceSubdivide(vec1); tmp_new2 = forceSubdivide(vec2); for(int l=0;l<tmp_new1.size();l++){ tmp_new_list1.push_back(tmp_new1[l]); tmp_new_list2.push_back(tmp_new2[l]); } } else{ tmp_new_list1.push_back(vec1); tmp_new_list2.push_back(vec2); } } new_list1 = tmp_new_list1; new_list2 = tmp_new_list2; } //Get final resulting triangles triangle_list1 = new_list1; triangle_list2 = new_list2; }
// ----------------------------------------------------------------------------- // ----------------------------------------------------------------------------- void ChPitmanArm::Synchronize(double time, double steering) { auto fun = std::static_pointer_cast<ChFunction_Setpoint>(m_revolute->GetAngleFunction()); fun->SetSetpoint(getMaxAngle() * steering, time); }