Manual

52
∠2=arc cos











∴ ∠α=∠1+∠2=arc tan(z/v)+ arc cos











The program should be:
alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) +
pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));
Similarly, ∠β= arc cos










.
The program should be:
beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 /
length_a / length_b);
Similarly, ∠γ=arc tan (y/x).
The program should be (here only analyze the case for the leg end in the first quadrant):
gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
Hereto all the transformation from coordinates of the leg end into the servo rotational angle
is done.
Each leg has its own coordinate system, which is calculated independently.
Servo_Service Function
After the function cartesian_to_polar is done in the sketch, immediately call the function void
polar_to_servo(int leg, float alpha, float beta, float gamma) to adjust the servo rotational angle
to the set angle. These two functions will be called one by one in the 50HZ service function void
servo_service(void). It is a critical function and you need to pay much attention here.
Streamline Programming
After you've understood the core code and the working sequence, review the code:
/* Installation and Adjustment -----------------------------------------------
*/
#define INSTALL //uncomment only this to install the robot
//#define ADJUST //uncomment only this to adjust the servos
//#define VERIFY //uncomment only this to verify the adjustment
Activate the INSTALL command line and then add a for() loop in setup.
void setup()
{
#ifdef INSTALL
//initialize all servos
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
SunFounder