Implementing inverse kinematics in code

[TatianaResend]

Make a function that receives the spatial coordinates (x,y,z) and converts it into the necessary angle in the motors that control the y axis and the z axis.

[TatianaResend]

Follow the structure of this example:

void CinInv (double coordY_CI, double coordZ_CI) {
  //theta 2
  theta2 = -acos((((pow(120 - coordY_CI, 2)) + (pow(200 - coordZ_CI, 2)) - (pow(L1, 2)) - (pow(L2, 2)))) / (48000)); //48000 = 2*L1*L2
  //theta 1
  float posz = 200 - coordZ_CI;
  float posy = 120 - coordY_CI;
  float poszz = L2 * sin(theta2);
  float posyy = L1 + 200 * cos(theta2);
  theta1 = atan((posz) / (posy)) + atan((poszz) / (posyy));
  //converter para DEG
  theta1 = theta1 * (180 / PI);
  theta2 = theta2 * (180 / PI);
  //determinar pulsos a ser executados pelo motor
  finalposXYZ[0] = round(theta2 * 55.5555555); // 55.555555 = 100 * PulPerRev/360 conversão para pulsos com redutora de 100:1
  finalposXYZ[1] = round(theta1 * 55.5555555); // 100 * (PulPerRev/360)
  //atribuir as posições para mover às coordenadas recebidas
  posTomoveY = coordY_CI;
  posTomoveZ = coordZ_CI;
}

[TatianaResend]

Need to confirm redundancy. In both, I used the arm on top (positive). It is necessary to test with both engines and make a prototype (something simple) to make sure you are positioning it correctly.

[TatianaResend]

Confirm component D1 and D2 are correct. I think I wrote it wrong. The correct is:

• D1=TyL5 - TyL6 - 0,5L5L6
• D2=-TyL5 + TyL6 - 0,5L5L6

[TatianaResend]

I need to have the prototype half assembled to be able to see if the kinematics are correct.

[TatianaResend]

It is essential to take into account the constraints defined by kinematics and be mindful that it will be necessary to implement a variable to apply the discount on z.

• [x] Done

[TatianaResend]

The kinematics have been tested and are correct, however there are some positioning errors. These are due to gaps in the connections and essentially to the temporary connection made between the motors and the arms.