public void DrawPassiveArm(ref OpenGL gl)
{
//gl.PushMatrix();
//gl.Translate(-0.5f, 0, -0.5f);
//gl.Scale(1f, fPassiveLeg * fRatio, 1f);
//gl.Color(0f, 1.0f, 0.5f);//color arm
//rc.DrawUnitCylinder(ref gl);
//
gl.PushMatrix();
InverseKinematic IK = new InverseKinematic();
vec3[] Top = IK.kinv_TopPos(GoalPos.x / fRatio, GoalPos.y / fRatio, GoalPos.z / fRatio);
vec3[] Bottom = IK.kinv_BotPos(GoalPos.x / fRatio, GoalPos.y / fRatio, GoalPos.z / fRatio);
gl.Color(0f, 1.0f, 0.5f);//color arm
//rc.DrawUnitCylinder(ref gl, fRatio * new vec3(Top[0].y, Top[0].x, Top[0].z), fRatio * new vec3(Bottom[0].y, -Bottom[0].x, Bottom[0].z));
rc.DrawUnitCylinder(ref gl, fRatio * new vec3(Top[0].y, Top[0].x, Top[0].z), fRatio * new vec3(Bottom[0].y, -Bottom[0].x, Bottom[0].z));
//gl.Translate(0.5f, 0, 0.5f); // moi sua chieu ngay 31/10
//rc.DrawUnitCylinder(ref gl, fRatio * new vec3(-Top[1].x, -Top[1].y, Top[1].z), fRatio * new vec3(-Bottom[1].x, -Bottom[1].y, Bottom[1].z));
rc.DrawUnitCylinder(ref gl, fRatio * new vec3(Top[1].y, Top[1].x, Top[1].z), fRatio * new vec3(Bottom[1].y, -Bottom[1].x, Bottom[1].z));
//vec3 test1 = fRatio * new vec3(Top[2].x, Top[2].y, Top[2].z);
//vec3 test2 = fRatio * new vec3(Bottom[2].x, Bottom[2].y, Bottom[2].z);
vec3 test1 = fRatio * new vec3(Top[2].y, Top[2].x, Top[2].z);
vec3 test2 = fRatio * new vec3(Bottom[2].y, -Bottom[2].x, Bottom[2].z);
rc.DrawUnitCylinder(ref gl, test1, test2);
gl.PopMatrix();
}
public Delta_3_Robot(vec3 GPos)
{
GoalPos = GPos;
/*
* GoalPos.x = 0.0f;
* GoalPos.y = -75.0f;
* GoalPos.z = -380.518f;*/
double[] tmp = new double[] { GoalPos.x, GoalPos.y, GoalPos.z };
vec3[] a = new vec3[3];
vec3[] b = new vec3[3];
InverseKinematic IK = new InverseKinematic();
IK.delta_calcInverse(tmp[0], tmp[1], tmp[2], ref ThetaMotor, ref ThetaPassiveRy, ref ThetaPassiveRz);
}
private void openGLControl_OpenGLDraw(object sender, PaintEventArgs e)
{
OpenGL gl = openGLControl.OpenGL;
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
gl.LoadIdentity();
gl.Perspective(fFovy, (double)Width / (double)Height, 0.01, 400.0);
gl.LookAt(vDrawOriginal.x, vDrawOriginal.y, vDrawOriginal.z, 0, 0, 0, 0, 1, 0);
gl.Rotate(vRotAngle.x, 1.0f, 0.0f, 0.0f);
gl.Rotate(vRotAngle.y, 0.0f, 1.0f, 0.0f);
gl.Rotate(vRotAngle.z, 0.0f, 0.0f, 1.0f);
drawGrid(gl);
Delta_3_Robot D3R = new Delta_3_Robot(new vec3(MovePlatePos.x, MovePlatePos.y, MovePlatePos.z));
D3R.DrawDelta3Robot(ref gl);
double[] ThetaMotor = new double[3];
double[] ThetaPassiveRx = new double[3];
double[] ThetaPassiveRy = new double[3];
InverseKinematic IK = new InverseKinematic();
IK.delta_calcInverse(MovePlatePos.x, MovePlatePos.y, MovePlatePos.z, ref ThetaMotor, ref ThetaPassiveRx, ref ThetaPassiveRy);
/* label6.Text = "Leg1 Theta 1: " + ThetaMotor[0];
* label5.Text = "Leg2 Theta 1: " + ThetaMotor[1];
* label4.Text = "Leg3 Theta 1: " + ThetaMotor[2];
*
* label9.Text = "Leg1 Theta 2 Rx: " + ThetaPassiveRx[0];
* label8.Text = "Leg2 Theta 2 Rx: " + ThetaPassiveRx[1];
* label7.Text = "Leg3 Theta 2 Rx: " + ThetaPassiveRx[2];
*
* label12.Text = "Leg1 Theta 2 Ry: " + ThetaPassiveRy[0];
* label11.Text = "Leg2 Theta 2 Ry: " + ThetaPassiveRy[1];
* label10.Text = "Leg3 Theta 2 Ry: " + ThetaPassiveRy[2]; */
tbXCoor.Text = Math.Round(MovePlatePos.x, 4).ToString();
tbYCoor.Text = Math.Round(MovePlatePos.y, 4).ToString();
tbZCoor.Text = Math.Round(MovePlatePos.z, 4).ToString();
UpdateCoorRv();
tbθ1Coor.Text = Math.Round(ThetaMotor[0], 4).ToString();
tbθ2Coor.Text = Math.Round(ThetaMotor[1], 4).ToString();
tbθ3Coor.Text = Math.Round(ThetaMotor[2], 4).ToString();
}
