Marlin kinematics update

Stuck in a city far away from my lab, I had to resort to playing with the firmware.

Based on Johann Rocholl’s Marlin for Rosstock  (a delta bot), this is the initial kinematics change I would like to test when the bot itself is finished:

in Marlin_main.cpp:

void calculate_delta(float cartesian[3])
{

 // SCARA "X" = theta
 // SCARA "Y" = psi+theta, motor movement inverted.
 //static float SCARA_C2, SCARA_S2, SCARA_theta, SCARA_psi - Defined above...

// Length of inner support arm
 //#define Linkage_1 150 //mm

// Length of outer support arm
 //#define Linkage_2 150 //mm

// SCARA tower offset (position of Tower relative to bed position)
 //#define SCARA_offset_x 100 //mm 
 //#define SCARA_offset_y -60 //mm

 SCARA_C2 = (sq(cartesian[X_AXIS])+sq(cartesian[Y_AXIS])-Linkage_1_sq-Linkage_2_sq)/(2*Linkage_1*Linkage_2);
 SCARA_S2 = sqrt(1-sq(SCARA_C2));

 SCARA_K1 = Linkage_1+Linkage_2*SCARA_C2;
 SCARA_K2 = Linkage_2*SCARA_S2;

 SCARA_theta = atan2(cartesian[X_AXIS],cartesian[Y_AXIS])-atan2(SCARA_K1, SCARA_K2);
 SCARA_psi = atan2(SCARA_C2,SCARA_S2);


 delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - theta is support arm angle
 delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG; // - equal to sub arm angle (inverted motor)

 delta[Z_AXIS] = cartesian[Z_AXIS]; // Standard Z for Morgan

}

Now the wait…