Delta robot

 #include <Servo.h>
  Servo  m1,m2, m3;
  double sP= 76,  Len=524,  len= 1244;
  double wB=164,  wP=22,  uP=44;
  double QHori1=0, QHori2=0, QHori3=0;
  double Pi= 3.141592;

   void setup() {
  m1.attach(3);
  m2.attach(6);
  m3.attach(9);
  Serial.begin(9600);
  Serial.setTimeout(0);
   }

  void loop() {
  
      MoveToXYZ(0,0,-1064.25, 1000);  // Len 이 수평일때의 위치임
       MoveToXYZ(0,0,-1064.30,1000);  
      MoveToXYZ(0,0,-1064.15,1000);
   while (true);
  }

  //주어진 end점 이송후 대기
   void MoveToXYZ(double x, double y, double z, double msec)
   { double Q1Deg, Q2Deg, Q3Deg;
      bool  success = InverseKin(x,y,z, Q1Deg, Q2Deg, Q3Deg);
       if( !success) return;
      double QMotor1Deg, QMotor2Deg, QMotor3Deg;
      MotorDegFromInverKIn(Q1Deg, Q2Deg, Q3Deg, QMotor1Deg, QMotor2Deg, QMotor3Deg);
      m1.write(QMotor1Deg);
      m2.write(QMotor2Deg);
      m3.write(QMotor3Deg);
 String str= "QMotor1Deg="+String(QMotor1Deg)+"   QMotor2Deg =" + String(QMotor2Deg)
                +"   QMotor3Deg:=   "+String(QMotor3Deg);
    Serial.println(str);
      delay(msec);
    }

  //역기구학 풀이 각도를 모터 명령 각도로 전환
    void  MotorDegFromInverKIn(double Q1Deg, double Q2Deg, double Q3Deg,  
          double& QMotor1Deg, double& QMotor2Deg, double& QMotor3Deg)
          { QMotor1Deg= -Q1Deg + QHori1;
            QMotor2Deg= -Q2Deg + QHori2;
            QMotor3Deg= -Q3Deg + QHori3;
          }
    //DeltaRobot의 역기구학 풀이
     bool InverseKin(double x, double y, double z, double& Q1Deg, double& Q2Deg, double& Q3Deg)
       { double a= wB-uP;
         double b= sP/2.0- sqrt(3)/2.0*wB;
         double c= wP- wB/2.0;
         double x2=x*x,  y2= y*y, z2=z*z;
         double a2= a*a, b2= b*b, c2= c*c;
         double Len2 = Len*Len,  len2= len*len;

         double E1= 2 * Len*(y+a);
         double F1= 2*z* Len;
         double G1= x2+y2+z2+a2+Len2+2*y*a-len2;

         double E2= -Len*(sqrt(3)*(x+b)+y+c);
         double F2= 2*z*Len;
         double G2= x2+y2+z2+b2+c2+Len2+2*(x*b+y*c)-len2;

         double E3= Len*(sqrt(3)*(x-b)-y-c);
         double F3= 2*z*Len;
         double G3= x2+y2+z2+b2+c2+Len2+2*(-x*b+y*c)-len2;

         double D1= E1*E1+ F1*F1-G1*G1;
         double D2= E2*E2+ F2*F2-G2*G2;
         double D3= E3*E3+F3*F3-G3*G3;
       //음수이면 허근이다
         if(D1<0 || D2<0 || D3<0) {Q1Deg=  Q2Deg = Q3Deg= 0;  return;}
         double t11= (-F1+sqrt(D1))/(G1-E1);
         double t12= (-F1-sqrt(D1))/(G1-E1); 

         double t21= (-F2+sqrt(D2))/(G2-E2);
         double t22= (-F2-sqrt(D2))/(G2-E2);

         double t31= (-F3+sqrt(D3))/(G3-E3);
         double t32= (-F3-sqrt(D3))/(G3-E3);

         double Q1Rad1= 2*atan(t11);
         double Q1Rad2= 2*atan(t12);
         double Q2Rad1= 2*atan(t21);
         double Q2Rad2= 2*atan(t22);
         double Q3Rad1= 2*atan(t31);
         double Q3Rad2= 2*atan(t32);

         //Q1Deg   팔이 밖으로 벌어질때 해을 얻는다(최소값 선택)
         if(abs(Q1Rad1) <= abs(Q1Rad2)) Q1Deg= Q1Rad1*180/Pi;
         else Q1Deg= Q1Rad2 *180/Pi;
         //Q2Deg
         if(abs(Q2Rad1) <= abs(Q2Rad2)) Q2Deg= Q2Rad1*180/Pi;
         else Q2Deg= Q2Rad2 *180/Pi;
         //Q3Deg
         if(abs(Q3Rad1) <= abs(Q3Rad2)) Q3Deg= Q3Rad1*180/Pi;
         else Q3Deg= Q3Rad2 *180/Pi;
          return true;
       }