49 lines
2.1 KiB
C++
49 lines
2.1 KiB
C++
#include <iostream>
|
|
#include "NewGon.h"
|
|
|
|
int main ()
|
|
{
|
|
NewGon myGon;
|
|
// myGon.readParamFile("params.txt");
|
|
// myGon.printParams();
|
|
|
|
double mcs[9];
|
|
// SHx,SHy,SHz, CHI,PHI,OMEGA, Ox,Oy,Oz
|
|
double ucs1[] = {27.35e-3,0,0, 0,0, 0, 188e-3, 0, 0};
|
|
double ucs2[] = {27.35e-3,0,0, 0,0, 0, 188e-3, 10e-3, 0};
|
|
double ucs3[] = {27.35e-3,0,0, 0,0, 0, 188e-3, 10e-3, 10e-3};
|
|
double ucs4[] = {27.35e-3,0,0, 0,0, 0, 188e-3,-10e-3, 10e-3};
|
|
double ucs5[] = {27.35e-3,0,0, 0,0, 0, 188e-3,-10e-3,-10e-3};
|
|
double ucs6[] = {27.35e-3,0,0, 0,0,10e-3, 188e-3, 10e-3, 0};
|
|
double ucs7[] = {27.35e-3,0,0, 20,0, 0, 188e-3, 0, 0};
|
|
double ucs8[] = {27.35e-3,0,0, 40,0, 0, 188e-3, 0, 0};
|
|
double ucs9[] = {27.35e-3,0,0, 50,0, 0, 188e-3, 0, 0};
|
|
double ucs10[]= {27.35e-3,0,0, 60,0, 0, 188e-3, 0, 0};
|
|
double ucs11[]= {27.35e-3,0,0, 70,0, 0, 188e-3, 0, 0};
|
|
double ucs12[]= {27.35e-3,0,0, 80,90, 0, 188e-3, 0, 0};
|
|
double ucs13[]= {27.35e-3,0,0, 90,0, 0, 188e-3, 0, 0};
|
|
|
|
|
|
|
|
// motor values: q = (q1, q2,q3,q(theta), q5,q6)
|
|
myGon.IK(ucs1, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs2, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs3, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs4, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs5, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs6, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs7, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs8, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs9, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs10,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs11,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs12,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
myGon.IK(ucs13,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
|
|
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
|