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);
}