void Robot::drawLine(int points[4], BioloidController bioloid)
{
InverseKinematics(points);
bioloid.poseSize = 2; // load two poses in, one for each vertex
bioloid.readPose();//find where the servos are currently
penUp();
bioloid.setNextPose(1, points[0]); //set the coordinates for the vertex to which the arm is moving first
bioloid.setNextPose(2, points[1]);
bioloid.interpolateSetup(5000); // setup for interpolation from current->next
while(bioloid.interpolating > 0)
{
//keep moving until next pose is reached
bioloid.interpolateStep();
delay(3);
}
penDown();
//bioloid.readPose();//find where the servos are currently
bioloid.setNextPose(1, points[2]);
bioloid.setNextPose(2, points[3]);
bioloid.interpolateSetup(1000); // setup for interpolation from current->next over 1/2 a second
while(bioloid.interpolating > 0)
{
//keep moving until next pose is reached
bioloid.interpolateStep();
delay(3);
}relaxArm(bioloid);
}
void PantographDevice::Setup(float tol, float rate, float rate_moving) {
goal_reaching_tol = tol;
// rate in Hz, rate_moving in rad(deg/tick?)/s
loop_dt = 1000.0 / rate;
dth = rate_moving * loop_dt / 1000.0;
// should not attach in constructor
servo_base_left.attach(pin_left);
servo_base_right.attach(pin_right);
// compute and move to center
x_goal = -a5 / 2;
y_goal = 3.0 * sqrt((a1 + a2) * (a1 + a2) - (0.5 * a5) * (0.5 * a5)) / 4.0 + 1.0;
x = x_goal;
y = y_goal;
InverseKinematics(x_goal, y_goal, th_goal_left, th_goal_right);
servo_base_left.write(th_goal_left);
servo_base_right.write(th_goal_right);
// enable the motor power
digitalWrite(power_pin, HIGH);
// give a little time for it to stabilize
delay(200);
// set power back off
digitalWrite(power_pin, LOW);
}
//----------------------------------------------------------------------------
bool KeyframeAnimation::OnPaint (HDC hDC)
{
if ( !m_pkPosSpline )
return false;
HPEN hPen;
HGDIOBJ hOldPen;
if ( m_bDrawKeys )
{
// draw key frames
hPen = CreatePen(PS_SOLID,2,RGB(0,0,0));
hOldPen = SelectObject(hDC,hPen);
for (int i = 0; i < m_iNumKeys; i++)
{
MoveToEx(hDC,int(m_akLeg[i].m_kH.x),int(m_akLeg[i].m_kH.y),NULL);
LineTo(hDC,int(m_akLeg[i].m_kK.x),int(m_akLeg[i].m_kK.y));
LineTo(hDC,int(m_akLeg[i].m_kA.x),int(m_akLeg[i].m_kA.y));
LineTo(hDC,int(m_akLeg[i].m_kF.x),int(m_akLeg[i].m_kF.y));
}
SelectObject(hDC,hOldPen);
DeleteObject(hPen);
}
// interpolate key frames
Leg kInterp;
float fAngle;
Vector3 kAxis;
kInterp.m_fHKLength = m_fHKLength;
kInterp.m_fKALength = m_fKALength;
kInterp.m_fAFLength = m_fAFLength;
kInterp.m_kH = m_pkPosSpline->Position(m_fTime);
m_pkHRotSpline->Q(m_fTime).ToAngleAxis(fAngle,kAxis);
kInterp.m_fHAngle = ( kAxis.z > 0.0f ? fAngle : -fAngle );
m_pkKRotSpline->Q(m_fTime).ToAngleAxis(fAngle,kAxis);
kInterp.m_fKAngle = ( kAxis.z > 0.0f ? fAngle : -fAngle );
m_pkARotSpline->Q(m_fTime).ToAngleAxis(fAngle,kAxis);
kInterp.m_fAAngle = ( kAxis.z > 0.0f ? fAngle : -fAngle );
InverseKinematics(kInterp);
// draw interpolated key frame
hPen = CreatePen(PS_SOLID,2,RGB(255,0,0));
hOldPen = SelectObject(hDC,hPen);
MoveToEx(hDC,int(kInterp.m_kH.x),int(kInterp.m_kH.y),NULL);
LineTo(hDC,int(kInterp.m_kK.x),int(kInterp.m_kK.y));
LineTo(hDC,int(kInterp.m_kA.x),int(kInterp.m_kA.y));
LineTo(hDC,int(kInterp.m_kF.x),int(kInterp.m_kF.y));
SelectObject(hDC,hOldPen);
DeleteObject(hPen);
return true;
}
void PantographDevice::SetGoal(float x_goal_new, float y_goal_new) {
x_goal = x_goal_new;
y_goal = y_goal_new;
// perform inverse kinmatics
InverseKinematics(x_goal, y_goal, th_goal_left, th_goal_right);
// reset flags
flag_goal_reached = false;
flag_new_goal_set = true;
}
void TestMult()
{
//printf("d 1 %f %f %f %f %f %f %f \n\r",desiredXPosition[0],desiredXPosition[1],desiredXPosition[2],desiredXPosition[3],desiredXPosition[4],desiredXPosition[5],desiredXPosition[6]);
InverseKinematics();
printf("d F %f %f %f %f %f %f %f \r",desiredQPosition[0],desiredQPosition[1],desiredQPosition[2],desiredQPosition[3],desiredQPosition[4],desiredQPosition[5],desiredQPosition[6]);
}
void GoToReferencePosition (void) {
DirectKinematics();
//calculate and amplify the errors
ReferenceGlobalSpeed[X] = Kp_Pos * (ReferencePosition[X] - Position[X]);
ReferenceGlobalSpeed[Y] = Kp_Pos * (ReferencePosition[Y] - Position[Y]);
ReferenceGlobalSpeed[G] = Kp_Pos * (ReferencePosition[G] - Position[G]);
InverseKinematics();
RotationalSpeedControl(M1);
RotationalSpeedControl(M2);
RotationalSpeedControl(M3);
}
int SetdesiredPosition(float p0,float p1,float p2,float p3,float p4,float p5,float p6, float tf)
{
int succes;
succes = 0;
SetMaxSpeeds(20);
updateQPosition();
if(tf<0.5)
{
return 0;
}
// 1) Escribe Dato. Posici髇 Cartesiana Deseada.
if(instructionSet==1)
{
desiredXPosition[0] = p0;
desiredXPosition[1] = p1;
desiredXPosition[2] = p2;
desiredXPosition[3] = p3;
desiredXPosition[4] = p4;
desiredXPosition[5] = p5;
desiredXPosition[6] = p6;
// 2) Cinem醫ica Inversa
//printf("Inverse Kinematics \n\r");
succes = InverseKinematics();
}
else if(instructionSet==2)
{
desiredQPosition[0] = p0;
desiredQPosition[1] = p1;
desiredQPosition[2] = p2;
desiredQPosition[3] = p3;
desiredQPosition[4] = p4;
desiredQPosition[5] = p5;
desiredQPosition[6] = p6;
// printf("Angular Pos \n\r");
succes = 1;
}
timeCounter = 0;
executionTF = tf;
printf("CurrentQPos: %f , %f , %f , %f , %f , %f , %f \r", currentQPosition[0],currentQPosition[1],currentQPosition[2],currentQPosition[3],currentQPosition[4],currentQPosition[5],currentQPosition[6]);
printf("DesiredQPos: %f , %f , %f , %f , %f , %f , %f \r", desiredQPosition[0],desiredQPosition[1],desiredQPosition[2],desiredQPosition[3],desiredQPosition[4],desiredQPosition[5],desiredQPosition[6]);
// 2) Constantes de Planeaci髇-
for(int i = 0;i<DOF;i++)
{
currentQPosition[i] = GetPosition(i);
initialQPosition[i] = currentQPosition[i];
}
for(int i = 0; i<DOF;i++)
{
PERcoef[i] = coefQuintico (initialQPosition[i],desiredQPosition[i], executionTF);
//LSPBcoef[i] = coefLSPB (initialQPosition[i],desiredQPosition[i], executionTF);
}
if(succes)
{
doControl = 1;
return 1;
}
else{
doControl = 0;
return 0;
}
}
//---------------------------------------------------------------------------
bool KeyframeAnimation::OnInitialize ()
{
// keyframe data
Vector3 akPos[7] =
{
Vector3(50.0f,34.0f,0.0f),
Vector3(50.0f,34.0f,0.0f),
Vector3(80.0f,33.0f,0.0f),
Vector3(110.0f,33.0f,0.0f),
Vector3(140.0f,34.0f,0.0f),
Vector3(207.0f,34.0f,0.0f),
Vector3(207.0f,34.0f,0.0f)
};
float afHAngle[7] =
{
0.0f, 0.0f, 0.392699f, 0.589049f, 0.490874f, 0.0f, 0.0f
};
float afKAngle[7] =
{
2.748894f, 2.748894f, 1.963495f, 2.356194f, 2.748894f, 2.748894f,
2.748894f
};
float afAAngle[7] =
{
-1.178097f, -1.178097f, -1.178097f, -1.570796f, -1.963495f,
-1.178097f, -1.178097f
};
PosKey akHPos[7];
RotKey akHRot[7], akKRot[7], akARot[7];
m_akLeg = new Leg[7];
int i;
for (i = 0; i < m_iNumKeys; i++)
{
float fTime = (float)(i-1);
akHPos[i].Time() = fTime;
akHPos[i].P() = akPos[i];
akHRot[i].Time() = fTime;
akHRot[i].Q().FromAngleAxis(afHAngle[i],Vector3::UNIT_Z);
akKRot[i].Time() = fTime;
akKRot[i].Q().FromAngleAxis(afKAngle[i],Vector3::UNIT_Z);
akARot[i].Time() = fTime;
akARot[i].Q().FromAngleAxis(afAAngle[i],Vector3::UNIT_Z);
m_akLeg[i].m_fHKLength = m_fHKLength;
m_akLeg[i].m_fKALength = m_fKALength;
m_akLeg[i].m_fAFLength = m_fAFLength;
m_akLeg[i].m_kH = akHPos[i].P();
m_akLeg[i].m_fHAngle = afHAngle[i];
m_akLeg[i].m_fKAngle = afKAngle[i];
m_akLeg[i].m_fAAngle = afAAngle[i];
InverseKinematics(m_akLeg[i]);
}
m_pkPosSpline = new PosSpline(m_iNumKeys,akHPos);
m_pkHRotSpline = new RotSpline(m_iNumKeys,akHRot);
m_pkKRotSpline = new RotSpline(m_iNumKeys,akKRot);
m_pkARotSpline = new RotSpline(m_iNumKeys,akARot);
InvalidateRect(GetWindowHandle(),NULL,TRUE);
return true;
}
