static double Angle0 = 1.5707963267; //Math.PI / 2; static Point[] CalculateRobotHands() { var mass = RobotMathematics.MoveTo(X0, Y0, Angle0); mass[2] = -Angle0; /*if (X0 < 0) * mass[1] = Math.PI * 2 - ( mass[1] - Math.PI + mass[0]); * else*/ mass[1] = mass[1] - Math.PI + mass[0]; var arm = new Point[4]; arm[0] = new Point(0, 0, 250, 250); if (double.IsNaN(mass[0])) { mass[0] = 0.2; } for (int i = 1; i < 4; ++i) { if (double.IsNaN(mass[0])) { return(null); } int X = arm[i - 1].X2 + (int)(Math.Cos(mass[i - 1]) * RobotMathematics.Arm(i) * DrowCoefficient); int Y = arm[i - 1].Y2 - (int)(Math.Sin(mass[i - 1]) * RobotMathematics.Arm(i) * DrowCoefficient); if (X == double.NaN || Y == double.NaN || X < 0 || X > 500 || Y < 0 || Y > 500) { return(null); } arm[i] = new Point(arm[i - 1].X2, arm[i - 1].Y2, X, Y); } return(arm); }
private static void ChangeAngles(double x, double y, double angle) { var angles = RobotMathematics.MoveTo(x / 10, y / 10, angle); Shoulder = 2 * Math.PI - angles[0]; Elbow = 2 * Math.PI - angles[1]; Wrist = 2 * Math.PI - angles[2]; lines[0].x2 = lines[0].x1 + RobotMathematics.UpperArm * Math.Cos(Shoulder) * 4; lines[0].y2 = lines[0].y1 + RobotMathematics.UpperArm * Math.Sin(Shoulder) * 4; lines[1].x1 = lines[0].x2; lines[1].y1 = lines[0].y2; lines[1].x2 = lines[1].x1 + RobotMathematics.Forearm * Math.Cos(Elbow + Shoulder - Math.PI) * 4; lines[1].y2 = lines[1].y1 + RobotMathematics.Forearm * Math.Sin(Elbow + Shoulder - Math.PI) * 4; lines[2].x1 = lines[1].x2; lines[2].y1 = lines[1].y2; lines[2].x2 = lines[2].x1 + RobotMathematics.Palm * Math.Cos(Wrist - Elbow - Shoulder) * 4; lines[2].y2 = lines[2].y1 + RobotMathematics.Palm * Math.Sin(Wrist - Elbow - Shoulder) * 4; }