int CKinematics::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts)
{
GeoCorrect(x,y,z,&x,&y,&z);
Acts[0] = x*m_MotionParams.CountsPerInchX;
Acts[1] = y*m_MotionParams.CountsPerInchY;
Acts[2] = z*m_MotionParams.CountsPerInchZ;
Acts[3] = a*m_MotionParams.CountsPerInchA;
Acts[4] = b*m_MotionParams.CountsPerInchB;
Acts[5] = c*m_MotionParams.CountsPerInchC;
return 0;
}
int CKinematics3Rod::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts)
{
// find lengths of each actuator
GeoCorrect(x,y,z,&x,&y,&z);
double r0 = sqrt(sqr(x-Act0Center.x) + sqr(y-Act0Center.y) + sqr(z-Act0Center.z)) - Act0Off;
double r1 = sqrt(sqr(x-Act1Center.x) + sqr(y-Act1Center.y) + sqr(z-Act1Center.z)) - Act1Off;
double r2 = sqrt(sqr(x-Act2Center.x) + sqr(y-Act2Center.y) + sqr(z-Act2Center.z)) - Act2Off;
Acts[0] = r0*m_MotionParams.CountsPerInchX;
Acts[1] = r1*m_MotionParams.CountsPerInchY;
Acts[2] = r2*m_MotionParams.CountsPerInchZ;
Acts[3] = a*m_MotionParams.CountsPerInchA;
Acts[4] = b*m_MotionParams.CountsPerInchB;
Acts[5] = c*m_MotionParams.CountsPerInchC;
return 0;
}
