Hello! I don't know if this is the right forum to ask this question, but I am currently working on inverse kinematics for a quadruped. I am using servo motors and I am using the law of cosines in order to find the initial angles for the legs. I have attached my code and output below. My problem is that when I enter the height (angleVal[2]) the output is spitting out wildly different numbers than expected.
The height is set to 189, the thigh and legs are set to 133.9. When I enter this into a calculator it gives me the correct initial angles which are 45 degrees for the shoulder and 90 degrees for the knee. I don't know why my output looks like this please help!
int angleVal[]={0,0,189.3};
if(radio.available()) {
while(radio.available()){
radio.read(&angleVal,sizeof(angleVal));
}
}
Serial.print("X1: ");
Serial.print(angleVal[0]);
Serial.print(" || Y1: ");
Serial.print(angleVal[1]);
Serial.print(" || counter: ");
Serial.print(angleVal[2]);
//kinematics(angleVal[0], angleVal[1], angleVal[2]);
float shoulder1 = sq(thigh) + sq(angleVal[2]) - sq(leg);
float shoulder2 = 2 * thigh * angleVal[2];
float shoulder3 = shoulder1/shoulder2;
float shoulderRad = acos(shoulder3);
float kneeRad = PI - (shoulderRad * 2);
float shoulder = shoulderRad * (180/PI);
float knee = kneeRad * (180/PI);
Serial.print(" || shoulderAngle: ");
Serial.print(shoulder);
Serial.print(" || knee: ");
Serial.println(knee);
