float l1 = 50;
float l2 = 50;
float l3 = 50;
float l4 = 50;
float q1, q2, q3, q4;
typedef String str;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
Serial.println("Hello, ESP32!");
}
float x = 100;
float y = 100;
float z = 0;
float q = -90;
void loop() {
q1 = atan2(y,x);
q1 = int(q1 * 180/PI);
float p1 = sqrt(pow(x,2) + pow(y,2));
float x1 = p1 - (l3 * cos(radians(q)));
float y1 = p1 - (l3 * sin(radians(q)));
float p2 = sqrt(pow(x1,2) + pow(y1,2));
float q21 = atan2(y1,x1);
q21 = int(q21 * 180/PI);
float q22 = acos(radians((pow(l1,2) + pow(p2,2) - pow(l2,2))) / (2*l1*p2));
q2 = q21 + q22;
// q1 = int(q1 * 180/PI);
// q2 = int(q2 * 180/PI);
// q3 = int(q3 * 180/PI);
// q4 = int(q4 * 180/PI);
// setCoxa(1,q1);
// setFemur(1,q2);
// setTibia(1,q3);
// setTarsus(1,q4);
Serial.println(str("p1:") + p1 + str(" x1:") + x1 + str(" y1:") + y1 + str(" p2:") + p2 + str(" q21:") + q21 + str(" q22:") + q22);
Serial.println(str("q1:") + q1 + str(" q2:") + q21 + str(" q3:") + q3 + str(" q4:") + q4);
delay(1000);
while(1){}
}