#include <Servo.h>
Servo R11; float r1a; const float r1a1 = +2; float r1z1; float r1v1; float r1V; //Objekt; Seite,Ort,abs; winkel; aufpunkt; zielkoordinate; Vektor;
Servo R12; float r1b; const float r1a2 = +3; float r1z2; float r1v2;
Servo R13; float r1c; const float r1a3 = +0; float r1z3; float r1v3;

Servo R21; float r2a; const float r2a1 = +2; float r2z1; float r2v1; float r2V;
Servo R22; float r2b; const float r2a2 = -3; float r2z2; float r2v2;
Servo R23; float r2c; const float r2a3 = +0; float r2z3; float r2v3;

Servo L11; float l1a; const float l1a1 = -2; float l1z1; float l1v1; float l1V;
Servo L12; float l1b; const float l1a2 = +3; float l1z2; float l1v2;
Servo L13; float l1c; const float l1a3 = +0; float l1z3; float l1v3;

Servo L21; float l2a; const float l2a1 = -2; float l2z1; float l2v1; float l2V;
Servo L22; float l2b; const float l2a2 = -3; float l2z2; float l2v2;
Servo L23; float l2c; const float l2a3 = +0; float l2z3; float l2v3; //Objekt; Seite,Ort,abs; winkel; aufpunkt; zielkoordinate; Vektor;

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

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


void setup() {
  // put your setup code here, to run once:
  R1();
  //R2();
  //L1();
  //L2();
  //Serial.begin(9600);
    //r1z1 = 2; r1z2 = -3; r1z3 = -10;//R1 zielkoordinate definierung

}

void loop() {

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

  r1z2=r1z2-0.5;
  if (r1z2<-7){r1z2=7;}
  //Serial.println(r1z2);
  //check_r1();

  r1v1 = (r1z1 - r1a1);//r1 vektor koordinaten
  r1v2 = (r1z2 - r1a2);
  r1v3 = (r1z3 - r1a3);
  
  r1V = sqrt((r1v1 * r1v1) + (r1v2 * r1v2) + (r1v3 * r1v3)); //bertrag von r1V vektor

  r1a = 180 * atan(r1v1 / r1v2) / PI;
  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);


  delay(200);


}/*
  r2z1 = 5; r2z2 = -5; r2z3 = -7;//R1 zielkoordinate definierung

  r2v1 = (r2z1 - r2a1);//r1 vektor koordinaten
  r2v2 = (r2z2 - r2a2);
  r2v3 = (r2z3 - r2a3);

  r2V = sqrt((r2v1 * r2v1) + (r2v2 * r2v2) + (r2v3 * r2v3)); //bertrag von r1V vektor

  r2a = 180 * atan(r2v1 / r2v2) / PI;
  r2b = 180 * acos((r2V / 2) / l1) / PI; //cos() im Rad ==> a*180/PI=a°
  r2c = 180 - r2b * 2;

  R21.write(r2a); R22.write(r2b); R23.write(r2c);
  //check();
  //delay(1000);


}*/


void R1() {
  R11.attach(2);  R12.attach(3);  R13.attach(4);
  R11.write(000); R12.write(000); R13.write(000); delay(200);R11.write(180); R12.write(180); R13.write(180); delay(200);R11.write(90); R12.write(90); R13.write(90); delay(200);
}
void R2() {
  R21.attach(5);  R22.attach(6);  R23.attach(7);
  //R21.write(000); R22.write(000); R23.write(000); delay(200);R21.write(180); R22.write(180); R23.write(180); delay(200);R21.write(90); R22.write(90); R23.write(90); delay(200);
}

void L1() {
  L11.attach(8);  L12.attach(9);  L13.attach(10);
  //L11.write(000); L12.write(000); L13.write(000); delay(200);L11.write(180); L12.write(180); L13.write(180); delay(200);L11.write(90); L12.write(90); L13.write(90); delay(200);
}
void L2() {
  L21.attach(11);  L22.attach(12);  L23.attach(13);
  //L21.write(000); L22.write(000); L23.write(000); delay(200);L21.write(180); L22.write(180); L23.write(180); delay(200);L21.write(90); L22.write(90); L23.write(90); delay(200);
}

void check_r1() {
  Serial.print("r1a="); Serial.print(r1a);
  Serial.print(",r1b="); Serial.print(r1b);
  Serial.print(",r1c="); Serial.print(r1c);
  Serial.println();}
  void check_r2(){
  Serial.print("r2a="); Serial.print(r2a);
  Serial.print(",r2b="); Serial.print(r2b);
  Serial.print(",r2c="); Serial.print(r2c);
  Serial.println();}
  void check_l1(){
  Serial.print("l1a="); Serial.print(l1a);
  Serial.print(",l1b="); Serial.print(l1b);
  Serial.print(",l1c="); Serial.print(l1c);
  Serial.println();}
  void check_l2(){
  Serial.print("l2a="); Serial.print(l2a);
  Serial.print(",l2b="); Serial.print(l2b);
  Serial.print(",l2c="); Serial.print(l2c);
  Serial.println();Serial.println();
}