#include <Servo.h>
Servo R11;//Objekt; Seite,Ort,aufpunkt; zielkoordinate; Vektor;
Servo R12;
Servo R13;

float r1a;//winkel
float r1b;
float r1c;

const float r1a1 = +2;//aufpunkt
const float r1a2 = +3;
const float r1a3 = +0;

float r1z1;//zielkoordinate
float r1z2;
float r1z3;

float r1v1;//x,y,z koordinaten des zielvektors
float r1v2;
float r1v3;

float r1V; //länge des zielvektors



const int l1 = 10; //cm, 1.armgliedlänge
//const int l2 = 10; //cm 2.armgliedlänge

//X1:rechts
//X2:Vorwerts
//X3: höhe zum Boden


void setup() {
  // put your setup code here, to run once:
  R1();
  Serial.begin(9600);

  r1z1 = 5;
  r1z2 = 7;
  r1z3 = -10;//R1 zielkoordinate definierung
}

void loop() {

  r1z2 = r1z2 - 0.5;
  if (r1z2 < -7) {
    r1z2 = 7;
  }
  /*
    r1v1 = (r1z1 - r1a1);//r1 vektor koordinaten
    r1v2 = (r1z2 - r1a2);
    r1v3 = (r1z3 - r1a3);
    r1V = sqrt((r1v1 * r1v1) + (r1v2 * r1v2) + (r1v3 * r1v3)); //bertrag von r1V vektor
  */
  if (r1a2 < r1z2) {
    r1a = 180 * atan(r1v1 / r1v2) / PI;
    Serial.println("if");
  }
  else if (r1z2 = r1a2) {
    r1a = 90;
    Serial.println("else if");
  }
  else if (r1a2 > r1z2) {
    r1a = 180 + 180 * atan(r1v1 / r1v2) / PI;
    Serial.println("else");
  }

  // r1b = 180 * acos((r1V / 2) / l1) / PI; //cos() im Rad ==> a*180/PI=a°
  //r1c = 180 - r1b * 2;

  R11.write(r1a);// R12.write(r1b); R13.write(r1c);
  check_r1();
  delay(1000);

}


void R1() {
  R11.attach(2);//  R12.attach(3);  R13.attach(4);
  R11.write(000);//R12.write(000);R13.write(000);
  delay(300);
  R11.write(180);//R12.write(180);R13.write(180);
  delay(300);
  R11.write(90);//R12.write(90);R13.write(90);
  delay(300);
}
void check_r1() {
  Serial.print("r1z2="); Serial.println(r1z2);
  Serial.print("r1a="); Serial.println(r1a);
  //Serial.print(",r1b="); Serial.print(r1b);
  //Serial.print(",r1c="); Serial.print(r1c);
  Serial.println();
}