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