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