Esempio n. 1
0
        static async Task Main(string[] args)
        {
            //var quadInit2 = Quadrilateral.WithTwoSidesTwoAngles(98, 96, 128.57, 100.8);
            //var quadDelta2 = Quadrilateral.WithFourSides(98, 96, quadInit2.SideC, Math.Abs(quadInit2.SideD - 20));
            //var quadInit2 = Quadrilateral.WithTwoSidesTwoAngles(98, 96, 141.17, 101.25);
            //var quadDelta2 = Quadrilateral.WithFourSides(98, 96, quadInit2.SideC, quadInit2.SideD + 30);

            // TODO: move to tests
            //var quad1 = Quadrilateral.WithTwoSidesTwoAngles(7.21, 12.17, 123.69, 65.77); // pass
            //var quad2 = Quadrilateral.WithFourSides(7.21, 12.17, 8, 8); // pass
            //var quad1 = Quadrilateral.WithTwoSidesTwoAngles(6.32, 6.32, 71.57, 126.87); // pass
            //var quad2 = Quadrilateral.WithFourSides(6.32, 6.32, 8, 8); // pass
            //var quad1 = Quadrilateral.WithTwoSidesTwoAngles(10.2, 6.32, 78.69, 82.87); // pass
            //var quad2 = Quadrilateral.WithFourSides(10.2, 6.32, 8, 8); // pass
            //var quad1 = Quadrilateral.WithTwoSidesTwoAngles(10.77, 12.17, 111.8, 58.74); // pass
            //var quad2 = Quadrilateral.WithFourSides(10.77, 12.17, 8, 8); // pass

            var device = new Controller();

            await Program.PointUp(device);

            var positions = await device.ServoPositionRead();

            Console.WriteLine(string.Join(",", positions));

            // Known:
            // side from base to first joint
            // a = 98 mm
            // side from first join to arm
            // b = 96 mm
            // angle between b and a (from side b to a clockwise)
            // ba = positions[3]: servo4[100, 500, 850] => ba(90, 180, 270)
            // angle between a and d (from side a to d clockwise)
            // ad = positions[4]: servo5[100, 500, 850] => ad(0, 90, 180)
            // angle between d and c (from side a to d clockwise) always 90 degrees
            // dc = 90 degrees
            // base direction angle
            // z = positions[0]: servo6[-90(right), 0, 90(left)] => [120, 500, 880]
            // direction vector in mm
            // v = (+/-x, +/-y, +/-z)

            //await device.MultiServoMove(1000, servo4: 500);
            //await device.MultiServoMove(1000, 700, 700, 850, 150, 600, 500);

            //positions = await device.ServoPositionRead();
            //Console.WriteLine(string.Join(",", positions));

            var xchange = 30;
            var ychange = 10;
            var zchange = 10;

            var ba = Arm.S4ToAngle(positions[3]);
            var ad = Arm.S5ToAngle(positions[4]);
            var z0 = Arm.S6ToAngle(positions[5]);

            // coordinates of ba point
            var ad_rad = Converter.DegreesToRadians(ad);
            var z0_rad = Converter.DegreesToRadians(z0);
            var ba_z   = Arm.A * Math.Cos(ad_rad) * Math.Sin(z0_rad);
            var ba_x   = Arm.A * Math.Cos(ad_rad) * Math.Cos(z0_rad);
            var ba_y   = Arm.A * Math.Sin(ad_rad);

            // caclulating coordinates of cb point
            var o       = Triangle.Opposite(Arm.A, Arm.B, z0 > 180.0 ? z0 - 180.0 : z0);
            var o_delta = Triangle.Angle(Arm.A, o, Arm.B);
            var od      = ad + (z0 < 180.0 ? -1 : 1) * o_delta;

            // coordinates of cb point
            var od_rad = Converter.DegreesToRadians(od);
            var cb_z   = o * Math.Cos(od_rad) * Math.Sin(z0_rad);
            var cb_x   = o * Math.Cos(od_rad) * Math.Cos(z0_rad);
            var cb_y   = o * Math.Sin(od_rad);

            // coordinates of cb1 point
            var cb1_z = cb_z + xchange;
            var cb1_x = cb_x + ychange;
            var cb1_y = cb_y + zchange;

            // calculate new position of ba1
            var o_magnitude = Math.Sqrt(cb1_x * cb1_x + cb1_y * cb1_y + cb1_z * cb1_z);
            var ba1         = Triangle.Angle(Arm.A, Arm.B, o_magnitude);

            // calculate new position of ad1
            var ad1_upper  = Triangle.Angle(Arm.A, o_magnitude, Arm.B);
            var ad1_bottom = Triangle.Right.Angle(o_magnitude, cb1_y);
            var ad1        = ad1_upper + ad1_bottom;

            // calculate new angles z1
            var z1 = Converter.RadiansToDegrees(Math.Acos(cb1_x / (o_magnitude * Converter.RadiansToDegrees(Math.Cos(ad1)))));

            await device.MultiServoMove(
                1000,
                servo4 : Arm.AngleToS4(ba1),
                servo5 : Arm.AngleToS5(ad1),
                servo6 : Arm.AngleToS6(z1)
                );

            ////var quadInit = Quadrilateral.WithTwoSidesTwoAngles(Arm.A, Arm.B, ad, ba);

            ////var x0 = Triangle.Right.Base(quadInit.SideD, z0);
            ////var y0 = Triangle.Right.Opposite(quadInit.SideD, z0);

            ////var x1 = x0 + xchange;
            ////var y1 = y0 + zchange;

            ////var zDelta = Vector.Angle(x0, y0, x1, y1);
            ////var z1 = z0 + zDelta;

            ////var quadDelta =
            ////    Quadrilateral.WithFourSides(
            ////        Arm.A,
            ////        Arm.B,
            ////        Math.Abs(quadInit.SideC + ychange),
            ////        Math.Abs(quadInit.SideD + xchange)
            ////    );

            ////Console.WriteLine(quadDelta.AngleAD);
            ////Console.WriteLine(quadDelta.AngleBA);

            ////await device.MultiServoMove(
            ////    1000,
            ////    servo4: Arm.AngleToS4(quadDelta.AngleBA),
            ////    servo5: Arm.AngleToS5(quadDelta.AngleAD),
            ////    servo6: Arm.AngleToS6(z1)
            ////);

            //for (int i = 0; i < 10; i++)
            //{
            //    await device.MultiServoMove(250, 700, 700, 613, 179, 738, 512);
            //    await device.MultiServoMove(250, 700, 700, 570, 203, 799, 456);
            //    await device.MultiServoMove(250, 700, 700, 542, 279, 896, 456);
            //    await device.MultiServoMove(250, 700, 700, 542, 376, 981, 478);
            //    await device.MultiServoMove(250, 700, 700, 521, 390, 995, 521);
            //    await device.MultiServoMove(250, 700, 700, 522, 356, 972, 565);
            //    await device.MultiServoMove(250, 700, 700, 538, 300, 916, 591);
            //    await device.MultiServoMove(250, 700, 700, 565, 171, 761, 591);
            //}

            //await Program.PointUp(device);

            //Console.WriteLine(string.Join(",", await device.ServoPositionRead()));

            Console.ReadLine();
        }