private void Form1_MouseUp(object sender, MouseEventArgs e)
{
myDrawer.WithHistory = checkBox3.Checked;
//myCollisionDetector.DrawArms(_RobotSmaller, _RobotBigger);
myClient.SendThetaToRobots();
//ShowCollisionInfo(myCollisionDetector.collisionDetected);
}
public void InitializeKinematics()
{
myDrawer = new Drawer(pictureBox1.CreateGraphics(), pictureBox1.Width, pictureBox1.Height, pictureBox2.CreateGraphics(), pictureBox2.Width, pictureBox2.Height);
myDrawer.WithHistory = checkBox3.Checked;
_RobotBigger = new Robot("big",
49, 59, 205, 127, 58, 164,
new double[5] {
426, 314, 291, 346, 300
},
new double[6] {
180 * Constants.TORAD,
90 * Constants.TORAD,
180 * Constants.TORAD,
0 * Constants.TORAD,
0 * Constants.TORAD,
0
}
);
_RobotSmaller = new Robot("small",
49, 59, 143, 98, 58, 164,
new double[5] {
390, 226, 433, 305, 364
},
new double[6] {
180 * Constants.TORAD,
90 * Constants.TORAD,
180 * Constants.TORAD,
0 * Constants.TORAD,
0 * Constants.TORAD,
0
}
);
myClient = new Client(_RobotSmaller, _RobotBigger);
myCollisionDetector = new CollisionDetector(myDrawer, _RobotSmaller, _RobotBigger);
//PREPARE TO HAVE TCP MATRIX
_RobotBigger.SolveDirectKinematics();
_RobotSmaller.SolveDirectKinematics();
//Initial values to inverse kinematics
Bigger_x = _RobotBigger.MainTCP[0, 3];
Bigger_y = _RobotBigger.MainTCP[1, 3];
Bigger_z = _RobotBigger.MainTCP[2, 3];
Smaller_x = _RobotSmaller.MainTCP[0, 3];
Smaller_y = _RobotSmaller.MainTCP[1, 3];
Smaller_z = _RobotSmaller.MainTCP[2, 3];
_RobotSmaller.ComputeRealTheta();
myClient.SendThetaToRobots();
}