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; }