Robot Limb Servo angles calculated by P2 using inverse cosine via CORDIC
Rayman
Posts: 14,633
Could have posted here: https://forums.parallax.com/discussion/175423
But, this is kind of generic to robotic arm motion, where you want P2 to calculate the servo angles for motion and/or position.
Turns out you need inverse cosine (AKA arccosine) to do this.
Need for that has probably been worked out a million times and is described here:
https://appliedgo.net/roboticarm/
Fortunately, the P2 has a CORDIC engine designed to make this calculation much faster that would be on P1, for example.
Also, fortunate that @DiverBob has already dived into this and posted a solution here:
https://forums.parallax.com/discussion/173046
Anyway, here are the slides from my analysis, a spreadsheet to check against SolidWorks reported angles, and example code that seems to be calculating the angles correctly to within 1 degree.
Comments
Using the PropTool Debug plotting function to test the servo angle calculations by drawing the robot on the screen.
The idea is that the feet should stay on the red circles while the robot moves forward.
It does OK, but I thinking about trying to improve it a little by increasing angular resolution...
You can try it yourself with the attached code.
Just got left and right sides fully walking in the debug window.
Now, just need to clean up and apply to robot servos...