int l1 = 25;
int l2 = 50;
int l3 = 65;
int l4 = 70;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
Serial.println("Hello, ESP32!");
// invKinematic(0,0,0);
invKinematic(l1+l2+l3+l4,0,0);
}
void loop() {
// put your main code here, to run repeatedly:
delay(10); // this speeds up the simulation
}
void invKinematic(int x, int y, int t){
float x3 = x - (l4 * cos(radians(t)));
float y3 = y - (l4 * sin(radians(t)));
float p3 = sqrt(pow(x3,2) + pow(y3,2));
Serial.print(x3);
Serial.print(" ");
Serial.print(y3);
Serial.print(" ");
Serial.println(x3);
}