// 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);
        }
Ejemplo n.º 3
0
      // 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;
      }
Ejemplo n.º 4
0
      // 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);
      }