6. Ustawienia ogólne robota

6.1. Ustawienie punktu odniesienia narzędzia - metoda sześciopunktowa

1/**
2 * @brief Ustawia punkt odniesienia narzędzia - metoda sześciopunktowa
3 * @param [in] point_num Numer punktu, zakres [1~6]
4 * @return Kod błędu
5 */
6errno_t SetToolPoint(int point_num);

6.2. Obliczenie układu współrzędnych narzędzia

1/**
2 * @brief  Oblicza układ współrzędnych narzędzia
3 * @param [out] tcp_pose Układ współrzędnych narzędzia
4 * @return Kod błędu
5 */
6errno_t ComputeTool(DescPose *tcp_pose);

6.3. Ustawienie punktu odniesienia narzędzia - metoda czteropunktowa

1/**
2 * @brief Ustawia punkt odniesienia narzędzia - metoda czteropunktowa
3 * @param [in] point_num Numer punktu, zakres [1~4]
4 * @return Kod błędu
5 */
6errno_t SetTcp4RefPoint(int point_num);

6.4. Obliczenie układu współrzędnych narzędzia

1/**
2 * @brief  Oblicza układ współrzędnych narzędzia
3 * @param [out] tcp_pose Układ współrzędnych narzędzia
4 * @return Kod błędu
5 */
6errno_t ComputeTcp4(DescPose *tcp_pose);

6.5. Obliczenie układu współrzędnych narzędzia na podstawie punktów

Nowe w wersji C++SDK-v2.1.8-3.7.8.

1/**
2 * @brief Oblicza układ współrzędnych narzędzia na podstawie punktów
3 * @param [in] method Metoda obliczeniowa; 0-metoda czteropunktowa; 1-metoda sześciopunktowa
4 * @param [in] pos Grupa pozycji stawów, długość tablicy 4 dla metody czteropunktowej, długość 6 dla metody sześciopunktowej
5 * @param [out] coord Wynik układu współrzędnych narzędzia
6 * @return Kod błędu
7 */
8errno_t ComputeToolCoordWithPoints(int method, JointPos pos[], DescPose& coord);

6.6. Ustawienie układu współrzędnych narzędzia

Zmienione w wersji C++SDK-v2.1.5.0.

 1/**
 2 * @brief  Ustawia układ współrzędnych narzędzia
 3 * @param  [in] id Numer układu współrzędnych, zakres [0~14]
 4 * @param  [in] coord  Pozycja środka narzędzia względem środka kołnierza końcowego
 5 * @param  [in] type  0-układ współrzędnych narzędzia, 1-układ współrzędnych czujnika
 6 * @param  [in] install Pozycja instalacji, 0-koniec robota, 1-na zewnątrz robota
 7 * @param  [in] toolID ID narzędzia
 8 * @param  [in] loadNum Numer obciążenia
 9 * @return  Kod błędu
10 */
11errno_t SetToolCoord(int id, DescPose *coord, int type, int install, int toolID, int loadNum);

6.7. Ustawienie listy układów współrzędnych narzędzia

Zmienione w wersji C++SDK-v2.1.5.0.

 1/**
 2 * @brief  Ustawia listę układów współrzędnych narzędzia
 3 * @param  [in] id Numer układu współrzędnych, zakres [0~14]
 4 * @param  [in] coord  Pozycja środka narzędzia względem środka kołnierza końcowego
 5 * @param  [in] type  0-układ współrzędnych narzędzia, 1-układ współrzędnych czujnika
 6 * @param  [in] install Pozycja instalacji, 0-koniec robota, 1-na zewnątrz robota
 7 * @param  [in] loadNum Numer obciążenia
 8 * @return  Kod błędu
 9 */
10errno_t SetToolList(int id, DescPose *coord, int type, int install, int loadNum);

6.8. Pobranie bieżącego układu współrzędnych narzędzia

1/**
2* @brief  Pobiera bieżący układ współrzędnych narzędzia
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] desc_pos Pozycja i orientacja układu współrzędnych narzędzia
5* @return  Kod błędu
6*/
7errno_t GetTCPOffset(uint8_t flag, DescPose *desc_pos);

6.9. Przykład kodu operacji na układzie współrzędnych narzędzia robota

 1 int TestTCPCompute(void)
 2 {
 3     ROBOT_STATE_PKG pkg = {};
 4     FRRobot robot;
 5     robot.LoggerInit();
 6     robot.SetLoggerLevel(1);
 7     int rtn = robot.RPC("192.168.58.2");
 8     if (rtn != 0)
 9     {
10         return -1;
11     }
12     robot.SetReConnectParam(true, 30000, 500);
13     DescPose p1Desc(186.331, 487.913, 209.850, 149.030, 0.688, -114.347);
14     JointPos p1Joint(-127.876, -75.341, 115.417, -122.741, -59.820, 74.300);
15     DescPose p2Desc(69.721, 535.073, 202.882, -144.406, -14.775, -89.012);
16     JointPos p2Joint(-101.780, -69.828, 110.917, -125.740, -127.841, 74.300);
17     DescPose p3Desc(146.861, 578.426, 205.598, 175.997, -36.178, -93.437);
18     JointPos p3Joint(-112.851, -60.191, 86.566, -80.676, -97.463, 74.300);
19     DescPose p4Desc(136.284, 509.876, 225.613, 178.987, 1.372, -100.696);
20     JointPos p4Joint(-116.397, -76.281, 113.845, -128.611, -88.654, 74.299);
21     DescPose p5Desc(138.395, 505.972, 298.016, 179.134, 2.147, -101.110);
22     JointPos p5Joint(-116.814, -82.333, 109.162, -118.662, -88.585, 74.302);
23     DescPose p6Desc(105.553, 454.325, 232.017, -179.426, 0.444, -99.952);
24     JointPos p6Joint(-115.649, -84.367, 122.447, -128.663, -90.432, 74.303);
25     ExaxisPos exaxisPos(0, 0, 0, 0);
26     DescPose offdese(0, 0, 0, 0, 0, 0);
27     JointPos posJ[6] = { p1Joint , p2Joint , p3Joint , p4Joint , p5Joint , p6Joint };
28     DescPose coordRtn = {};
29     rtn = robot.ComputeToolCoordWithPoints(1, posJ, coordRtn);
30     printf("ComputeToolCoordWithPoints    %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
31     robot.MoveJ(&p1Joint, &p1Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
32     robot.SetToolPoint(1);
33     robot.MoveJ(&p2Joint, &p2Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
34     robot.SetToolPoint(2);
35     robot.MoveJ(&p3Joint, &p3Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
36     robot.SetToolPoint(3);
37     robot.MoveJ(&p4Joint, &p4Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
38     robot.SetToolPoint(4);
39     robot.MoveJ(&p5Joint, &p5Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
40     robot.SetToolPoint(5);
41     robot.MoveJ(&p6Joint, &p6Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
42     robot.SetToolPoint(6);
43     rtn = robot.ComputeTool(&coordRtn);
44     printf("6 Point ComputeTool        %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
45     robot.SetToolList(1, &coordRtn, 0, 0, 0);
46     robot.MoveJ(&p1Joint, &p1Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
47     robot.SetTcp4RefPoint(1);
48     robot.MoveJ(&p2Joint, &p2Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
49     robot.SetTcp4RefPoint(2);
50     robot.MoveJ(&p3Joint, &p3Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
51     robot.SetTcp4RefPoint(3);
52     robot.MoveJ(&p4Joint, &p4Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
53     robot.SetTcp4RefPoint(4);
54     rtn = robot.ComputeTcp4(&coordRtn);
55     printf("4 Point ComputeTool        %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
56     robot.SetToolCoord(2, &coordRtn, 0, 0, 1, 0);
57     DescPose getCoord = {};
58     rtn = robot.GetTCPOffset(0, &getCoord);
59     printf("GetTCPOffset    %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
60     robot.CloseRPC();
61     return 0;
62 }

6.10. Ustawienie punktu odniesienia zewnętrznego narzędzia - metoda sześciopunktowa

1/**
2 * @brief Ustawia punkt odniesienia zewnętrznego narzędzia - metoda sześciopunktowa
3 * @param [in] point_num Numer punktu, zakres [1~4]
4 * @return Kod błędu
5 */
6errno_t SetExTCPPoint(int point_num);

6.11. Obliczenie zewnętrznego układu współrzędnych narzędzia

1/**
2 * @brief  Oblicza zewnętrzny układ współrzędnych narzędzia
3 * @param [out] tcp_pose Zewnętrzny układ współrzędnych narzędzia
4 * @return Kod błędu
5 */
6errno_t ComputeExTCF(DescPose *tcp_pose);

6.12. Ustawienie zewnętrznego układu współrzędnych narzędzia

Zmienione w wersji C++SDK-v2.1.2.0.

1/**
2* @brief  Ustawia zewnętrzny układ współrzędnych narzędzia
3* @param  [in] id Numer układu współrzędnych, zakres [0~14]
4* @param  [in] etcp  Pozycja środka narzędzia względem środka kołnierza końcowego
5* @param  [in] etool  Do określenia
6* @return  Kod błędu
7*/
8errno_t SetExToolCoord(int id, DescPose *etcp, DescPose *etool);

6.13. Ustawienie listy zewnętrznych układów współrzędnych narzędzia

Zmienione w wersji C++SDK-v2.1.2.0.

1/**
2* @brief  Ustawia listę zewnętrznych układów współrzędnych narzędzia
3* @param  [in] id Numer układu współrzędnych, zakres [0~14]
4* @param  [in] etcp  Pozycja środka narzędzia względem środka kołnierza końcowego
5* @param  [in] etool  Do określenia
6* @return  Kod błędu
7*/
8errno_t SetExToolList(int id, DescPose *etcp, DescPose *etool);

6.14. Przykład kodu operacji na zewnętrznym układzie współrzędnych narzędzia robota

 1int TestExtCoord(void)
 2{
 3   ROBOT_STATE_PKG pkg = {};
 4   FRRobot robot;
 5   robot.LoggerInit();
 6   robot.SetLoggerLevel(1);
 7   int rtn = robot.RPC("192.168.58.2");
 8   if (rtn != 0)
 9   {
10      return -1;
11   }
12   robot.SetReConnectParam(true, 30000, 500);
13   DescPose p1Desc(-89.606, 779.517, 193.516, 178.000, 0.476, -92.484);
14   JointPos p1Joint(-108.145, -50.137, 85.818, -125.599, -87.946, 74.329);
15   DescPose p2Desc(-24.656, 850.384, 191.361, 177.079, -2.058, -95.355);
16   JointPos p2Joint(-111.024, -41.538, 69.222, -114.913, -87.743, 74.329);
17   DescPose p3Desc(-99.813, 766.661, 241.878, -176.817, 1.917, -91.604);
18   JointPos p3Joint(-107.266, -56.116, 85.971, -122.560, -92.548, 74.331);
19   ExaxisPos exaxisPos(0, 0, 0, 0);
20   DescPose offdese(0, 0, 0, 0, 0, 0);
21   DescPose posTCP[3] = { p1Desc , p2Desc , p3Desc };
22   DescPose coordRtn = {};
23   robot.MoveJ(&p1Joint, &p1Desc, 1, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
24   robot.SetExTCPPoint(1);
25   robot.MoveJ(&p2Joint, &p2Desc, 1, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
26   robot.SetExTCPPoint(2);
27   robot.MoveJ(&p3Joint, &p3Desc, 1, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
28   robot.SetExTCPPoint(3);
29   rtn = robot.ComputeExTCF(&coordRtn);
30   printf("ComputeExTCF          %d coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
31   robot.SetExToolCoord(1, &coordRtn, &offdese);
32   robot.SetExToolList(1, &coordRtn, &offdese);
33   robot.CloseRPC();
34   return 0;
35}

6.15. Ustawienie punktu odniesienia przedmiotu - metoda trzypunktowa

1/**
2 * @brief Ustawia punkt odniesienia przedmiotu - metoda trzypunktowa
3 * @param [in] point_num Numer punktu, zakres [1~3]
4 * @return Kod błędu
5 */
6errno_t SetWObjCoordPoint(int point_num);

6.16. Obliczenie układu współrzędnych przedmiotu

Zmienione w wersji C++SDK-v2.1.5.0.

1/**
2 * @brief  Oblicza układ współrzędnych przedmiotu
3 * @param [in] method Metoda obliczeniowa 0: początek - oś X - oś Z  1: początek - oś X - płaszczyzna XY
4 * @param [in] refFrame Układ odniesienia
5 * @param [out] wobj_pose Układ współrzędnych przedmiotu
6 * @return Kod błędu
7 */
8errno_t ComputeWObjCoord(int method, int refFrame, DescPose *wobj_pose);

6.17. Ustawienie układu współrzędnych przedmiotu

Zmienione w wersji C++SDK-v2.1.5.0.

1/**
2 * @brief  Ustawia układ współrzędnych przedmiotu
3 * @param  [in] id Numer układu współrzędnych, zakres [0~14]
4 * @param  [in] coord  Pozycja układu współrzędnych przedmiotu względem środka kołnierza końcowego
5 * @param  [in] refFrame Układ odniesienia
6 * @return  Kod błędu
7 */
8errno_t SetWObjCoord(int id, DescPose *coord, int refFrame);

6.18. Ustawienie listy układów współrzędnych przedmiotu

Zmienione w wersji C++SDK-v2.1.5.0.

1/**
2 * @brief  Ustawia listę układów współrzędnych przedmiotu
3 * @param  [in] id Numer układu współrzędnych, zakres [0~14]
4 * @param  [in] coord  Pozycja układu współrzędnych przedmiotu względem środka kołnierza końcowego
5 * @param  [in] refFrame Układ odniesienia
6 * @return  Kod błędu
7 */
8errno_t SetWObjList(int id, DescPose *coord, int refFrame);

6.19. Obliczenie układu współrzędnych przedmiotu na podstawie punktów

Nowe w wersji C++SDK-v2.1.8-3.7.8.

1/**
2 * @brief Oblicza układ współrzędnych przedmiotu na podstawie punktów
3 * @param [in] method Metoda obliczeniowa; 0: początek - oś X - oś Z  1: początek - oś X - płaszczyzna XY
4 * @param [in] pos Trzy grupy pozycji TCP
5 * @param [in] refFrame Układ odniesienia
6 * @param [out] coord Wynik układu współrzędnych narzędzia
7 * @return Kod błędu
8 */
9errno_t ComputeWObjCoordWithPoints(int method, DescPose pos[], int refFrame, DescPose& coord);

6.20. Pobranie bieżącego układu współrzędnych przedmiotu

1/**
2* @brief  Pobiera bieżący układ współrzędnych przedmiotu
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] desc_pos Pozycja i orientacja układu współrzędnych przedmiotu
5* @return  Kod błędu
6*/
7errno_t GetWObjOffset(uint8_t flag, DescPose *desc_pos);

6.21. Przykład kodu operacji na układzie współrzędnych przedmiotu robota

 1 int TestWobjCoord(void)
 2 {
 3     ROBOT_STATE_PKG pkg = {};
 4     FRRobot robot;
 5     robot.LoggerInit();
 6     robot.SetLoggerLevel(1);
 7     int rtn = robot.RPC("192.168.58.2");
 8     if (rtn != 0)
 9     {
10         return -1;
11     }
12     robot.SetReConnectParam(true, 30000, 500);
13     DescPose p1Desc(-89.606, 779.517, 193.516, 178.000, 0.476, -92.484);
14     JointPos p1Joint(-108.145, -50.137, 85.818, -125.599, -87.946, 74.329);
15     DescPose p2Desc(-24.656, 850.384, 191.361, 177.079, -2.058, -95.355);
16     JointPos p2Joint(-111.024, -41.538, 69.222, -114.913, -87.743, 74.329);
17     DescPose p3Desc(-99.813, 766.661, 241.878, -176.817, 1.917, -91.604);
18     JointPos p3Joint(-107.266, -56.116, 85.971, -122.560, -92.548, 74.331);
19     ExaxisPos exaxisPos(0, 0, 0, 0);
20     DescPose offdese(0, 0, 0, 0, 0, 0);
21     DescPose posTCP[3] = { p1Desc , p2Desc , p3Desc };
22     DescPose coordRtn = {};
23     rtn = robot.ComputeWObjCoordWithPoints(1, posTCP, 0, coordRtn);
24     printf("ComputeWObjCoordWithPoints    %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
25     robot.MoveJ(&p1Joint, &p1Desc, 1, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
26     robot.SetWObjCoordPoint(1);
27     robot.MoveJ(&p2Joint, &p2Desc, 1, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
28     robot.SetWObjCoordPoint(2);
29     robot.MoveJ(&p3Joint, &p3Desc, 1, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
30     robot.SetWObjCoordPoint(3);
31     rtn = robot.ComputeWObjCoord(1, 0, &coordRtn);
32     printf("ComputeWObjCoord                   %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
33     robot.SetWObjCoord(1, &coordRtn, 0);
34     robot.SetWObjList(1, &coordRtn, 0);
35     DescPose getWobjDesc = {};
36     rtn = robot.GetWObjOffset(0, &getWobjDesc);
37     printf("GetWObjOffset                   %d  coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
38     robot.CloseRPC();
39     return 0;
40 }

6.22. Ustawienie prędkości globalnej

1/**
2* @brief  Ustawia prędkość globalną
3* @param  [in] vel  Procent prędkości, zakres [0~100]
4* @return  Kod błędu
5*/
6errno_t SetSpeed(int vel);

6.23. Ustawienie przyspieszenia robota

1/**
2 * @brief Ustawia przyspieszenie robota
3 * @param [in] acc Procent przyspieszenia robota
4 * @return Kod błędu
5 */
6errno_t SetOaccScale(double acc);

6.24. Pobranie domyślnej prędkości robota

1/**
2* @brief  Pobiera domyślną prędkość robota
3* @param  [out] vel  Prędkość, jednostka mm/s
4* @return  Kod błędu
5*/
6errno_t GetDefaultTransVel(float *vel);

6.25. Ustawienie masy ładunku końcowego

Zmienione w wersji C++SDK-v2.1.8-3.7.8.

1/**
2 * @brief  Ustawia masę ładunku końcowego
3 * @param  [in] loadNum Numer obciążenia
4 * @param  [in] weight  Masa ładunku, jednostka kg
5 * @return  Kod błędu
6 */
7errno_t SetLoadWeight(int loadNum = 0, float weight);

6.26. Ustawienie współrzędnych środka ciężkości ładunku końcowego

Nowe w wersji C++SDK-v3.8.6.

1/**
2* @brief Ustawia współrzędne środka ciężkości ładunku końcowego
3* @param [in] loadNum Numer obciążenia
4* @param [in] coord Współrzędne środka ciężkości, jednostka mm
5* @return Kod błędu
6*/
7errno_t SetLoadCoord(int loadNum, DescTran* coord);

6.27. Pobranie masy bieżącego ładunku

1/**
2* @brief  Pobiera masę bieżącego ładunku
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] weight Masa ładunku, jednostka kg
5* @return  Kod błędu
6*/
7errno_t GetTargetPayload(uint8_t flag, float *weight);

6.28. Pobranie środka ciężkości bieżącego ładunku

1/**
2* @brief  Pobiera środek ciężkości bieżącego ładunku
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] cog Środek ciężkości ładunku, jednostka mm
5* @return  Kod błędu
6*/
7errno_t GetTargetPayloadCog(uint8_t flag, DescTran *cog);

6.29. Ustawienie sposobu instalacji robota

1/**
2* @brief  Ustawia sposób instalacji robota
3* @param  [in] install  Sposób instalacji, 0-instalacja normalna, 1-instalacja boczna, 2-instalacja odwrócona
4* @return  Kod błędu
5*/
6errno_t SetRobotInstallPos(uint8_t install);

6.30. Ustawienie kąta instalacji robota

1/**
2* @brief  Ustawia kąt instalacji robota, instalacja swobodna
3* @param  [in] yangle  Kąt nachylenia
4* @param  [in] zangle  Kąt obrotu
5* @return  Kod błędu
6*/
7errno_t SetRobotInstallAngle(double yangle, double zangle);

6.31. Pobranie kąta instalacji robota

1/**
2* @brief  Pobiera kąt instalacji robota
3* @param  [out] yangle Kąt nachylenia
4* @param  [out] zangle Kąt obrotu
5* @return  Kod błędu
6*/
7errno_t GetRobotInstallAngle(float *yangle, float *zangle);

6.32. Ustawienie wartości zmiennej systemowej

1/**
2* @brief  Ustawia wartość zmiennej systemowej
3* @param  [in] id  Numer zmiennej, zakres [1~20]
4* @param  [in] value Wartość zmiennej
5* @return  Kod błędu
6*/
7errno_t SetSysVarValue(int id, float value);

6.33. Pobranie wartości zmiennej systemowej

1/**
2* @brief  Pobiera wartość zmiennej systemowej
3* @param  [in] id Numer zmiennej systemowej, zakres [1~20]
4* @param  [out] value  Wartość zmiennej systemowej
5* @return  Kod błędu
6*/
7errno_t GetSysVarValue(int id, float *value);

6.34. Przykład kodu ustawień ogólnych robota

 1 int TestLoadInstall(void)
 2 {
 3     ROBOT_STATE_PKG pkg = {};
 4     FRRobot robot;
 5     robot.LoggerInit();
 6     robot.SetLoggerLevel(1);
 7     int rtn = robot.RPC("192.168.58.2");
 8     if (rtn != 0)
 9     {
10         return -1;
11     }
12     robot.SetReConnectParam(true, 30000, 500);
13     for (int i = 1; i < 100; i++)
14     {
15         robot.SetSpeed(i);
16         robot.SetOaccScale(i);
17         robot.Sleep(30);
18     }
19     float defaultVel = 0.0;
20     robot.GetDefaultTransVel(&defaultVel);
21     printf("GetDefaultTransVel is %f\n", defaultVel);
22     for (int i = 1; i < 21; i++)
23     {
24         robot.SetSysVarValue(i, i + 0.5);
25         robot.Sleep(100);
26     }
27     for (int i = 1; i < 21; i++)
28     {
29         float value = 0;
30         robot.GetSysVarValue(i, &value);
31         printf("sys value  %d is :%f\n", i, value);
32         robot.Sleep(100);
33     }
34     robot.SetLoadWeight(0, 2.5);
35     DescTran loadCoord = {};
36     loadCoord.x = 3.0;
37     loadCoord.y = 4.0;
38     loadCoord.z = 5.0;
39     robot.SetLoadCoord(&loadCoord);
40     robot.Sleep(1000);
41     float getLoad = 0.0;
42     robot.GetTargetPayload(0, &getLoad);
43     DescTran getLoadTran = {};
44     robot.GetTargetPayloadCog(0, &getLoadTran);
45     printf("get load is %f; get load cog is %f %f %f\n", getLoad, getLoadTran.x, getLoadTran.y, getLoadTran.z);
46     robot.SetRobotInstallPos(0);
47     robot.SetRobotInstallAngle(15.0, 25.0);
48     float anglex = 0.0;
49     float angley = 0.0;
50     robot.GetRobotInstallAngle(&anglex, &angley);
51     printf("GetRobotInstallAngle x:  %f;  y:  %f\n", anglex, angley);
52     robot.CloseRPC();
53     return 0;
54 }

6.35. Przełącznik kompensacji tarcia stawów

1/**
2* @brief  Przełącznik kompensacji tarcia stawów
3* @param  [in] state  0-wył., 1-wł.
4* @return  Kod błędu
5*/
6errno_t FrictionCompensationOnOff(uint8_t state);

6.36. Ustawienie współczynnika kompensacji tarcia stawów - instalacja normalna

1/**
2* @brief  Ustawia współczynnik kompensacji tarcia stawów - instalacja normalna
3* @param  [in] coeff Współczynniki kompensacji dla sześciu stawów, zakres [0~1]
4* @return  Kod błędu
5*/
6errno_t SetFrictionValue_level(float coeff[6]);

6.37. Ustawienie współczynnika kompensacji tarcia stawów - instalacja boczna

1/**
2* @brief  Ustawia współczynnik kompensacji tarcia stawów - instalacja boczna
3* @param  [in] coeff Współczynniki kompensacji dla sześciu stawów, zakres [0~1]
4* @return  Kod błędu
5*/
6errno_t SetFrictionValue_wall(float coeff[6]);

6.38. Ustawienie współczynnika kompensacji tarcia stawów - instalacja odwrócona

1/**
2* @brief  Ustawia współczynnik kompensacji tarcia stawów - instalacja odwrócona
3* @param  [in] coeff Współczynniki kompensacji dla sześciu stawów, zakres [0~1]
4* @return  Kod błędu
5*/
6errno_t SetFrictionValue_ceiling(float coeff[6]);

6.39. Ustawienie współczynnika kompensacji tarcia stawów - instalacja swobodna

1/**
2* @brief  Ustawia współczynnik kompensacji tarcia stawów - instalacja swobodna
3* @param  [in] coeff Współczynniki kompensacji dla sześciu stawów, zakres [0~1]
4* @return  Kod błędu
5*/
6errno_t SetFrictionValue_freedom(float coeff[6]);

6.40. Przykład kodu ustawiania kompensacji tarcia stawów robota

 1int TestFriction(void)
 2{
 3   ROBOT_STATE_PKG pkg = {};
 4   FRRobot robot;
 5
 6   robot.LoggerInit();
 7   robot.SetLoggerLevel(1);
 8   int rtn = robot.RPC("192.168.58.2");
 9   if (rtn != 0)
10   {
11      return -1;
12   }
13   robot.SetReConnectParam(true, 30000, 500);
14   float lcoeff[6] = { 0.9,0.9,0.9,0.9,0.9,0.9 };
15   float wcoeff[6] = { 0.4,0.4,0.4,0.4,0.4,0.4 };
16   float ccoeff[6] = { 0.6,0.6,0.6,0.6,0.6,0.6 };
17   float fcoeff[6] = { 0.5,0.5,0.5,0.5,0.5,0.5 };
18   rtn = robot.FrictionCompensationOnOff(1);
19   printf("FrictionCompensationOnOff rtn is %d\n", rtn);
20   rtn = robot.SetFrictionValue_level(lcoeff);
21   printf("SetFrictionValue_level rtn is %d\n", rtn);
22   rtn = robot.SetFrictionValue_wall(wcoeff);
23   printf("SetFrictionValue_wall rtn is %d\n", rtn);
24   rtn = robot.SetFrictionValue_ceiling(ccoeff);
25   printf("SetFrictionValue_ceiling rtn is %d\n", rtn);
26   rtn = robot.SetFrictionValue_freedom(fcoeff);
27   printf("SetFrictionValue_freedom rtn is %d\n", rtn);
28   robot.CloseRPC();
29   return 0;
30}

6.41. Pobranie kodu błędu robota

1/**
2 * @brief  Pobiera kod błędu robota
3 * @param  [out] maincode   Główny kod błędu
4 * @param  [out] subcode    Podrzędny kod błędu
5 * @return  Kod błędu
6 */
7errno_t GetRobotErrorCode(int *maincode, int *subcode);

6.42. Wyczyść błędy

1/**
2* @brief  Czyści stany błędów
3* @return  Kod błędu
4*/
5errno_t ResetAllError();

6.43. Przykład kodu pobierania stanu awaryjnego i czyszczenia błędów robota

 1int TestGetError(void)
 2{
 3   ROBOT_STATE_PKG pkg = {};
 4   FRRobot robot;
 5   robot.LoggerInit();
 6   robot.SetLoggerLevel(1);
 7   int rtn = robot.RPC("192.168.58.2");
 8   if (rtn != 0)
 9   {
10      return -1;
11   }
12   robot.SetReConnectParam(true, 30000, 500);
13   int maincode, subcode;
14   robot.GetRobotErrorCode(&maincode, &subcode);
15   printf("robot maincode is %d; subcode is %d\n", maincode, subcode);
16   robot.ResetAllError();
17   robot.Sleep(1000);
18   robot.GetRobotErrorCode(&maincode, &subcode);
19   printf("robot maincode is %d; subcode is %d\n", maincode, subcode);
20   robot.CloseRPC();
21   return 0;
22}

6.44. Ustawienie parametrów monitorowania temperatury i prądu wentylatora skrzynki sterowniczej szerokiego napięcia

1/**
2* @brief Ustawia parametry monitorowania temperatury i prądu wentylatora skrzynki sterowniczej szerokiego napięcia
3* @param [in] enable 0-nie włączaj monitorowania; 1-włącz monitorowanie
4* @param [in] period Okres monitorowania (s), zakres 1-100
5* @return Kod błędu
6*/
7errno_t SetWideBoxTempFanMonitorParam(int enable, int period);

6.45. Pobranie parametrów monitorowania temperatury i prądu wentylatora skrzynki sterowniczej szerokiego napięcia

1/**
2* @brief Pobiera parametry monitorowania temperatury i prądu wentylatora skrzynki sterowniczej szerokiego napięcia
3* @param [out] enable 0-nie włączaj monitorowania; 1-włącz monitorowanie
4* @param [out] period Okres monitorowania (s), zakres 1-100
5* @return Kod błędu
6*/
7errno_t GetWideBoxTempFanMonitorParam(int &enable, int &period);

6.46. Przykład kodu pobierania temperatury i prądu wentylatora skrzynki sterowniczej szerokiego napięcia

 1 int TestWideVoltageCtrlBoxtemp(void)
 2 {
 3     ROBOT_STATE_PKG pkg = {};
 4     FRRobot robot;
 5     robot.LoggerInit();
 6     robot.SetLoggerLevel(1);
 7     int rtn = robot.RPC("192.168.58.2");
 8     printf("robot rpc rtn is %d\n", rtn);
 9     if (rtn != 0)
10     {
11         return -1;
12     }
13     robot.SetReConnectParam(true, 30000, 500);
14     robot.SetWideBoxTempFanMonitorParam(1, 2);
15     int enable = 0;
16     int period = 0;
17     robot.GetWideBoxTempFanMonitorParam(enable, period);
18     printf("GetWideBoxTempFanMonitorParam enable is %d   period is %d\n", enable, period);
19     for (int i = 0; i < 100; i++)
20     {
21         robot.GetRobotRealTimeState(&pkg);
22         printf("robot ctrl box temp is %f,  fan current is %d\n", pkg.wideVoltageCtrlBoxTemp, pkg.wideVoltageCtrlBoxFanCurrent);
23         robot.Sleep(100);
24     }
25     rtn = robot.SetWideBoxTempFanMonitorParam(0, 2);
26     printf("SetWideBoxTempFanMonitorParam rtn is %d\n", rtn);
27     enable = 0;
28     period = 0;
29     robot.GetWideBoxTempFanMonitorParam(enable, period);
30     printf("GetWideBoxTempFanMonitorParam enable is %d   period is %d\n", enable, period);
31     for (int i = 0; i < 100; i++)
32     {
33         robot.GetRobotRealTimeState(&pkg);
34         printf("robot ctrl box temp is %f,  fan current is %d\n", pkg.wideVoltageCtrlBoxTemp, pkg.wideVoltageCtrlBoxFanCurrent);
35         robot.Sleep(100);
36     }
37     robot.CloseRPC();
38     robot.Sleep(2000);
39     return 0;
40 }

6.47. Obliczenie wyniku kalibracji ogniska

1/**
2* @brief Oblicza wynik kalibracji ogniska
3* @param [in] pointNum Liczba punktów kalibracji
4* @param [out] resultPos Wynik kalibracji XYZ
5* @param [out] accuracy Błąd dokładności kalibracji
6* @return Kod błędu
7*/
8errno_t ComputeFocusCalib(int pointNum, DescTran& resultPos, float& accuracy);

6.48. Ustawienie współrzędnych ogniska

1/**
2* @brief Ustawia współrzędne ogniska
3* @param [in] pos Współrzędne ogniska XYZ
4* @return Kod błędu
5*/
6errno_t SetFocusPosition(DescTran pos);

6.49. Rozpoczęcie śledzenia ogniska

 1/**
 2* @brief Rozpoczyna śledzenie ogniska
 3* @param [in] kp Parametr proporcjonalny, domyślnie 50.0
 4* @param [in] kpredict Parametr sprzężenia przedniego, domyślnie 19.0
 5* @param [in] aMax Maksymalne ograniczenie przyspieszenia kątowego, domyślnie 1440°/s²
 6* @param [in] vMax Maksymalne ograniczenie prędkości kątowej, domyślnie 180°/s
 7* @param [in] type Zablokowanie kierunku osi X (0-odniesienie do wektora wejściowego; 1-poziomo; 2-pionowo)
 8* @return Kod błędu
 9*/
10errno_t FocusStart(double kp, double kpredict, double aMax, double vMax, int type);

6.50. Zatrzymanie śledzenia ogniska

1/**
2* @brief Zatrzymuje śledzenie ogniska
3* @return Kod błędu
4*/
5errno_t FocusEnd();

6.51. Przykład kodu śledzenia ogniska robota

 1int TestFocus()
 2{
 3  ROBOT_STATE_PKG pkg = {};
 4  FRRobot robot;
 5  robot.LoggerInit();
 6  robot.SetLoggerLevel(1);
 7  int rtn = robot.RPC("192.168.58.2");
 8  if (rtn != 0)
 9  {
10    return -1;
11  }
12  robot.SetReConnectParam(true, 30000, 500);
13  DescPose p1Desc(186.331, 487.913, 209.850, 149.030, 0.688, -114.347);
14  JointPos p1Joint(-127.876, -75.341, 115.417, -122.741, -59.820, 74.300);
15  DescPose p2Desc(69.721, 535.073, 202.882, -144.406, -14.775, -89.012);
16  JointPos p2Joint(-101.780, -69.828, 110.917, -125.740, -127.841, 74.300);
17  DescPose p3Desc(146.861, 578.426, 205.598, 175.997, -36.178, -93.437);
18  JointPos p3Joint(-112.851, -60.191, 86.566, -80.676, -97.463, 74.300);
19  DescPose p4Desc(136.284, 509.876, 225.613, 178.987, 1.372, -100.696);
20  JointPos p4Joint(-116.397, -76.281, 113.845, -128.611, -88.654, 74.299);
21  DescPose p5Desc(138.395, 505.972, 298.016, 179.134, 2.147, -101.110);
22  JointPos p5Joint(-116.814, -82.333, 109.162, -118.662, -88.585, 74.302);
23  DescPose p6Desc(105.553, 454.325, 232.017, -179.426, 0.444, -99.952);
24  JointPos p6Joint(-115.649, -84.367, 122.447, -128.663, -90.432, 74.303);
25  ExaxisPos exaxisPos(0, 0, 0, 0);
26  DescPose offdese(0, 0, 100, 0, 0, 0);
27  robot.MoveJ(&p1Joint, &p1Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
28  robot.SetTcp4RefPoint(1);
29  robot.MoveJ(&p2Joint, &p2Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
30  robot.SetTcp4RefPoint(2);
31  robot.MoveJ(&p3Joint, &p3Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
32  robot.SetTcp4RefPoint(3);
33  robot.MoveJ(&p4Joint, &p4Desc, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
34  robot.SetTcp4RefPoint(4);
35  DescPose coordRtn = {};
36  rtn = robot.ComputeTcp4(&coordRtn);
37  printf("4 Point ComputeTool    %d coord is %f %f %f %f %f %f \n", rtn, coordRtn.tran.x, coordRtn.tran.y, coordRtn.tran.z, coordRtn.rpy.rx, coordRtn.rpy.ry, coordRtn.rpy.rz);
38  robot.SetToolCoord(1, &coordRtn, 0, 0, 1, 0);
39  robot.GetForwardKin(&p1Joint, &p1Desc);
40  robot.GetForwardKin(&p2Joint, &p2Desc);
41  robot.GetForwardKin(&p3Joint, &p3Desc);
42  robot.SetFocusCalibPoint(1, p1Desc);
43  robot.SetFocusCalibPoint(2, p2Desc);
44  robot.SetFocusCalibPoint(3, p3Desc);
45  DescTran resultPos = {};
46  float accuracy = 0.0;
47  rtn = robot.ComputeFocusCalib(3, resultPos, accuracy);
48  printf("ComputeFocusCalib coord is %d %f %f %f accuracy is %f\n", rtn, resultPos.x, resultPos.y, resultPos.z, accuracy);
49  rtn = robot.SetFocusPosition(resultPos);
50  robot.GetForwardKin(&p5Joint, &p5Desc);
51  robot.GetForwardKin(&p6Joint, &p6Desc);
52  robot.MoveL(&p5Joint, &p5Desc, 1, 0, 10, 100, 100, -1, 0, &exaxisPos, 0, 1, &offdese);
53  robot.MoveL(&p6Joint, &p6Desc, 1, 0, 10, 100, 100, -1, 0, &exaxisPos, 0, 1, &offdese);
54  robot.FocusStart(50, 19, 710, 90, 0);
55  robot.MoveL(&p5Joint, &p5Desc, 1, 0, 10, 100, 100, -1, 0, &exaxisPos, 0, 1, &offdese);
56  robot.MoveL(&p6Joint, &p6Desc, 1, 0, 10, 100, 100, -1, 0, &exaxisPos, 0, 1, &offdese);
57  robot.FocusEnd();
58  robot.CloseRPC();
59  return 0;
60}

6.52. Włączenie funkcji kalibracji czułości czujnika momentu obrotowego stawu

1/**
2* @brief Włączenie funkcji kalibracji czułości czujnika momentu obrotowego stawu
3* @param [in] status 0-wyłączone; 1-włączone
4* @return  Kod błędu
5*/
6errno_t JointSensitivityEnable(int status);

6.53. Zbieranie danych czułości czujnika momentu obrotowego stawu

1/**
2* @brief Zbieranie danych czułości czujnika momentu obrotowego stawu
3* @return Kod błędu
4*/
5errno_t JointSensitivityCollect();

6.54. Pobranie wyniku kalibracji czułości czujnika momentu obrotowego stawu

1/**
2* @brief Pobiera wynik kalibracji czułości czujnika momentu obrotowego stawu
3* @param [out] calibResult Czułość stawów j1~j6 [0-1]
4* @param [out] linearity Liniowość stawów j1~j6 [0-1]
5* @return Kod błędu
6*/
7errno_t JointSensitivityCalibration(double calibResult[6], double linearity[6]);

6.55. Pobranie błędu histerezy czujnika momentu obrotowego stawu

1/**
2* @brief Pobiera błąd histerezy czujnika momentu obrotowego stawu
3* @param [out] hysteresisError Błąd histerezy stawów j1~j6
4* @return Kod błędu
5*/
6errno_t JointHysteresisError(double hysteresisError[6]);

6.56. Pobranie powtarzalności czujnika momentu obrotowego stawu

1/**
2* @brief Pobiera powtarzalność czujnika momentu obrotowego stawu
3* @param [out] repeatability Powtarzalność czujnika momentu obrotowego stawów j1~j6
4* @return Kod błędu
5*/
6errno_t JointRepeatability(double repeatability[6]);

6.57. Ustawienie parametrów czujnika siły stawu

 1/**
 2* @brief Ustawia parametry czujnika siły stawu
 3* @param [in] M Współczynniki masy J1-J6 [0.001 ~ 10]
 4* @param [in] B Współczynniki tłumienia J1-J6 [0.001 ~ 10]
 5* @param [in] K Współczynniki sztywności J1-J6 [0.001 ~ 10]
 6* @param [in] threshold Próg sterowania siłą, Nm
 7* @param [in] sensitivity Czułość, Nm/V, [0 ~ 10]
 8* @param [in] setZeroFlag Bit flagi włączenia funkcji; 0-wyłączone; 1-włączone; 2-rejestracja zera w pozycji 1; 3-rejestracja zera w pozycji 2
 9* @return Kod błędu
10*/
11errno_t SetAdmittanceParams(double M[6], double B[6], double K[6], double threshold[6], double sensitivity[6], int setZeroFlag);

6.58. Przykład kodu automatycznej kalibracji czułości czujnika momentu obrotowego stawu

  1int TestSensitivityCalib()
  2{
  3    ROBOT_STATE_PKG pkg = {};
  4    FRRobot robot;
  5    robot.LoggerInit();
  6    robot.SetLoggerLevel(1);
  7    robot.SetReConnectParam(true, 30000, 500);
  8    int rtn = robot.RPC("192.168.58.2");
  9    if (rtn != 0)
 10    {
 11        return 0;
 12    }
 13    rtn = robot.JointSensitivityEnable(0);
 14    rtn = robot.JointSensitivityEnable(1);
 15    printf("JointSensitivityEnable rtn is %d\n", rtn);
 16    JointPos curJPos = {};
 17    robot.GetActualJointPosDegree(0, &curJPos);
 18    ExaxisPos epos = { 0,0,0,0 };
 19    DescPose offset_pos = { 0,0,0,0,0,0 };
 20    JointPos jointPos1 = { curJPos.jPos[0], 0, 0, -90, 0.02, curJPos.jPos[5] };
 21    DescPose descPos1 = {};
 22    robot.GetForwardKin(&jointPos1, &descPos1);
 23    robot.MoveJ(&jointPos1, &descPos1, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 24    robot.Sleep(200);
 25    rtn = robot.JointSensitivityCollect();
 26    printf("JointSensitivityCollect 1 rtn is %d\n", rtn);
 27    robot.Sleep(100);
 28    JointPos jointPos2 = { curJPos.jPos[0], -30, 0, -90, 0.02, curJPos.jPos[5] };
 29    DescPose descPos2 = {};
 30    robot.GetForwardKin(&jointPos2, &descPos2);
 31    robot.MoveJ(&jointPos2, &descPos2, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 32    robot.Sleep(100);
 33    rtn = robot.JointSensitivityCollect();
 34    printf("JointSensitivityCollect 2 rtn is %d\n", rtn);
 35    robot.Sleep(100);
 36    JointPos jointPos3 = { curJPos.jPos[0], -60, 0, -90, 0.02, curJPos.jPos[5] };
 37    DescPose descPos3 = {};
 38    robot.GetForwardKin(&jointPos3, &descPos3);
 39    robot.MoveJ(&jointPos3, &descPos3, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 40    robot.Sleep(100);
 41    rtn = robot.JointSensitivityCollect();
 42    printf("JointSensitivityCollect 3 rtn is %d\n", rtn);
 43    robot.Sleep(100);
 44    JointPos jointPos4 = { curJPos.jPos[0], -90, 0, -90, 0.02, curJPos.jPos[5] };
 45    DescPose descPos4 = {};
 46    robot.GetForwardKin(&jointPos4, &descPos4);
 47    robot.MoveJ(&jointPos4, &descPos4, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 48    robot.Sleep(100);
 49    rtn = robot.JointSensitivityCollect();
 50    printf("JointSensitivityCollect 4 rtn is %d\n", rtn);
 51    robot.Sleep(100);
 52    JointPos jointPos5 = { curJPos.jPos[0], -120, 0, -90, 0.02, curJPos.jPos[5] };
 53    DescPose descPos5 = {};
 54    robot.GetForwardKin(&jointPos5, &descPos5);
 55    robot.MoveJ(&jointPos5, &descPos5, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 56    robot.Sleep(100);
 57    rtn = robot.JointSensitivityCollect();
 58    printf("JointSensitivityCollect 5 rtn is %d\n", rtn);
 59    robot.Sleep(100);
 60    JointPos jointPos6 = { curJPos.jPos[0], -150, 0, -90, 0.02, curJPos.jPos[5] };
 61    DescPose descPos6 = {};
 62    robot.GetForwardKin(&jointPos6, &descPos6);
 63    robot.MoveJ(&jointPos6, &descPos6, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 64    robot.Sleep(100);
 65    rtn = robot.JointSensitivityCollect();
 66    printf("JointSensitivityCollect 6 rtn is %d\n", rtn);
 67    robot.Sleep(100);
 68    JointPos jointPos7 = { curJPos.jPos[0], -180, 0, -90, 0.02, curJPos.jPos[5] };
 69    DescPose descPos7 = {};
 70    robot.GetForwardKin(&jointPos7, &descPos7);
 71    robot.MoveJ(&jointPos7, &descPos7, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 72    robot.Sleep(100);
 73    rtn = robot.JointSensitivityCollect();
 74    printf("JointSensitivityCollect 7 rtn is %d\n", rtn);
 75    robot.Sleep(100);
 76    //-------------------
 77    robot.MoveJ(&jointPos6, &descPos6, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 78    robot.Sleep(100);
 79    rtn = robot.JointSensitivityCollect();
 80    printf("JointSensitivityCollect 8 rtn is %d\n", rtn);
 81    robot.Sleep(100);
 82    robot.MoveJ(&jointPos5, &descPos5, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 83    robot.Sleep(100);
 84    rtn = robot.JointSensitivityCollect();
 85    printf("JointSensitivityCollect 9 rtn is %d\n", rtn);
 86    robot.Sleep(100);
 87    robot.MoveJ(&jointPos4, &descPos4, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 88    robot.Sleep(100);
 89    rtn = robot.JointSensitivityCollect();
 90    printf("JointSensitivityCollect 10 rtn is %d\n", rtn);
 91    robot.Sleep(100);
 92    robot.MoveJ(&jointPos3, &descPos3, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 93    robot.Sleep(100);
 94    rtn = robot.JointSensitivityCollect();
 95    printf("JointSensitivityCollect 11 rtn is %d\n", rtn);
 96    robot.Sleep(100);
 97    robot.MoveJ(&jointPos2, &descPos2, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
 98    robot.Sleep(100);
 99    rtn = robot.JointSensitivityCollect();
100    printf("JointSensitivityCollect 12 rtn is %d\n", rtn);
101    robot.Sleep(100);
102    robot.MoveJ(&jointPos1, &descPos1, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
103    robot.Sleep(200);
104    rtn = robot.JointSensitivityCollect();
105    printf("JointSensitivityCollect 13 rtn is %d\n", rtn);
106    robot.Sleep(100);
107    double calibResult[6] = { 0.0 };
108    double linearity[6] = { 0.0 };
109    rtn = robot.JointSensitivityCalibration(calibResult, linearity);
110    printf("JointSensitivityCalibration rtn is %d\n", rtn);
111    rtn = robot.JointSensitivityEnable(0);
112    printf("JointSensitivityEnable rtn is %d\n", rtn);
113    printf("jointSensor Calib result is %f %f %f %f %f %f\njointSensor linearity is %f %f %f %f %f %f\n",
114        calibResult[0], calibResult[1], calibResult[2],
115        calibResult[3], calibResult[4], calibResult[5],
116        linearity[0], linearity[1], linearity[2],
117        linearity[3], linearity[4], linearity[5]);
118    double hysteresisError[6] = { 0.0 };
119    rtn = robot.JointHysteresisError(hysteresisError);
120    printf("JointHysteresisError result is %f %f %f %f %f %f\n",
121        hysteresisError[0], hysteresisError[1], hysteresisError[2],
122        hysteresisError[3], hysteresisError[4], hysteresisError[5]);
123    double repeatability[6] = { 0.0 };
124    rtn = robot.JointRepeatability(repeatability);
125    printf("JointRepeatability result is %f %f %f %f %f %f\n",
126        repeatability[0], repeatability[1], repeatability[2],
127        repeatability[3], repeatability[4], repeatability[5]);
128    double M[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
129    double B[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
130    double K[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
131    double threshold[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
132    int setZeroFlag = 1;
133    rtn = robot.SetAdmittanceParams(M, B, K, threshold, calibResult, setZeroFlag);
134    printf("SetAdmittanceParams rtn is %d\n", rtn);
135    robot.CloseRPC();
136}

6.59. Pobranie liczby błędnych ramek dla 8 portów stacji podrzędnych robota

 1/**
 2* @brief Pobiera liczbę błędnych ramek dla 8 portów stacji podrzędnych robota
 3* @param [out] inRecvErr Liczba błędnych ramek odebranych na wejściu
 4* @param [out] inCRCErr Liczba błędnych ramek CRC na wejściu
 5* @param [out] inTransmitErr Liczba błędnych ramek przesłanych na wejściu
 6* @param [out] inLinkErr Liczba błędnych ramek łącza na wejściu
 7* @param [out] outRecvErr Liczba błędnych ramek odebranych na wyjściu
 8* @param [out] outCRCErr Liczba błędnych ramek CRC na wyjściu
 9* @param [out] outTransmitErr Liczba błędnych ramek przesłanych na wyjściu
10* @param [out] outLinkErr Liczba błędnych ramek łącza na wyjściu
11* @return Kod błędu
12*/
13errno_t GetSlavePortErrCounter(int inRecvErr[8], int inCRCErr[8], int inTransmitErr[8], int inLinkErr[8], int outRecvErr[8], int outCRCErr[8], int outTransmitErr[8], int outLinkErr[8]);

6.60. Zerowanie licznika błędnych ramek portu stacji podrzędnej

1/**
2* @brief Zeruje licznik błędnych ramek portu stacji podrzędnej
3* @param [in] slaveID Numer stacji podrzędnej 0~7
4* @return Kod błędu
5*/
6errno_t SlavePortErrCounterClear(int slaveID);

6.61. Przykład kodu pobierania błędnych ramek portów stacji podrzędnych

 1int TestSlavePortErr()
 2{
 3    ROBOT_STATE_PKG pkg = {};
 4    FRRobot robot;
 5    robot.LoggerInit();
 6    robot.SetLoggerLevel(1);
 7    int rtn = robot.RPC("192.168.58.2");
 8    if (rtn != 0)
 9    {
10        return 0;
11    }
12    robot.SetReConnectParam(true, 30000, 500);
13    int inRecvErr[8] = {0.0};
14    int inCRCErr[8] = { 0.0 };
15    int inTransmitErr[8] = { 0.0 };
16    int inLinkErr[8] = { 0.0 };
17    int outRecvErr[8] = { 0.0 };
18    int outCRCErr[8] = { 0.0 };
19    int outTransmitErr[8] = { 0.0 };
20    int outLinkErr[8] = { 0.0 };
21    robot.GetSlavePortErrCounter(inRecvErr,  inCRCErr, inTransmitErr, inLinkErr,
22        outRecvErr, outCRCErr, outTransmitErr, outLinkErr);
23    for (int i = 0; i < 8; i++)
24    {
25        if (inRecvErr[i] != 0)
26        {
27            printf("inRecvErr %d is %d\n", i, inRecvErr[i]);
28        }
29        if (inCRCErr[i] != 0)
30        {
31            printf("inRecvErr %d is %d\n", i, inCRCErr[i]);
32        }
33        if (inTransmitErr[i] != 0)
34        {
35            printf("inRecvErr %d is %d\n", i, inTransmitErr[i]);
36        }
37        if (inLinkErr[i] != 0)
38        {
39            printf("inRecvErr %d is %d\n", i, inLinkErr[i]);
40        }
41        if (outRecvErr[i] != 0)
42        {
43            printf("outRecvErr %d is %d\n", i, outRecvErr[i]);
44        }
45        if (outCRCErr[i] != 0)
46        {
47            printf("outCRCErr %d is %d\n", i, outCRCErr[i]);
48        }
49        if (outTransmitErr[i] != 0)
50        {
51            printf("outTransmitErr %d is %d\n", i, outTransmitErr[i]);
52        }
53        if (outLinkErr[i] != 0)
54        {
55            printf("outLinkErr %d is %d\n", i, outLinkErr[i]);
56        }
57    }
58    printf("others has no err!\n");
59    for (int i = 0; i < 8; i++)
60    {
61        robot.SlavePortErrCounterClear(i);
62    }
63    robot.CloseRPC();
64    return 0;
65}

6.62. Ustawienie współczynnika sprzężenia przedniego prędkości dla każdej osi

1/**
2* @brief Ustawia współczynnik sprzężenia przedniego prędkości dla każdej osi
3* @param [in] radio Współczynnik sprzężenia przedniego prędkości dla każdej osi
4* @return Kod błędu
5*/
6errno_t SetVelFeedForwardRatio(double radio[6]);

6.63. Pobranie współczynnika sprzężenia przedniego prędkości dla każdej osi

1/**
2* @brief Pobiera współczynnik sprzężenia przedniego prędkości dla każdej osi
3* @param [out] radio Współczynnik sprzężenia przedniego prędkości dla każdej osi
4* @return Kod błędu
5*/
6errno_t GetVelFeedForwardRatio(double radio[6]);

6.64. Przykład kodu współczynnika sprzężenia przedniego prędkości robota

 1int TestVelFeedForwardRatio()
 2{
 3    ROBOT_STATE_PKG pkg = {};
 4    FRRobot robot;
 5    robot.LoggerInit();
 6    robot.SetLoggerLevel(1);
 7    int rtn = robot.RPC("192.168.58.2");
 8    if (rtn != 0)
 9    {
10        return 0;
11    }
12    robot.SetReConnectParam(true, 30000, 500);
13    double setRadio[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
14    robot.SetVelFeedForwardRatio(setRadio);
15    double getRadio[6] = { 0.0 };
16    robot.GetVelFeedForwardRatio(getRadio);
17    printf(" %f %f %f %f %f %f\n", getRadio[0], getRadio[1], getRadio[2], getRadio[3], getRadio[4], getRadio[5]);
18    robot.CloseRPC();
19    return 0;
20}

6.65. Kalibracja TCP czujnika fotoelektrycznego - obliczenie RPY narzędzia

 1/**
 2* @brief Kalibracja TCP czujnika fotoelektrycznego - obliczenie RPY narzędzia
 3* @param [in] Btool Pozycja kartezjańska robota
 4* @param [in] Etool Bieżące wartości układu współrzędnych narzędzia
 5* @param [in] senser Bieżące wartości układu współrzędnych czujnika (jeszcze nieudostępnione)
 6* @param [in] radius Promień ruchu po okręgu mm (jeszcze nieudostępnione)
 7* @param [in] dz Odległość ruchu wzdłuż ujemnego kierunku osi Z podstawowego układu współrzędnych; gdy dz = 10000, funkcja bezpośrednio zwraca RPY narzędzia
 8* @param [out] TCPRPY Wartości RPY narzędzia
 9* @return Kod błędu
10*/
11errno_t TCPComputeRPY(DescPose Btool, DescPose Etool, DescPose sensor, double radius, double dz, Rpy& TCPRPY);

6.66. Kalibracja TCP czujnika fotoelektrycznego - obliczenie XYZ narzędzia

 1/**
 2* @brief Kalibracja TCP czujnika fotoelektrycznego - obliczenie XYZ narzędzia
 3* @param [in] select 0-oblicz TCP narzędzia; 1-oblicz początek czujnika; 2-oblicz orientację czujnika; 3-bezpośrednio zwróć TCP narzędzia; 4-zapisz bieżący układ współrzędnych przedmiotu i narzędzia
 4* @param [in] originDirection 0-kierunek X; 1-kierunek Y; 2-kierunek Z
 5* @param [in] pos1 Pozycja kartezjańska robota 1
 6* @param [in] pos2 Pozycja kartezjańska robota 2
 7* @param [in] pos3 Pozycja kartezjańska robota 3
 8* @param [in] pos4 Pozycja kartezjańska robota 4
 9* @param [out] TCP Wartości XYZ narzędzia
10* @return Kod błędu
11*/
12errno_t TCPComputeXYZ(int select, double originDirection, DescTran pos1, DescTran pos2, DescTran pos3, DescTran pos4, DescTran& TCP);

6.67. Kalibracja TCP czujnika fotoelektrycznego - rozpoczęcie rejestracji pozycji środka kołnierza końcowego

1/**
2* @brief Kalibracja TCP czujnika fotoelektrycznego - rozpoczęcie rejestracji pozycji środka kołnierza końcowego
3* @return Kod błędu
4*/
5errno_t TCPRecordFlangePosStart();

6.68. Kalibracja TCP czujnika fotoelektrycznego - zatrzymanie rejestracji pozycji środka kołnierza końcowego

1/**
2* @brief Kalibracja TCP czujnika fotoelektrycznego - zatrzymanie rejestracji pozycji środka kołnierza końcowego
3* @return Kod błędu
4*/
5errno_t TCPRecordFlangePosEnd();

6.69. Kalibracja TCP czujnika fotoelektrycznego - pobranie pozycji środka narzędzia końcowego

1/**
2* @brief Kalibracja TCP czujnika fotoelektrycznego - pobranie pozycji środka narzędzia końcowego
3* @param [out] TCP Pozycja środka narzędzia (x,y,z)
4* @return Kod błędu
5*/
6errno_t TCPGetRecordFlangePos(DescTran& TCP);

6.70. Kalibracja TCP czujnika fotoelektrycznego

1/**
2* @brief Kalibracja TCP czujnika fotoelektrycznego
3* @param [in] luaPath Ścieżka programu lua automatycznej kalibracji: "FR_CalibrateTheToolTcp.lua"
4* @param [in] offset Przesunięcie punktu nauczania (x,y,z) mm
5* @param [out] TCP Układ współrzędnych narzędzia po kalibracji (x,y,z,rx,ry,rz)
6* @return Kod błędu
7*/
8errno_t PhotoelectricSensorTCPCalibration(std::string luaPath, DescTran offset, DescPose& TCP);

6.71. Przykład kodu kalibracji TCP czujnika fotoelektrycznego

 1int TestPhotoelectricSensorTCPCalib(void)
 2{
 3    ROBOT_STATE_PKG pkg = {};
 4    FRRobot robot;
 5    robot.LoggerInit();
 6    robot.SetLoggerLevel(1);
 7    int rtn = robot.RPC("192.168.58.2");
 8    if (rtn != 0)
 9    {
10        return 0;
11    }
12    robot.SetReConnectParam(true, 30000, 500);
13    DescTran offset = { 10.0, 10.0, 3.0 };
14    DescPose TCP = {};
15    rtn = robot.PhotoelectricSensorTCPCalibration("FR_CalibrateTheToolTcp.lua", offset, TCP);
16    printf("PhotoelectricSensorTCPCalibration rtn is  %d %f %f %f %f %f %f \n", rtn, TCP.tran.x, TCP.tran.y, TCP.tran.z, TCP.rpy.rx, TCP.rpy.ry, TCP.rpy.rz);
17    robot.CloseRPC();
18    robot.Sleep(9999999);
19    return 0;
20}

6.72. Ustawianie Prędkości Globalnej w Czasie Rzeczywistym

1/**
2* @brief Ustawia prędkość globalną w czasie rzeczywistym
3* @param [in] vel Procent prędkości, zakres [0~100]
4* @return Kod błędu
5*/
6errno_t SetSpeedInstant(int vel);