델타로봇제어

#include <Servo.h>

Servo m1, m2, m3;

double sP = 43.3013, Len = 76.4853, len = 150;

double wB = 31.4620, wP = 12.5, uP = 25;

double QHori1 = 113, QHori2 = 110, QHori3 = 116;

// 수평일 때 각도

void setup() {

m1.attach(3);

m2.attach(6);

m3.attach(9);

Serial.begin(9600);

Serial.setTimeout(0);

}

void loop() {

int icase = 1; // 1 ~ 6

// case 1 : Up-Down

if(icase == 1)

{

MoveToXYZ(0, 0, -150, 1000);

MoveToXYZ(0, 0, -180, 1000);

}

// case 2 : X-Motion

else if(icase == 2)

{

MoveToXYZ(50, 0, -150, 1000);

MoveToXYZ(-50, 0, -150, 1000);

}

// case 3 : Y-Motion

else if(icase == 3)

{

MoveToXYZ(0, 50, -150, 1000);

MoveToXYZ(0, -50, -150, 1000);

}

// case 4 : Rectanble

else if(icase == 4)

{

MoveToXYZ(0, 0, -150, 1000);

MoveToXYZ(-50, -50, -150, 1000);

MoveToXYZ(50, -50, -150, 1000);

MoveToXYZ(50, 50, -150, 1000);

MoveToXYZ(-50, 50, -150, 1000);

MoveToXYZ(-50, -50, -150, 1000);

}

// case 5 : Circle

else if(icase == 5)

{

for(int i = 0; i < 360; i+=5)

{

double rad = i*PI/180.0;

double R = 50;

double x = R * cos(rad);

double y = R * sin(rad);

double z = -150;

MoveToXYZ(x, y, z, 15);

}

}

// case 6 : Star

else if(icase == 6)

{

for(int i = 0; i < 360; i+=60)

{

double zup = -145;

double zdown = -150;

MoveToXYZ(0, 0, zup, 500);

double rad = i*PI/180.0;

double R = 50;

double x = R * cos(rad);

double y = R * sin(rad);

MoveToXYZ(x, y, zup, 500);

MoveToXYZ(x, y, zdown, 500);

MoveToXYZ(x, y, zup, 500);

}

}

}

//----- 주어진 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;

MotorDegFromInvKin(Q1Deg, Q2Deg, Q3Deg,

QMotor1Deg, QMotor2Deg, QMotor3Deg);

m1.write(QMotor1Deg);

m2.write(QMotor2Deg);

m3.write(QMotor3Deg);

delay(msec);

}

//----- 역기구학 풀이 각도를 모터 명령 각도로 전

환 -----

void MotorDegFromInvKin(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;

double y2 = y * y;

double z2 = z * z;

double a2 = a * a;

double b2 = b * b;

double c2 = c * c;

double Len2 = Len * Len;

double 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 false;

}

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;

}