// Convert a list of x/y points to a list of angle-angle-lingertime triples
void pointsToTriples(List <PointF> scaledPoints, float arm1Length, float arm2Length, PointF hubLocation, out List <angleTimeTriplet> triples)
{
triples = new List <angleTimeTriplet>();
#if CALIBRATION_ONLY
angleTimeTriplet att = new angleTimeTriplet();
att.arm1angle = att.arm2angle = (float)(Math.PI / 2); // 90 degree
att.milliseconds = 10000;
triples.Add(att);
return;
#else
// Add the first point to the list
angleTimeTriplet triple;
if (encodePoint(scaledPoints[0], hubLocation, arm1Length, arm2Length, out triple) == EncodingEnum.OK)
{
// The very first position the arms go to will allow a 10-second pause for inserting the pen
triple.linger = firstPointTimeMs;
triples.Add(triple);
}
// Then, generate lines between all the other point pairs and add them to the list
for (int i = 0; i < scaledPoints.Count - 1; ++i)
{
encodeLine(scaledPoints[i], scaledPoints[i + 1], hubLocation, arm1Length, arm2Length, ref triples);
}
#endif
// The last point should remain a long time also--to allow pen removal
angleTimeTriplet last = triples.Last();
last.linger = lastPointTimeMs;
triples.Add(last);
}
// Convert a point into an angle-angle-lingertime triple, given hublocation and arm lengths
private EncodingEnum encodePoint(PointF pt, PointF hubLocation, float arm1Length, float arm2length, out angleTimeTriplet triple)
{
triple = new angleTimeTriplet();
// First find the distance from the hub to the point
double dx = pt.X - hubLocation.X;
double dy = pt.Y - hubLocation.Y;
double vectorLength = Math.Sqrt(dx * dx + dy * dy);
// Since we now know the three sides of the triangle formed by the two arms and the vector,
// calculate the angle between the vector and the first arm using law of cosines
double angleArm1ToVector = Math.Acos((vectorLength * vectorLength + arm1Length * arm1Length - arm2length * arm2length) / (2 * vectorLength * arm1Length));
// If the point is too far away to be reached by the fully-extended arms, the value passed to acos will be greater than 1 (< -1)
if (double.IsNaN(angleArm1ToVector))
{
Console.WriteLine("Point too far: " + pt.X + "," + pt.Y);
badTooFar.Add(pt);
return(EncodingEnum.TOO_FAR);
}
// Then determine the angle between the baseline and the vector
double angleBaselineVector = dx == 0 ? Math.PI / 2 : Math.Atan(dy / (double)dx);
if (angleBaselineVector < 0)
{
angleBaselineVector += Math.PI;
}
angleBaselineVector += baselineAngleAdjustment;
// Now the angle between the arm and the baseline is simply the difference between these two angles
triple.arm1angle = (float)(angleBaselineVector - angleArm1ToVector);
triple.arm2angle = (float)Math.Acos((arm1Length * arm1Length + arm2length * arm2length - vectorLength * vectorLength) / (2 * arm2length * arm1Length));
triple.arm2angle += (float)armAngleAdjustment;
triple.linger = intermediatePointTimeMs;
// If the resulting angle would require the servo to move less than 0 degrees or more than 180, well, then that's unreachable too.
if (triple.arm1angle < 0.0F || triple.arm1angle > Math.PI ||
triple.arm2angle < 0.0F || triple.arm2angle > Math.PI)
{
Console.WriteLine("Angle out of range: arm1=" + radiansToDegrees((float)triple.arm1angle) + " arm2=" + radiansToDegrees((float)triple.arm2angle));
badUnreachable.Add(pt);
return(EncodingEnum.OUT_OF_RANGE);
}
#if false
Console.WriteLine("Pt=" + pt.X + "," + pt.Y + " DX=" + (float)dx + " DY=" + (float)dy + " VectLen=" + (float)vectorLength + " Arm1-to-vect=" + radiansToDegrees((float)angleArm1ToVector) +
" Base-to-Vect=" + radiansToDegrees((float)angleBaselineVector) + " arm1=" + radiansToDegrees((float)triple.arm1angle) + " arm2=" + radiansToDegrees((float)triple.arm2angle));
#endif
return(EncodingEnum.OK);
}
// Convert a point into an angle-angle-lingertime triple, given hublocation and arm lengths
private EncodingEnum encodePoint(PointF pt, PointF hubLocation, float arm1Length, float arm2length, out angleTimeTriplet triple)
{
triple = new angleTimeTriplet();
// First find the distance from the hub to the point
double dx = pt.X - hubLocation.X;
double dy = pt.Y - hubLocation.Y;
double vectorLength = Math.Sqrt(dx * dx + dy * dy);
// Since we now know the three sides of the triangle formed by the two arms and the vector,
// calculate the angle between the vector and the first arm using law of cosines
double angleArm1ToVector = Math.Acos((vectorLength * vectorLength + arm1Length * arm1Length - arm2length * arm2length) / (2 * vectorLength * arm1Length));
// If the point is too far away to be reached by the fully-extended arms, the value passed to acos will be greater than 1 (< -1)
if (double.IsNaN(angleArm1ToVector))
{
Console.WriteLine("Point too far: " + pt.X + "," + pt.Y);
badTooFar.Add(pt);
return EncodingEnum.TOO_FAR;
}
// Then determine the angle between the baseline and the vector
double angleBaselineVector = dx == 0 ? Math.PI / 2 : Math.Atan(dy / (double)dx);
if (angleBaselineVector < 0)
angleBaselineVector += Math.PI;
angleBaselineVector += baselineAngleAdjustment;
// Now the angle between the arm and the baseline is simply the difference between these two angles
triple.arm1angle = (float)(angleBaselineVector - angleArm1ToVector);
triple.arm2angle = (float)Math.Acos((arm1Length * arm1Length + arm2length * arm2length - vectorLength * vectorLength) / (2 * arm2length * arm1Length));
triple.arm2angle += (float)armAngleAdjustment;
triple.linger = intermediatePointTimeMs;
// If the resulting angle would require the servo to move less than 0 degrees or more than 180, well, then that's unreachable too.
if (triple.arm1angle < 0.0F || triple.arm1angle > Math.PI ||
triple.arm2angle < 0.0F || triple.arm2angle > Math.PI)
{
Console.WriteLine("Angle out of range: arm1=" + radiansToDegrees((float)triple.arm1angle) + " arm2=" + radiansToDegrees((float)triple.arm2angle));
badUnreachable.Add(pt);
return EncodingEnum.OUT_OF_RANGE;
}
#if false
Console.WriteLine("Pt=" + pt.X + "," + pt.Y + " DX=" + (float)dx + " DY=" + (float)dy + " VectLen=" + (float)vectorLength + " Arm1-to-vect=" + radiansToDegrees((float)angleArm1ToVector) +
" Base-to-Vect=" + radiansToDegrees((float)angleBaselineVector) + " arm1=" + radiansToDegrees((float)triple.arm1angle) + " arm2=" + radiansToDegrees((float)triple.arm2angle));
#endif
return EncodingEnum.OK;
}
// Convert a list of x/y points to a list of angle-angle-lingertime triples
void pointsToTriples(List<PointF> scaledPoints, float arm1Length, float arm2Length, PointF hubLocation, out List<angleTimeTriplet> triples)
{
triples = new List<angleTimeTriplet>();
#if CALIBRATION_ONLY
angleTimeTriplet att = new angleTimeTriplet();
att.arm1angle = att.arm2angle = (float)(Math.PI / 2); // 90 degree
att.milliseconds = 10000;
triples.Add(att);
return;
#else
// Add the first point to the list
angleTimeTriplet triple;
if (encodePoint(scaledPoints[0], hubLocation, arm1Length, arm2Length, out triple) == EncodingEnum.OK)
{
// The very first position the arms go to will allow a 10-second pause for inserting the pen
triple.linger = firstPointTimeMs;
triples.Add(triple);
}
// Then, generate lines between all the other point pairs and add them to the list
for (int i = 0; i < scaledPoints.Count - 1; ++i)
encodeLine(scaledPoints[i], scaledPoints[i + 1], hubLocation, arm1Length, arm2Length, ref triples);
#endif
// The last point should remain a long time also--to allow pen removal
angleTimeTriplet last = triples.Last();
last.linger = lastPointTimeMs;
triples.Add(last);
}