Created
June 16, 2019 07:17
-
-
Save symmetryninja/6b6bc5a25db5b3a6252429be717de908 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// note PI10 is a constant with pi to ten places | |
void ikTrippleAngle(double targetX, double targetY, double targetZ) { | |
double legLengthXY, legLength, base, lift, reach; | |
// determining the leglengths is our first hurdle | |
legLengthXY = sqrt(sq(targetX) + sq(targetY)); // view from the top | |
legLength = sqrt(sq(legLengthXY) + sq(targetZ)); // view from the side | |
// base rotation | |
base = atan(targetY/targetX) * 180/PI10; | |
// lift servo position | |
double a1 = atan(targetX/targetZ) * 180/PI10; | |
double a2 = acos( | |
(sq(RADIUS) - sq(HUMEROUS) - sq(legLength)) | |
/ | |
(-2 * legLength * HUMEROUS) | |
) * 180/PI10; | |
lift = 180 - (a1 + a2); // my joints are zero on the vertical | |
// reach servo position | |
reach = acos( | |
(sq(legLength) - sq(RADIUS) - sq(HUMEROUS)) | |
/ | |
(-2 * RADIUS * HUMEROUS) | |
) * 180/PI10; | |
dxLegState.angleBase = base; | |
dxLegState.angleReach = reach; | |
dxLegState.angleLift = lift; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment