4. Ruch robota
4.1. Ruch jałowy (JOG)
1/**
2* @brief Ruch jałowy (JOG)
3* @param [in] ref 0-ruch jałowy stawów, 2-ruch jałowy w układzie bazowym, 4-ruch jałowy w układzie narzędzia, 8-ruch jałowy w układzie obiektu
4* @param [in] nb 1-staw 1 (lub oś x), 2-staw 2 (lub oś y), 3-staw 3 (lub oś z), 4-staw 4 (lub obrót wokół osi x), 5-staw 5 (lub obrót wokół osi y), 6-staw 6 (lub obrót wokół osi z)
5* @param [in] dir 0-kierunek ujemny, 1-kierunek dodatni
6* @param [in] vel Procent prędkości, [0~100]
7* @param [in] acc Procent przyspieszenia, [0~100]
8* @param [in] max_dis Maksymalny kąt pojedynczego ruchu jałowego, jednostka [°] lub odległość, jednostka [mm]
9* @return Kod błędu
10*/
11errno_t StartJOG(uint8_t ref, uint8_t nb, uint8_t dir, float vel, float acc, float max_dis);
4.2. Zatrzymanie ruchu jałowego z opóźnieniem
1/**
2* @brief Zatrzymanie ruchu jałowego z opóźnieniem
3* @param [in] ref 1-zatrzymanie ruchu jałowego stawów, 3-zatrzymanie ruchu jałowego w układzie bazowym, 5-zatrzymanie ruchu jałowego w układzie narzędzia, 9-zatrzymanie ruchu jałowego w układzie obiektu
4* @return Kod błędu
5*/
6errno_t StopJOG(uint8_t ref);
4.3. Natychmiastowe zatrzymanie ruchu jałowego
1/**
2* @brief Natychmiastowe zatrzymanie ruchu jałowego
3* @return Kod błędu
4*/
5errno_t ImmStopJOG();
4.4. Przykład kodu sterowania ruchem jałowym robota
1 int TestJOG(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 = 0; i < 6; i++)
14 {
15 robot.StartJOG(0, i + 1, 0, 20.0, 20.0, 30.0);
16 robot.Sleep(1000);
17 robot.ImmStopJOG();
18 robot.Sleep(1000);
19 }
20 for (int i = 0; i < 6; i++)
21 {
22 robot.StartJOG(2, i + 1, 0, 20.0, 20.0, 30.0);
23 robot.Sleep(1000);
24 robot.ImmStopJOG();
25 robot.Sleep(1000);
26 }
27 for (int i = 0; i < 6; i++)
28 {
29 robot.StartJOG(4, i + 1, 0, 20.0, 20.0, 30.0);
30 robot.Sleep(1000);
31 robot.StopJOG(5);
32 robot.Sleep(1000);
33 }
34 for (int i = 0; i < 6; i++)
35 {
36 robot.StartJOG(8, i + 1, 0, 20.0, 20.0, 30.0);
37 robot.Sleep(1000);
38 robot.StopJOG(9);
39 robot.Sleep(1000);
40 }
41 robot.CloseRPC();
42 return 0;
43 }
4.5. Ruch w przestrzeni stawów
1/**
2* @brief Ruch w przestrzeni stawów
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
5* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] vel Procent prędkości, zakres [0~100]
8* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
10* @param [in] epos Pozycja osi rozszerzonej, jednostka mm
11* @param [in] blendT [-1.0]-ruch do pozycji (blokujący), [0~500.0]-czas wygładzania (nieblokujący), jednostka ms
12* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
13* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
14* @return Kod błędu
15*/
16errno_t MoveJ(JointPos *joint_pos, DescPose *desc_pos, int tool, int user, float vel, float acc, float ovl, ExaxisPos *epos, float blendT, uint8_t offset_flag, DescPose *offset_pos);
4.6. Ruch w przestrzeni stawów (automatyczne obliczanie prostej kinematyki)
1/**
2* @brief Ruch w przestrzeni stawów (automatyczne obliczanie prostej kinematyki)
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] vel Procent prędkości, zakres [0~100]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
9* @param [in] epos Pozycja osi rozszerzonej, jednostka mm
10* @param [in] blendT [-1.0]-ruch do pozycji (blokujący), [0~500.0]-czas wygładzania (nieblokujący), jednostka ms
11* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
12* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
13* @return Kod błędu
14*/
15errno_t MoveJ(JointPos* joint_pos, int tool, int user, float vel, float acc, float ovl, ExaxisPos* epos, float blendT, uint8_t offset_flag, DescPose* offset_pos);
4.7. Ruch liniowy w przestrzeni kartezjańskiej
1/**
2* @brief Ruch liniowy w przestrzeni kartezjańskiej
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
5* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] vel Procent prędkości, zakres [0~100]
8* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] ovl Współczynnik skalowania prędkości [0~100] / prędkość fizyczna (mm/s)
10* @param [in] blendR [-1.0]-ruch do pozycji (blokujący), [0~1000.0]-promień wygładzania (nieblokujący), jednostka mm
11* @param [in] blendMode Sposób przejścia; 0-przejście styczne wewnętrznie; 1-przejście narożne
12* @param [in] epos Pozycja osi rozszerzonej, jednostka mm
13* @param [in] search 0-brak lokalizacji drutu spawalniczego, 1-lokalizacja drutu spawalniczego
14* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
15* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
16* @param [in] oacc Współczynnik skalowania przyspieszenia [0-100] / przyspieszenie fizyczne (mm/s2)
17* @param [in] velAccParamMode Tryb parametrów prędkości i przyspieszenia; 0-procent; 1-prędkość fizyczna (mm/s) przyspieszenie fizyczne (mm/s2)
18* @param [in] overSpeedStrategy Strategia obsługi przekroczenia prędkości, 1-standardowa; 2-zgłoś błąd i zatrzymaj przy przekroczeniu prędkości; 3-automatyczne zmniejszenie prędkości, domyślnie 0
19* @param [in] speedPercent Procent progu dozwolonego zmniejszenia prędkości [0-100], domyślnie 10%
20* @return Kod błędu
21*/
22errno_t MoveL(JointPos *joint_pos, DescPose *desc_pos, int tool, int user, float vel, float acc, float ovl, float blendR, int blendMode, ExaxisPos *epos, uint8_t search, uint8_t offset_flag, DescPose *offset_pos, float oacc = 100.0, int velAccParamMode = 0, int overSpeedStrategy = 0, int speedPercent = 10);
4.8. Ruch liniowy w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
1/**
2* @brief Ruch liniowy w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
3* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
4* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] vel Procent prędkości, zakres [0~100]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
9* @param [in] blendR [-1.0]-ruch do pozycji (blokujący), [0~1000.0]-promień wygładzania (nieblokujący), jednostka mm
10* @param [in] blendMode Sposób przejścia; 0-przejście styczne wewnętrznie; 1-przejście narożne
11* @param [in] epos Pozycja osi rozszerzonej, jednostka mm
12* @param [in] search 0-brak lokalizacji drutu spawalniczego, 1-lokalizacja drutu spawalniczego
13* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
14* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
15* @param [in] config Konfiguracja przestrzeni stawów dla odwrotnej kinematyki, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia zgodnie z określoną konfiguracją przestrzeni stawów
16* @param [in] overSpeedStrategy Strategia obsługi przekroczenia prędkości, 1-standardowa; 2-zgłoś błąd i zatrzymaj przy przekroczeniu prędkości; 3-automatyczne zmniejszenie prędkości, domyślnie 0
17* @param [in] speedPercent Procent progu dozwolonego zmniejszenia prędkości [0-100], domyślnie 10%
18* @return Kod błędu
19*/
20errno_t MoveL(DescPose* desc_pos, int tool, int user, float vel, float acc, float ovl, float blendR, int blendMode, ExaxisPos* epos, uint8_t search, uint8_t offset_flag, DescPose* offset_pos, int config = -1, int overSpeedStrategy = 0, int speedPercent = 10);
4.9. Ruch łukowy w przestrzeni kartezjańskiej
1/**
2* @brief Ruch łukowy w przestrzeni kartezjańskiej
3* @param [in] joint_pos_p Pozycja stawów punktu pośredniego, jednostka deg
4* @param [in] desc_pos_p Poza i orientacja punktu pośredniego w kartezjańskim układzie współrzędnych
5* @param [in] ptool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] puser Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] pvel Procent prędkości, zakres [0~100]
8* @param [in] pacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] epos_p Pozycja osi rozszerzonej, jednostka mm
10* @param [in] poffset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
11* @param [in] offset_pos_p Wartość przesunięcia pozy i orientacji
12* @param [in] joint_pos_t Pozycja stawów punktu docelowego, jednostka deg
13* @param [in] desc_pos_t Poza i orientacja punktu docelowego w kartezjańskim układzie współrzędnych
14* @param [in] ttool Numer układu współrzędnych narzędzia, zakres [0~14]
15* @param [in] tuser Numer układu współrzędnych obiektu, zakres [0~14]
16* @param [in] tvel Procent prędkości, zakres [0~100]
17* @param [in] tacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
18* @param [in] epos_t Pozycja osi rozszerzonej, jednostka mm
19* @param [in] toffset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
20* @param [in] offset_pos_t Wartość przesunięcia pozy i orientacji
21* @param [in] ovl Współczynnik skalowania prędkości [0~100] / prędkość fizyczna (mm/s)
22* @param [in] blendR [-1.0]-ruch do pozycji (blokujący), [0~1000.0]-promień wygładzania (nieblokujący), jednostka mm
23* @param [in] oacc Współczynnik skalowania przyspieszenia [0-100] / przyspieszenie fizyczne (mm/s2)
24* @param [in] velAccParamMode Tryb parametrów prędkości i przyspieszenia; 0-procent; 1-prędkość fizyczna (mm/s) przyspieszenie fizyczne (mm/s2)
25* @return Kod błędu
26*/
27errno_t MoveC(JointPos *joint_pos_p, DescPose *desc_pos_p, int ptool, int puser, float pvel, float pacc, ExaxisPos *epos_p, uint8_t poffset_flag, DescPose *offset_pos_p, JointPos *joint_pos_t, DescPose *desc_pos_t, int ttool, int tuser, float tvel, float tacc, ExaxisPos *epos_t, uint8_t toffset_flag, DescPose *offset_pos_t, float ovl, float blendR, float oacc = 100.0, int velAccParamMode = 0);
4.10. Ruch łukowy w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
1/**
2* @brief Ruch łukowy w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
3* @param [in] desc_pos_p Poza i orientacja punktu pośredniego w kartezjańskim układzie współrzędnych
4* @param [in] ptool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] puser Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] pvel Procent prędkości, zakres [0~100]
7* @param [in] pacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] epos_p Pozycja osi rozszerzonej, jednostka mm
9* @param [in] poffset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
10* @param [in] offset_pos_p Wartość przesunięcia pozy i orientacji
11* @param [in] desc_pos_t Poza i orientacja punktu docelowego w kartezjańskim układzie współrzędnych
12* @param [in] ttool Numer układu współrzędnych narzędzia, zakres [0~14]
13* @param [in] tuser Numer układu współrzędnych obiektu, zakres [0~14]
14* @param [in] tvel Procent prędkości, zakres [0~100]
15* @param [in] tacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
16* @param [in] epos_t Pozycja osi rozszerzonej, jednostka mm
17* @param [in] toffset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
18* @param [in] offset_pos_t Wartość przesunięcia pozy i orientacji
19* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
20* @param [in] blendR [-1.0]-ruch do pozycji (blokujący), [0~1000.0]-promień wygładzania (nieblokujący), jednostka mm
21* @param [in] config Konfiguracja przestrzeni stawów dla odwrotnej kinematyki, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia zgodnie z określoną konfiguracją przestrzeni stawów
22* @return Kod błędu
23*/
24errno_t MoveC(DescPose* desc_pos_p, int ptool, int puser, float pvel, float pacc, ExaxisPos* epos_p, uint8_t poffset_flag, DescPose* offset_pos_p, DescPose* desc_pos_t, int ttool, int tuser, float tvel, float tacc, ExaxisPos* epos_t, uint8_t toffset_flag, DescPose* offset_pos_t, float ovl, float blendR, int config = -1);
4.11. Ruch pełnego okręgu w przestrzeni kartezjańskiej
1/**
2* @brief Ruch pełnego okręgu w przestrzeni kartezjańskiej
3* @param [in] joint_pos_p Pozycja stawów punktu pośredniego 1, jednostka deg
4* @param [in] desc_pos_p Poza i orientacja punktu pośredniego 1 w kartezjańskim układzie współrzędnych
5* @param [in] ptool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] puser Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] pvel Procent prędkości, zakres [0~100]
8* @param [in] pacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] epos_p Pozycja osi rozszerzonej, jednostka mm
10* @param [in] joint_pos_t Pozycja stawów punktu pośredniego 2, jednostka deg
11* @param [in] desc_pos_t Poza i orientacja punktu pośredniego 2 w kartezjańskim układzie współrzędnych
12* @param [in] ttool Numer układu współrzędnych narzędzia, zakres [0~14]
13* @param [in] tuser Numer układu współrzędnych obiektu, zakres [0~14]
14* @param [in] tvel Procent prędkości, zakres [0~100]
15* @param [in] tacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
16* @param [in] epos_t Pozycja osi rozszerzonej, jednostka mm
17* @param [in] ovl Współczynnik skalowania prędkości [0~100] / prędkość fizyczna (mm/s)
18* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
19* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
20* @param [in] oacc Współczynnik skalowania przyspieszenia [0-100] / przyspieszenie fizyczne (mm/s2)
21* @param [in] blendR -1: blokujący; 0~1000: promień wygładzania
22* @param [in] velAccParamMode Tryb parametrów prędkości i przyspieszenia; 0-procent; 1-prędkość fizyczna (mm/s) przyspieszenie fizyczne (mm/s2)
23* @return Kod błędu
24*/
25errno_t Circle(JointPos* joint_pos_p, DescPose* desc_pos_p, int ptool, int puser, float pvel, float pacc, ExaxisPos* epos_p, JointPos* joint_pos_t, DescPose* desc_pos_t, int ttool, int tuser, float tvel, float tacc, ExaxisPos* epos_t, float ovl, uint8_t offset_flag, DescPose* offset_pos, double oacc = 100.0, double blendR = -1, int velAccParamMode = 0);
4.12. Ruch pełnego okręgu w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
1/**
2* @brief Ruch pełnego okręgu w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
3* @param [in] desc_pos_p Poza i orientacja punktu pośredniego 1 w kartezjańskim układzie współrzędnych
4* @param [in] ptool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] puser Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] pvel Procent prędkości, zakres [0~100]
7* @param [in] pacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] epos_p Pozycja osi rozszerzonej, jednostka mm
9* @param [in] desc_pos_t Poza i orientacja punktu pośredniego 2 w kartezjańskim układzie współrzędnych
10* @param [in] ttool Numer układu współrzędnych narzędzia, zakres [0~14]
11* @param [in] tuser Numer układu współrzędnych obiektu, zakres [0~14]
12* @param [in] tvel Procent prędkości, zakres [0~100]
13* @param [in] tacc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
14* @param [in] epos_t Pozycja osi rozszerzonej, jednostka mm
15* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
16* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
17* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
18* @param [in] oacc Procent przyspieszenia
19* @param [in] blendR -1: blokujący; 0~1000: promień wygładzania
20* @param [in] config Konfiguracja przestrzeni stawów dla odwrotnej kinematyki, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia zgodnie z określoną konfiguracją przestrzeni stawów
21* @return Kod błędu
22*/
23errno_t Circle(DescPose* desc_pos_p, int ptool, int puser, float pvel, float pacc, ExaxisPos* epos_p, DescPose* desc_pos_t, int ttool, int tuser, float tvel, float tacc, ExaxisPos* epos_t, float ovl, uint8_t offset_flag, DescPose* offset_pos, double oacc = 100.0, double blendR = -1, int config = -1);
4.13. Ruch punkt-punkt w przestrzeni kartezjańskiej
1/**
2* @brief Ruch punkt-punkt w przestrzeni kartezjańskiej
3* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych lub przyrost pozy i orientacji
4* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] vel Procent prędkości, zakres [0~100]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
9* @param [in] blendT [-1.0]-ruch do pozycji (blokujący), [0~500.0]-czas wygładzania (nieblokujący), jednostka ms
10* @param [in] config Konfiguracja przestrzeni stawów, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia w oparciu o określoną konfigurację przestrzeni stawów, domyślnie -1
11* @return Kod błędu
12*/
13errno_t MoveCart(DescPose *desc_pos, int tool, int user, float vel, float acc, float ovl, float blendT, int config);
4.14. Przykład kodu podstawowych instrukcji ruchu robota
1int TestMove(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
15 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
16 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
17 JointPos j3(-29.777, -84.536, 109.275, -114.075, -86.655, 74.257);
18 JointPos j4(-31.154, -95.317, 94.276, -88.079, -89.740, 74.256);
19 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
20 DescPose desc_pos2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
21 DescPose desc_pos3(-487.434, 154.362, 308.576, 176.600, 0.268, -14.061);
22 DescPose desc_pos4(-443.165, 147.881, 480.951, 179.511, -0.775, -15.409);
23 DescPose offset_pos(0, 0, 0, 0, 0, 0);
24 ExaxisPos epos(0, 0, 0, 0);
25 int tool = 0;
26 int user = 0;
27 float vel = 100.0;
28 float acc = 100.0;
29 float ovl = 100.0;
30 float oacc = 100.0;
31 float blendT = 0.0;
32 float blendR = 0.0;
33 uint8_t flag = 0;
34 uint8_t search = 0;
35 int blendMode = 0;
36 int velAccMode = 0;
37 robot.SetSpeed(20);
38 rtn = robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
39 printf("movej errcode:%d\n", rtn);
40 rtn = robot.MoveL(&j2, &desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, &epos, search, flag, &offset_pos, oacc, velAccMode);
41 printf("movel errcode:%d\n", rtn);
42 rtn = robot.MoveC(&j3, &desc_pos3, tool, user, vel, acc, &epos, flag, &offset_pos, &j4, &desc_pos4, tool, user, vel, acc, &epos, flag, &offset_pos, ovl, blendR, oacc, velAccMode);
43 printf("movec errcode:%d\n", rtn);
44 rtn = robot.MoveJ(&j2, &desc_pos2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
45 printf("movej errcode:%d\n", rtn);
46 rtn = robot.Circle(&j3, &desc_pos3, tool, user, vel, acc, &epos, &j1, &desc_pos1, tool, user, vel, acc, &epos, ovl, flag, &offset_pos, oacc, -1, velAccMode);
47 printf("circle errcode:%d\n", rtn);
48 rtn = robot.MoveCart(&desc_pos4, tool, user, vel, acc, ovl, blendT, -1);
49 printf("MoveCart errcode:%d\n", rtn);
50 rtn = robot.MoveJ(&j1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
51 printf("movej errcode:%d\n", rtn);
52 rtn = robot.MoveL(&desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, &epos, search, flag, &offset_pos, -1, velAccMode);
53 printf("movel errcode:%d\n", rtn);
54 rtn = robot.MoveC(&desc_pos3, tool, user, vel, acc, &epos, flag, &offset_pos, &desc_pos4, tool, user, vel, acc, &epos, flag, &offset_pos, ovl, blendR, -1, velAccMode);
55 printf("movec errcode:%d\n", rtn);
56 rtn = robot.MoveJ(&j2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
57 printf("movej errcode:%d\n", rtn);
58 rtn = robot.Circle(&desc_pos3, tool, user, vel, acc, &epos, &desc_pos1, tool, user, vel, acc, &epos, ovl, flag, &offset_pos, oacc, blendR, -1, velAccMode);
59 printf("circle errcode:%d\n", rtn);
60 robot.CloseRPC();
61 return 0;
62}
4.15. Ruch spiralny w przestrzeni kartezjańskiej
1/**
2* @brief Ruch spiralny w przestrzeni kartezjańskiej
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
5* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] vel Procent prędkości, zakres [0~100]
8* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] epos Pozycja osi rozszerzonej, jednostka mm
10* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
11* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
12* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
13* @param [in] spiral_param Parametry spirali
14* @return Kod błędu
15*/
16errno_t NewSpiral(JointPos *joint_pos, DescPose *desc_pos, int tool, int user, float vel, float acc, ExaxisPos *epos, float ovl, uint8_t offset_flag, DescPose *offset_pos, SpiralParam spiral_param);
4.16. Ruch spiralny w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
1/**
2* @brief Ruch spiralny w przestrzeni kartezjańskiej (automatyczne obliczanie odwrotnej kinematyki)
3* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
4* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] vel Procent prędkości, zakres [0~100]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] epos Pozycja osi rozszerzonej, jednostka mm
9* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
10* @param [in] offset_flag 0-brak przesunięcia, 1-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
11* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
12* @param [in] spiral_param Parametry spirali
13* @param [in] config Konfiguracja przestrzeni stawów dla odwrotnej kinematyki, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia zgodnie z określoną konfiguracją przestrzeni stawów
14* @return Kod błędu
15*/
16errno_t NewSpiral(DescPose* desc_pos, int tool, int user, float vel, float acc, ExaxisPos* epos, float ovl, uint8_t offset_flag, DescPose* offset_pos, SpiralParam spiral_param, int config = -1);
4.17. Przykład kodu ruchu spiralnego
1int TestSpiral(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 JointPos j(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 DescPose desc_pos(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
15 DescPose offset_pos1(50, 0, 0, -30, 0, 0);
16 DescPose offset_pos2(50, 0, 0, -5, 0, 0);
17 ExaxisPos epos(0, 0, 0, 0);
18 SpiralParam sp;
19 sp.circle_num = 5;
20 sp.circle_angle = 5.0;
21 sp.rad_init = 50.0;
22 sp.rad_add = 10.0;
23 sp.rotaxis_add = 10.0;
24 sp.rot_direction = 0;
25 int tool = 0;
26 int user = 0;
27 float vel = 100.0;
28 float acc = 100.0;
29 float ovl = 100.0;
30 float blendT = 0.0;
31 uint8_t flag = 2;
32 robot.SetSpeed(20);
33 rtn = robot.MoveJ(&j, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos1);
34 printf("movej errcode:%d\n", rtn);
35 rtn = robot.NewSpiral(&desc_pos, tool, user, vel, acc, &epos, ovl, flag, &offset_pos2, sp);
36 printf("newspiral errcode:%d\n", rtn);
37 robot.CloseRPC();
38 return 0;
39}
4.18. Rozpoczęcie ruchu serwo
1/**
2* @brief Rozpoczęcie ruchu serwo, używane razem z instrukcjami ServoJ, ServoCart
3* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
4* @return Kod błędu
5*/
6errno_t ServoMoveStart(int comType = 0);
4.19. Zakończenie ruchu serwo
1/**
2* @brief Zakończenie ruchu serwo, używane razem z instrukcjami ServoJ, ServoCart
3* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
4* @return Kod błędu
5*/
6errno_t ServoMoveEnd(int comType = 0);
4.20. Ruch w trybie serwo w przestrzeni stawów
1/**
2* @brief Ruch w trybie serwo w przestrzeni stawów
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] axisPos Pozycja osi zewnętrznej, jednostka mm
5* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne, domyślnie 0
6* @param [in] vel Procent prędkości, zakres [0~100], tymczasowo niedostępne, domyślnie 0
7* @param [in] cmdT Okres wysyłania instrukcji, jednostka s, zalecany zakres [0.001~0.0016]
8* @param [in] filterT Czas filtracji, jednostka s, tymczasowo niedostępny, domyślnie 0
9* @param [in] gain Wzmacniacz proporcjonalności pozycji docelowej, tymczasowo niedostępny, domyślnie 0
10* @param [in] id ID instrukcji servoJ, domyślnie 0
11* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
12* @return Kod błędu
13*/
14errno_t ServoJ(JointPos *joint_pos, ExaxisPos* axisPos, float acc, float vel, float cmdT, float filterT, float gain, int id = 0, int comType = 0);
4.21. Przykład programu ruchu w trybie serwo w przestrzeni stawów
1int TestServoJ(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 JointPos j(0, 0, 0, 0, 0, 0);
14 ExaxisPos epos(0, 0, 0, 0);
15 float vel = 0.0;
16 float acc = 0.0;
17 float cmdT = 0.008;
18 float filterT = 0.0;
19 float gain = 0.0;
20 uint8_t flag = 0;
21 int count = 500;
22 float dt = 0.1;
23 int cmdID = 0;
24 int ret = robot.GetActualJointPosDegree(flag, &j);
25 if (ret == 0)
26 {
27 robot.ServoMoveStart();
28 while (count)
29 {
30 robot.ServoJ(&j, &epos, acc, vel, cmdT, filterT, gain, cmdID);
31 j.jPos[0] += dt;
32 count -= 1;
33 robot.WaitMs(cmdT * 1000);
34 }
35 robot.ServoMoveEnd();
36 }
37 else
38 {
39 printf("GetActualJointPosDegree errcode:%d\n", ret);
40 }
41 robot.CloseRPC();
42 return 0;
43}
4.22. Przykład kodu ruchu w trybie serwo w przestrzeni stawów robota opartego na komunikacji UDP
1void UDPFrameCallBack(int srcType, int count, int cmdID, int len, std::string content)
2{
3 cout << "recv cmd: cmdID: " << to_string(cmdID) << " content is " << content << " count is " << count << endl;;
4 return;
5}
6
7int TestServoJUDP(void)
8{
9 ROBOT_STATE_PKG pkg = {};
10 FRRobot robot;
11 int rtn = 0;
12 robot.LoggerInit();
13 robot.SetLoggerLevel(1);
14 rtn = robot.SetCmdRpyCallback(UDPFrameCallBack);
15 printf("SetCmdRpyCallback rtn is %d\n", rtn);
16 rtn = robot.RPC("192.168.58.2");
17 if (rtn != 0)
18 {
19 return -1;
20 }
21 robot.SetReConnectParam(true, 30000, 50);
22 JointPos j(0, -90, 90, 0, 0, 0);
23 ExaxisPos epos(0, 0, 0, 0);
24 DescPose offset_pos(0, 0, 0, 0, 0, 0);
25 while (true)
26 {
27 robot.MoveJ(&j, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
28 float vel = 0.0;
29 float acc = 0.0;
30 float cmdT = 0.016;
31 float filterT = 0.0;
32 float gain = 0.0;
33 uint8_t flag = 0;
34 float dt = 0.1;
35 int cmdID = 0;
36 int ret = robot.GetActualJointPosDegree(flag, &j);
37 if (ret != 0)
38 {
39 printf("GetActualJointPosDegree errcode:%d\n", ret);
40 }
41 int comType = 1;
42 int count = 300;
43 rtn = robot.ServoMoveStart(comType);
44 printf("ServoMoveStart rtn is %d\n", rtn);
45 while (count)
46 {
47 rtn = robot.ServoJ(&j, &epos, acc, vel, cmdT, filterT, gain, cmdID, comType);
48 printf("ServoJ rtn is %d\n", rtn);
49 j.jPos[0] += dt;
50 j.jPos[1] += dt;
51 j.jPos[2] += dt;
52 j.jPos[3] += dt;
53 j.jPos[4] += dt;
54 j.jPos[5] += dt;
55 epos.ePos[0] += dt;
56 count -= 1;
57 robot.Sleep(15);
58 }
59 robot.ServoMoveEnd(comType);
60 printf("ServoMoveEnd rtn is %d\n", rtn);
61 count = 300;
62 robot.ServoMoveStart(comType);
63 printf("ServoMoveStart rtn is %d\n", rtn);
64 while (count)
65 {
66 robot.ServoJ(&j, &epos, acc, vel, cmdT, filterT, gain, cmdID, comType);
67 printf("ServoJ rtn is %d\n", rtn);
68 j.jPos[0] -= dt;
69 j.jPos[1] -= dt;
70 j.jPos[2] -= dt;
71 j.jPos[3] -= dt;
72 j.jPos[4] -= dt;
73 j.jPos[5] -= dt;
74 epos.ePos[0] -= dt;
75 count -= 1;
76 robot.Sleep(15);
77 }
78 robot.ServoMoveEnd(comType);
79 printf("ServoMoveEnd rtn is %d\n", rtn);
80 }
81 robot.Sleep(4000);
82 robot.CloseRPC();
83 return 0;
84}
4.23. Rozpoczęcie sterowania momentem stawów
Nowe w wersji C++SDK-v2.1.5.0.
1/**
2* @brief Rozpoczęcie sterowania momentem stawów
3* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
4* @return Kod błędu
5*/
6errno_t ServoJTStart(int comType = 0);
4.24. Sterowanie momentem stawów
Nowe w wersji C++SDK-v2.1.5.0.
1/**
2* @brief Sterowanie momentem stawów
3* @param [in] torque Moment stawów j1~j6, jednostka Nm
4* @param [in] interval Okres instrukcji, jednostka s, zakres [0.001~0.008]
5* @param [in] checkFlag Strategia wykrywania 0-brak ograniczenia; 1-ograniczenie mocy; 2-ograniczenie prędkości; 3-jednoczesne ograniczenie mocy i prędkości
6* @param [in] jPowerLimit Maksymalne ograniczenie mocy stawu (W)
7* @param [in] jVelLimit Maksymalna prędkość stawu (°/s)
8* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
9* @return Kod błędu
10*/
11errno_t ServoJT(float torque[], double interval, int checkFlag, double jPowerLimit[6], double jVelLimit[6], int comType = 0);
4.25. Zakończenie sterowania momentem stawów
Nowe w wersji C++SDK-v2.1.5.0.
1/**
2* @brief Zakończenie sterowania momentem stawów
3* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
4* @return Kod błędu
5*/
6errno_t ServoJTEnd(int comType = 0);
4.26. Przykład kodu sterowania momentem stawów
Nowe w wersji C++SDK-v2.1.5.0.
1int TestServoJT(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 robot.DragTeachSwitch(1);
14 float torques[] = { 0, 0, 0, 0, 0, 0 };
15 robot.GetJointTorques(1, torques);
16 int count = 100;
17 robot.ServoJTStart();
18 int error = 0;
19 while (count > 0)
20 {
21 error = robot.ServoJT(torques, 0.001);
22 count = count - 1;
23 robot.Sleep(1);
24 }
25 error = robot.ServoJTEnd();
26 robot.DragTeachSwitch(0);
27 robot.CloseRPC();
28 return 0;
29 }
4.27. Przykład kodu sterowania momentem stawów z ochroną przed przekroczeniem prędkości
1int ServoJTWithSafety(FRRobot* robot)
2{
3 robot->ResetAllError();
4 robot->Sleep(500);
5 float torques[] = { 0, 0, 0, 0, 0, 0 };
6 robot->GetJointTorques(1, torques);
7 robot->ServoJTStart();
8 ROBOT_STATE_PKG pkg = {};
9 robot->DragTeachSwitch(1);
10 int checkFlag = 3;
11 //double jPowerLimit[6] = {1, 1, 1, 1, 1, 1};
12 double jPowerLimit[6] = { 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 };
13 double jVelLimit[6] = { 181, 80, 80, 80, 80, 80 };
14 int count = 800000;
15 int error = 0;
16 while (count > 0)
17 {
18 torques[2] = torques[2] + 0.01;
19 error = robot->ServoJT(torques, 0.008, checkFlag, jPowerLimit, jVelLimit);
20 if (error != 0)
21 {
22 robot->ServoJTEnd();
23 }
24 printf("ServoJT rtn is %d\n", error);
25 count = count - 1;
26 robot->Sleep(1);
27 robot->GetRobotRealTimeState(&pkg);
28 printf("maincode %d, subcode %d\n", pkg.main_code, pkg.sub_code);
29 }
30 robot->DragTeachSwitch(0);
31 error = robot->ServoJTEnd();
32 return 0;
33}
4.28. Przykład kodu sterowania momentem stawów robota opartego na komunikacji UDP
1void UDPFrameCallBack(int srcType, int count, int cmdID, int len, std::string content)
2{
3 cout << "recv cmd: cmdID: " << to_string(cmdID) << " content is " << content << " count is " << count << endl;
4
5 return;
6}
7int TestServoJTUDP(void)
8{
9 ROBOT_STATE_PKG pkg = {};
10 FRRobot robot;
11 robot.LoggerInit();
12 robot.SetLoggerLevel(1);
13 robot.SetCmdRpyCallback(UDPFrameCallBack);
14 int rtn = robot.RPC("192.168.58.2");
15 if (rtn != 0)
16 {
17 return -1;
18 }
19 robot.SetReConnectParam(true, 30000, 500);
20 JointPos j(0, -90, 90, 0, 0, 0);
21 ExaxisPos epos(0, 0, 0, 0);
22 DescPose offset_pos(0, 0, 0, 0, 0, 0);
23 robot.MoveJ(&j, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
24 robot.Sleep(1000);
25 robot.DragTeachSwitch(1);
26 float torques[] = { 0, 0, 0, 0, 0, 0 };
27 robot.GetJointTorques(1, torques);
28 int comType = 1;
29 int count = 100;
30 int checkFlag = 3;
31 double jPowerLimit[6] = { 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 };
32 double jVelLimit[6] = { 80, 80, 80, 80, 80, 80 };
33 rtn = robot.ServoJTStart(comType);
34 printf("ServoJTStart rtn is %d\n", rtn);
35 while (true)
36 {
37 torques[0] = 0.05;
38 rtn = robot.ServoJT(torques, 0.001, checkFlag, jPowerLimit, jVelLimit, comType);
39 printf("ServoJT rtn is %d\n", rtn);
40 robot.Sleep(1);
41 robot.GetRobotRealTimeState(&pkg);
42 if (pkg.jt_cur_pos[0] > 30)
43 {
44 break;
45 }
46 }
47 while (true)
48 {
49 torques[0] = -0.03;
50 rtn = robot.ServoJT(torques, 0.001, checkFlag, jPowerLimit, jVelLimit, comType);
51 printf("ServoJT rtn is %d\n", rtn);
52 robot.Sleep(1);
53 robot.GetRobotRealTimeState(&pkg);
54 if (pkg.jt_cur_pos[0] < 0 || pkg.jt_cur_pos[1] < -110)
55 {
56 break;
57 }
58 }
59 rtn = robot.ServoJTEnd(comType);
60 printf("ServoJTEnd rtn is %d\n", rtn);
61 robot.DragTeachSwitch(0);
62 robot.Sleep(1000);
63 robot.CloseRPC();
64 return 0;
65}
4.29. Ruch w trybie serwo w przestrzeni kartezjańskiej
1/**
2* @brief Ruch w trybie serwo w przestrzeni kartezjańskiej
3* @param [in] mode 0-ruch absolutny (układ bazowy), 1-ruch przyrostowy (układ bazowy), 2-ruch przyrostowy (układ narzędzia)
4* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych lub przyrost pozy i orientacji
5* @param [in] exaxis Pozycja osi rozszerzonej
6* @param [in] pos_gain Współczynnik proporcjonalności przyrostu pozy i orientacji, działa tylko w ruchu przyrostowym, zakres [0~1]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne, domyślnie 0
8* @param [in] vel Procent prędkości, zakres [0~100], tymczasowo niedostępne, domyślnie 0
9* @param [in] cmdT Okres wysyłania instrukcji, jednostka s, zalecany zakres [0.001~0.016]
10* @param [in] filterT Czas filtracji, jednostka s, tymczasowo niedostępny, domyślnie 0
11* @param [in] gain Wzmacniacz proporcjonalności pozycji docelowej, tymczasowo niedostępny, domyślnie 0
12* @return Kod błędu
13*/
14errno_t ServoCart(int mode, DescPose *desc_pose, ExaxisPos exaxis, float pos_gain[6], float acc, float vel, float cmdT, float filterT, float gain);
4.30. Przykład kodu ruchu w trybie serwo w przestrzeni kartezjańskiej
1int TestServoCart(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 desc_pos_dt = { 83.00800, 50.525000 , 29.246 , 179.629 , -7.138 , -166.975 };
14 ExaxisPos exaxis = { 100.0, 0.0, 0.0, 0.0 };
15 float pos_gain[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
16 int mode = 0;
17 float vel = 0.0;
18 float acc = 0.0;
19 float cmdT = 0.001;
20 float filterT = 0.0;
21 float gain = 0.0;
22 uint8_t flag = 0;
23 int count = 5000;
24 robot.SetSpeed(20);
25 while (count)
26 {
27 rtn = robot.ServoCart(mode, &desc_pos_dt, exaxis, pos_gain, acc, vel, cmdT, filterT, gain);
28 printf("ServoCart rtn is %d\n", rtn);
29 count -= 1;
30 desc_pos_dt.tran.x += 0.01;
31 exaxis.ePos[0] += 0.01;
32 }
33 robot.CloseRPC();
34 return 0;
35}
4.31. Rozpoczęcie ruchu funkcją sklejaną (Spline)
1/**
2* @brief Rozpoczęcie ruchu funkcją sklejaną
3* @return Kod błędu
4*/
5errno_t SplineStart();
4.32. Ruch funkcją sklejaną w przestrzeni stawów (automatyczne obliczanie prostej kinematyki)
1/**
2* @brief Ruch funkcją sklejaną w przestrzeni stawów (automatyczne obliczanie prostej kinematyki)
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] vel Procent prędkości, zakres [0~100]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
9* @return Kod błędu
10*/
11errno_t SplinePTP(JointPos* joint_pos, int tool, int user, float vel, float acc, float ovl);
4.33. Ruch funkcją sklejaną PTP
1/**
2* @brief Ruch funkcją sklejaną w przestrzeni stawów
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
5* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] vel Procent prędkości, zakres [0~100]
8* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
10* @return Kod błędu
11*/
12errno_t SplinePTP(JointPos *joint_pos, DescPose *desc_pos, int tool, int user, float vel, float acc, float ovl);
4.34. Zakończenie ruchu funkcją sklejaną
1/**
2* @brief Zakończenie ruchu funkcją sklejaną
3* @return Kod błędu
4*/
5errno_t SplineEnd();
4.35. Przykład kodu ruchu funkcją sklejaną
1int TestSpline(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 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 JointPos j3(-61.954, -84.409, 108.153, -116.316, -91.283, 74.260);
16 JointPos j4(-89.575, -80.276, 102.713, -116.302, -91.284, 74.267);
17 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
18 DescPose desc_pos2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
19 DescPose desc_pos3(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
20 DescPose desc_pos4(-104.066, 544.321, 327.023, -177.715, 3.371, -73.818);
21 DescPose offset_pos(0, 0, 0, 0, 0, 0);
22 ExaxisPos epos(0, 0, 0, 0);
23 int tool = 0;
24 int user = 0;
25 float vel = 100.0;
26 float acc = 100.0;
27 float ovl = 100.0;
28 float blendT = -1.0;
29 uint8_t flag = 0;
30 robot.SetSpeed(20);
31 int err1 = robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
32 printf("movej errcode:%d\n", err1);
33 robot.SplineStart();
34 robot.SplinePTP(&j1, &desc_pos1, tool, user, vel, acc, ovl);
35 robot.SplinePTP(&j2, &desc_pos2, tool, user, vel, acc, ovl);
36 robot.SplinePTP(&j3, &desc_pos3, tool, user, vel, acc, ovl);
37 robot.SplinePTP(&j4, &desc_pos4, tool, user, vel, acc, ovl);
38 robot.SplineEnd();
39 err1 = robot.MoveJ(&j1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
40 printf("movej errcode:%d\n", err1);
41 robot.SplineStart();
42 robot.SplinePTP(&j1, tool, user, vel, acc, ovl);
43 robot.SplinePTP(&j2, tool, user, vel, acc, ovl);
44 robot.SplinePTP(&j3, tool, user, vel, acc, ovl);
45 robot.SplinePTP(&j4, tool, user, vel, acc, ovl);
46 robot.SplineEnd();
47 robot.CloseRPC();
48 return 0;
49}
4.36. Rozpoczęcie nowego ruchu funkcją sklejaną
Zmienione w wersji C++SDK-v2.1.3.0.
1/**
2* @brief Rozpoczęcie nowego ruchu funkcją sklejaną
3* @param [in] type 0-przejście łukowe, 1-punkty zadane są punktami ścieżki
4* @param [in] averageTime Globalny średni czas połączenia (ms) (10 ~ ), domyślnie 2000
5* @return Kod błędu
6*/
7errno_t NewSplineStart(int type, int averageTime=2000);
4.37. Punkt nowej funkcji sklejanej
1/**
2* @brief Punkt nowej funkcji sklejanej
3* @param [in] joint_pos Docelowa pozycja stawów, jednostka deg
4* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
5* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
6* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
7* @param [in] vel Procent prędkości, zakres [0~100]
8* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
9* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
10* @param [in] blendR [-1.0]-ruch do pozycji (blokujący), [0~1000.0]-promień wygładzania (nieblokujący), jednostka mm
11* @param [in] lastFlag Czy to ostatni punkt, 0-nie, 1-tak
12* @return Kod błędu
13*/
14errno_t NewSplinePoint(JointPos *joint_pos, DescPose *desc_pos, int tool, int user, float vel, float acc, float ovl, float blendR, int lastFlag);
4.38. Punkt nowej funkcji sklejanej (automatyczne obliczanie odwrotnej kinematyki)
1/**
2* @brief Punkt nowej funkcji sklejanej (automatyczne obliczanie odwrotnej kinematyki)
3* @param [in] desc_pos Docelowa poza i orientacja w kartezjańskim układzie współrzędnych
4* @param [in] tool Numer układu współrzędnych narzędzia, zakres [0~14]
5* @param [in] user Numer układu współrzędnych obiektu, zakres [0~14]
6* @param [in] vel Procent prędkości, zakres [0~100]
7* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne
8* @param [in] ovl Współczynnik skalowania prędkości, zakres [0~100]
9* @param [in] blendR [-1.0]-ruch do pozycji (blokujący), [0~1000.0]-promień wygładzania (nieblokujący), jednostka mm
10* @param [in] lastFlag Czy to ostatni punkt, 0-nie, 1-tak
11* @param [in] config Konfiguracja przestrzeni stawów dla odwrotnej kinematyki, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia zgodnie z określoną konfiguracją przestrzeni stawów
12* @return Kod błędu
13*/
14errno_t NewSplinePoint(DescPose* desc_pos, int tool, int user, float vel, float acc, float ovl, float blendR, int lastFlag, int config = -1);
4.39. Zakończenie nowego ruchu funkcją sklejaną
1/**
2* @brief Zakończenie nowego ruchu funkcją sklejaną
3* @return Kod błędu
4*/
5errno_t NewSplineEnd();
4.40. Przykład kodu nowego ruchu funkcją sklejaną
1int TestNewSpline(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 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 JointPos j3(-61.954, -84.409, 108.153, -116.316, -91.283, 74.260);
16 JointPos j4(-89.575, -80.276, 102.713, -116.302, -91.284, 74.267);
17 JointPos j5(-95.228, -54.621, 73.691, -112.245, -91.280, 74.268);
18 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
19 DescPose desc_pos2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
20 DescPose desc_pos3(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
21 DescPose desc_pos4(-104.066, 544.321, 327.023, -177.715, 3.371, -73.818);
22 DescPose desc_pos5(-33.421, 732.572, 275.103, -177.907, 2.709, -79.482);
23 DescPose offset_pos(0, 0, 0, 0, 0, 0);
24 ExaxisPos epos(0, 0, 0, 0);
25 int tool = 0;
26 int user = 0;
27 float vel = 100.0;
28 float acc = 100.0;
29 float ovl = 100.0;
30 float blendT = -1.0;
31 uint8_t flag = 0;
32 robot.SetSpeed(20);
33 int err1 = robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
34 printf("movej errcode:%d\n", err1);
35 robot.NewSplineStart(1, 2000);
36 robot.NewSplinePoint(&j1, &desc_pos1, tool, user, vel, acc, ovl, -1, 0);
37 robot.NewSplinePoint(&j2, &desc_pos2, tool, user, vel, acc, ovl, -1, 0);
38 robot.NewSplinePoint(&j3, &desc_pos3, tool, user, vel, acc, ovl, -1, 0);
39 robot.NewSplinePoint(&j4, &desc_pos4, tool, user, vel, acc, ovl, -1, 0);
40 robot.NewSplinePoint(&j5, &desc_pos5, tool, user, vel, acc, ovl, -1, 0);
41 robot.NewSplineEnd();
42 err1 = robot.MoveJ(&j1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
43 printf("movej errcode:%d\n", err1);
44 robot.NewSplineStart(1, 2000);
45 robot.NewSplinePoint(&desc_pos1, tool, user, vel, acc, ovl, -1, 0);
46 robot.NewSplinePoint(&desc_pos2, tool, user, vel, acc, ovl, -1, 0);
47 robot.NewSplinePoint(&desc_pos3, tool, user, vel, acc, ovl, -1, 0);
48 robot.NewSplinePoint(&desc_pos4, tool, user, vel, acc, ovl, -1, 0);
49 robot.NewSplinePoint(&desc_pos5, tool, user, vel, acc, ovl, -1, 0);
50 robot.NewSplineEnd();
51 robot.CloseRPC();
52 return 0;
53}
4.41. Zatrzymanie ruchu
1/**
2* @brief Zatrzymanie ruchu
3* @return Kod błędu
4*/
5errno_t StopMotion();
4.42. Wstrzymanie ruchu
1/**
2* @brief Wstrzymanie ruchu
3* @return Kod błędu
4*/
5errno_t PauseMotion();
4.43. Wznowienie ruchu
1/**
2* @brief Wznowienie ruchu
3* @return Kod błędu
4*/
5errno_t ResumeMotion();
4.44. Przykład kodu wstrzymywania, wznawiania i zatrzymywania ruchu
1 int TestPause(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 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos j5(-95.228, -54.621, 73.691, -112.245, -91.280, 74.268);
15 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
16 DescPose desc_pos5(-33.421, 732.572, 275.103, -177.907, 2.709, -79.482);
17 DescPose offset_pos(0, 0, 0, 0, 0, 0);
18 ExaxisPos epos(0, 0, 0, 0);
19 int tool = 0;
20 int user = 0;
21 float vel = 100.0;
22 float acc = 100.0;
23 float ovl = 100.0;
24 float blendT = -1.0;
25 uint8_t flag = 0;
26 robot.SetSpeed(20);
27 rtn = robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
28 rtn = robot.MoveJ(&j5, &desc_pos5, tool, user, vel, acc, ovl, &epos, 1, flag, &offset_pos);
29 robot.Sleep(1000);
30 robot.PauseMotion();
31 robot.Sleep(1000);
32 robot.ResumeMotion();
33 robot.Sleep(1000);
34 robot.StopMotion();
35 robot.Sleep(1000);
36 robot.CloseRPC();
37 return 0;
38 }
4.45. Rozpoczęcie globalnego przesunięcia punktów
1/**
2* @brief Rozpoczęcie globalnego przesunięcia punktów
3* @param [in] flag 0-przesunięcie w układzie bazowym/układzie obiektu, 2-przesunięcie w układzie narzędzia
4* @param [in] offset_pos Wartość przesunięcia pozy i orientacji
5* @return Kod błędu
6*/
7errno_t PointsOffsetEnable(int flag, DescPose *offset_pos);
4.46. Zakończenie globalnego przesunięcia punktów
1/**
2* @brief Zakończenie globalnego przesunięcia punktów
3* @return Kod błędu
4*/
5errno_t PointsOffsetDisable();
4.47. Przykład kodu przesunięcia punktów
1 int TestOffset(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 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
16 DescPose desc_pos2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
17 DescPose offset_pos(0, 0, 0, 0, 0, 0);
18 DescPose offset_pos1(0, 0, 50, 0, 0, 0);
19 ExaxisPos epos(0, 0, 0, 0);
20 int tool = 0;
21 int user = 0;
22 float vel = 100.0;
23 float acc = 100.0;
24 float ovl = 100.0;
25 float blendT = -1.0;
26 uint8_t flag = 0;
27 robot.SetSpeed(20);
28 robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
29 robot.MoveJ(&j2, &desc_pos2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
30 robot.Sleep(1000);
31 robot.PointsOffsetEnable(0, &offset_pos1);
32 robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
33 robot.MoveJ(&j2, &desc_pos2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
34 robot.PointsOffsetDisable();
35 robot.CloseRPC();
36 return 0;
37 }
4.48. Rozpoczęcie pomiaru przelotowego AO szafy sterowniczej
Nowe w wersji C++SDK-v2.1.4.0.
1/**
2* @brief Rozpoczęcie pomiaru przelotowego AO szafy sterowniczej
3* @param [in] AONum Numer AO szafy sterowniczej
4* @param [in] maxTCPSpeed Maksymalna wartość prędkości TCP [1-5000mm/s], domyślnie 1000
5* @param [in] maxAOPercent Procent AO odpowiadający maksymalnej wartości prędkości TCP, domyślnie 100%
6* @param [in] zeroZoneCmp Procent AO dla kompensacji martwego pola, liczba całkowita, domyślnie 20%, zakres [0-100]
7* @return Kod błędu
8*/
9errno_t MoveAOStart(int AONum, int maxTCPSpeed, int maxAOPercent, int zeroZoneCmp);
4.49. Zatrzymanie pomiaru przelotowego AO szafy sterowniczej
Nowe w wersji C++SDK-v2.1.4.0.
1/**
2* @brief Zatrzymanie pomiaru przelotowego AO szafy sterowniczej
3* @return Kod błędu
4*/
5errno_t MoveAOStop();
4.50. Rozpoczęcie pomiaru przelotowego AO końcówki
Nowe w wersji C++SDK-v2.1.4.0.
1/**
2* @brief Rozpoczęcie pomiaru przelotowego AO końcówki
3* @param [in] AONum Numer AO końcówki
4* @param [in] maxTCPSpeed Maksymalna wartość prędkości TCP [1-5000mm/s], domyślnie 1000
5* @param [in] maxAOPercent Procent AO odpowiadający maksymalnej wartości prędkości TCP, domyślnie 100%
6* @param [in] zeroZoneCmp Procent AO dla kompensacji martwego pola, liczba całkowita, domyślnie 20%, zakres [0-100]
7* @return Kod błędu
8*/
9errno_t MoveToolAOStart(int AONum, int maxTCPSpeed, int maxAOPercent, int zeroZoneCmp);
4.51. Zatrzymanie pomiaru przelotowego AO końcówki
Nowe w wersji C++SDK-v2.1.4.0.
1/**
2* @brief Zatrzymanie pomiaru przelotowego AO końcówki
3* @return Kod błędu
4*/
5errno_t MoveToolAOStop();
4.52. Przykład kodu pomiaru przelotowego AO
1 int TestMoveAO(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 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
16 DescPose desc_pos2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
17 DescPose offset_pos(0, 0, 0, 0, 0, 0);
18 DescPose offset_pos1(0, 0, 50, 0, 0, 0);
19 ExaxisPos epos(0, 0, 0, 0);
20 int tool = 0;
21 int user = 0;
22 float vel = 20.0;
23 float acc = 20.0;
24 float ovl = 100.0;
25 float blendT = -1.0;
26 uint8_t flag = 0;
27 robot.SetSpeed(20);
28 robot.MoveAOStart(0, 100, 100, 20);
29 robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
30 robot.MoveJ(&j2, &desc_pos2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
31 robot.MoveAOStop();
32 robot.Sleep(1000);
33 robot.MoveToolAOStart(0, 100, 100, 20);
34 robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
35 robot.MoveJ(&j2, &desc_pos2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
36 robot.MoveToolAOStop();
37 robot.CloseRPC();
38 return 0;
39 }
4.53. Rozpoczęcie filtracji FIR ruchu PTP
Nowe w wersji V3.7.7.
1/**
2* @brief Rozpoczęcie filtracji FIR ruchu PTP
3* @param [in] maxAcc Maksymalna wartość przyspieszenia (deg/s2)
4* @param [in] maxJek Maksymalna wartość zrywu dla wszystkich stawów (deg/s3)
5* @return Kod błędu
6*/
7errno_t PtpFIRPlanningStart(double maxAcc, double maxJek = 1000);
4.54. Zakończenie filtracji FIR ruchu PTP
Nowe w wersji V3.7.7.
1/**
2 * @brief Zakończenie filtracji FIR ruchu PTP
3 * @return Kod błędu
4 */
5 errno_t PtpFIRPlanningEnd();
4.55. Rozpoczęcie filtracji FIR ruchu LIN i ARC
Nowe w wersji V3.7.7.
1/**
2 * @brief Rozpoczęcie filtracji FIR ruchu LIN i ARC
3 * @param [in] maxAccLin Maksymalna wartość liniowego przyspieszenia (mm/s2)
4 * @param [in] maxAccDeg Maksymalna wartość kątowego przyspieszenia (deg/s2)
5 * @param [in] maxJerkLin Maksymalna wartość liniowego zrywu (mm/s3)
6 * @param [in] maxJerkDeg Maksymalna wartość kątowego zrywu (deg/s3)
7 * @return Kod błędu
8 */
9 errno_t LinArcFIRPlanningStart(double maxAccLin, double maxAccDeg, double maxJerkLin, double maxJerkDeg);
4.56. Zakończenie filtracji FIR ruchu LIN i ARC
Nowe w wersji V3.7.7.
1/**
2 * @brief Zakończenie filtracji FIR ruchu LIN i ARC
3 * @return Kod błędu
4 */
5 errno_t LinArcFIRPlanningEnd();
4.57. Przykład kodu filtracji FIR
1 int TestFIR(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 JointPos startjointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos midjointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 JointPos endjointPos(-29.777, -84.536, 109.275, -114.075, -86.655, 74.257);
16 DescPose startdescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
17 DescPose middescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
18 DescPose enddescPose(-487.434, 154.362, 308.576, 176.600, 0.268, -14.061);
19 ExaxisPos exaxisPos(0, 0, 0, 0);
20 DescPose offdese(0, 0, 0, 0, 0, 0);
21 rtn = robot.PtpFIRPlanningStart(1000, 1000);
22 cout << "PtpFIRPlanningStart rtn is " << rtn << endl;
23 robot.MoveJ(&startjointPos, &startdescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
24 robot.MoveJ(&endjointPos, &enddescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
25 robot.PtpFIRPlanningEnd();
26 cout << "PtpFIRPlanningEnd rtn is " << rtn << endl;
27 robot.LinArcFIRPlanningStart(1000, 1000, 1000, 1000);
28 cout << "LinArcFIRPlanningStart rtn is " << rtn << endl;
29 robot.MoveL(&startjointPos, &startdescPose, 0, 0, 100, 100, 100, -1, &exaxisPos, 0, 0, &offdese, 1, 1);
30 robot.MoveC(&midjointPos, &middescPose, 0, 0, 100, 100, &exaxisPos, 0, &offdese, &endjointPos, &enddescPose, 0, 0, 100, 100, &exaxisPos, 0, &offdese, 100, -1);
31 robot.LinArcFIRPlanningEnd();
32 cout << "LinArcFIRPlanningEnd rtn is " << rtn << endl;
33 robot.CloseRPC();
34 return 0;
35 }
4.58. Włączenie wygładzania przyspieszenia
Nowe w wersji C++SDK-v2.2.1-3.8.1.
1/**
2* @brief Włączenie wygładzania przyspieszenia
3* @param [in] saveFlag Czy zapisać po odcięciu zasilania
4* @return Kod błędu
5*/
6errno_t AccSmoothStart(bool saveFlag);
4.59. Wyłączenie wygładzania przyspieszenia
Nowe w wersji C++SDK-v2.2.1-3.8.1.
1/**
2* @brief Wyłączenie wygładzania przyspieszenia
3* @param [in] saveFlag Czy zapisać po odcięciu zasilania
4* @return Kod błędu
5*/
6errno_t AccSmoothEnd(bool saveFlag);
4.60. Przykład kodu wygładzania przyspieszenia
1int TestAccSmooth(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 JointPos startjointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos endjointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 DescPose startdescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
16 DescPose enddescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
17 ExaxisPos exaxisPos(0, 0, 0, 0);
18 DescPose offdese(0, 0, 0, 0, 0, 0);
19 rtn = robot.AccSmoothStart(0);
20 cout << "AccSmoothStart rtn is " << rtn << endl;
21 robot.MoveJ(&startjointPos, &startdescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
22 robot.MoveJ(&endjointPos, &enddescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
23 rtn = robot.AccSmoothEnd(0);
24 cout << "AccSmoothEnd rtn is " << rtn << endl;
25 robot.CloseRPC();
26 return 0;
27 }
4.61. Włączenie określonej prędkości postawy
Nowe w wersji C++SDK-v2.1.5.0.
1/**
2* @brief Włączenie określonej prędkości postawy
3* @param [in] ratio Procent prędkości postawy [0-300]
4* @return Kod błędu
5*/
6errno_t AngularSpeedStart(int ratio);
4.62. Wyłączenie określonej prędkości postawy
Nowe w wersji C++SDK-v2.1.5.0.
1/**
2* @brief Wyłączenie określonej prędkości postawy
3* @return Kod błędu
4*/
5errno_t AngularSpeedEnd();
4.63. Przykład kodu określonej prędkości postawy robota
1int TestAngularSpeed(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 JointPos startjointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos endjointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 DescPose startdescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
16 DescPose enddescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
17 ExaxisPos exaxisPos(0, 0, 0, 0);
18 DescPose offdese(0, 0, 0, 0, 0, 0);
19 rtn = robot.AngularSpeedStart(50);
20 cout << "AngularSpeedStart rtn is " << rtn << endl;
21 robot.MoveJ(&startjointPos, &startdescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
22 robot.MoveJ(&endjointPos, &enddescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
23 rtn = robot.AngularSpeedEnd();
24 cout << "AngularSpeedEnd rtn is " << rtn << endl;
25 robot.CloseRPC();
26 return 0;
27 }
4.64. Rozpoczęcie ochrony przed osobliwą pozą i orientacją
Nowe w wersji C++SDK-v2.1.5.0.
1 /**
2 * @brief Rozpoczęcie ochrony przed osobliwą pozą i orientacją
3 * @param [in] protectMode Tryb ochrony przed osobliwością, 0: tryb stawów; 1: tryb kartezjański
4 * @param [in] minShoulderPos Zakres regulacji osobliwości ramienia (mm), domyślnie 100
5 * @param [in] minElbowPos Zakres regulacji osobliwości łokcia (mm), domyślnie 50
6 * @param [in] minWristPos Zakres regulacji osobliwości nadgarstka (°), domyślnie 10
7 * @return Kod błędu
8 */
9 errno_t SingularAvoidStart(int protectMode, double minShoulderPos, double minElbowPos, double minWristPos);
4.65. Zatrzymanie ochrony przed osobliwą pozą i orientacją
Nowe w wersji C++SDK-v2.1.5.0.
1 /**
2 * @brief Zatrzymanie ochrony przed osobliwą pozą i orientacją
3 * @return Kod błędu
4 */
5 errno_t SingularAvoidEnd();
4.66. Przykład kodu ochrony przed osobliwą pozą i orientacją robota
1int TestAngularSpeed(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 JointPos startjointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos endjointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 DescPose startdescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
16 DescPose enddescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
17 ExaxisPos exaxisPos(0, 0, 0, 0);
18 DescPose offdese(0, 0, 0, 0, 0, 0);
19 rtn = robot.SingularAvoidStart(2, 10, 5, 5);
20 cout << "SingularAvoidStart rtn is " << rtn << endl;
21 robot.MoveJ(&startjointPos, &startdescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
22 robot.MoveJ(&endjointPos, &enddescPose, 0, 0, 100, 100, 100, &exaxisPos, -1, 0, &offdese);
23 rtn = robot.SingularAvoidEnd();
24 cout << "SingularAvoidEnd rtn is " << rtn << endl;
25 robot.CloseRPC();
26 return 0;
27 }
4.67. Wyczyszczenie kolejki instrukcji ruchu
1/**
2* @brief Wyczyszczenie kolejki instrukcji ruchu
3* @return Kod błędu
4*/
5errno_t MotionQueueClear();
4.68. Przejazd do punktu początkowego linii przecięcia rur
1/**
2* @brief Przejazd do punktu początkowego linii przecięcia rur
3* @param [in] mainPoint Pozy i orientacje kartezjańskie 6 punktów nauczania rury głównej
4* @param [in] mainExaxisPos Pozycje osi rozszerzonej dla 6 punktów nauczania rury głównej
5* @param [in] piecePoint Pozy i orientacje kartezjańskie 6 punktów nauczania rury pomocniczej
6* @param [in] pieceExaxisPos Pozycje osi rozszerzonej dla 6 punktów nauczania rury łączącej
7* @param [in] extAxisFlag Czy włączyć oś rozszerzoną; 0-nie włączaj; 1-włącz
8* @param [in] exaxisPos Pozycja osi rozszerzonej punktu początkowego
9* @param [in] tool Numer układu współrzędnych narzędzia
10* @param [in] wobj Numer układu współrzędnych obiektu
11* @param [in] vel Procent prędkości
12* @param [in] acc Procent przyspieszenia
13* @param [in] ovl Współczynnik skalowania prędkości
14* @param [in] oacc Współczynnik skalowania przyspieszenia
15* @param [in] moveType Typ ruchu; 0-PTP; 1-LIN
16* @param [in] moveDirection Kierunek ruchu; 0-zgodnie z ruchem wskazówek zegara; 1-przeciwnie do ruchu wskazówek zegara
17* @param [in] offset Wartość przesunięcia
18* @return Kod błędu
19*/
20errno_t MoveToIntersectLineStart(DescPose mainPoint[6], ExaxisPos mainExaxisPos[6], DescPose piecePoint[6], ExaxisPos pieceExaxisPos[6], int extAxisFlag, ExaxisPos exaxisPos, int tool, int wobj, double vel, double acc, double ovl, double oacc, int moveType, int moveDirection, DescPose offset);
4.69. Ruch po linii przecięcia rur
1/**
2* @brief Ruch po linii przecięcia rur
3* @param [in] mainPoint Pozy i orientacje kartezjańskie 6 punktów nauczania rury głównej
4* @param [in] mainExaxisPos Pozycje osi rozszerzonej dla 6 punktów nauczania rury głównej
5* @param [in] piecePoint Pozy i orientacje kartezjańskie 6 punktów nauczania rury pomocniczej
6* @param [in] pieceExaxisPos Pozycje osi rozszerzonej dla 6 punktów nauczania rury łączącej
7* @param [in] extAxisFlag Czy włączyć oś rozszerzoną; 0-nie włączaj; 1-włącz
8* @param [in] exaxisPos Pozycja osi rozszerzonej punktu początkowego
9* @param [in] tool Numer układu współrzędnych narzędzia
10* @param [in] wobj Numer układu współrzędnych obiektu
11* @param [in] vel Procent prędkości
12* @param [in] acc Procent przyspieszenia
13* @param [in] ovl Współczynnik skalowania prędkości
14* @param [in] oacc Współczynnik skalowania przyspieszenia
15* @param [in] moveDirection Kierunek ruchu; 0-zgodnie z ruchem wskazówek zegara; 1-przeciwnie do ruchu wskazówek zegara
16* @param [in] offset Wartość przesunięcia
17* @return Kod błędu
18*/
19errno_t MoveIntersectLine(DescPose mainPoint[6], ExaxisPos mainExaxisPos[6], DescPose piecePoint[6], ExaxisPos pieceExaxisPos[6], int extAxisFlag, ExaxisPos exaxisPos[4], int tool, int wobj, double vel, double acc, double ovl, double oacc, int moveDirection, DescPose offset);
4.70. Przykład kodu ruchu po linii przecięcia rur robota
1void TestIntersectLineMove()
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(3);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return ;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 DescPose mainPoint[6] = {};
14 DescPose piecePoint[6] = {};
15 ExaxisPos mainExaxisPos[6] = {};
16 ExaxisPos pieceExaxisPos[6] = {};
17 int extAxisFlag = 1;
18 ExaxisPos exaxisPos[4] = {};
19 DescPose offset = { 0.0, 2.0 ,30.0, -2.0, 0.0, 0.0 };
20 mainPoint[0] = {490.004, -383.194, 402.735, -9.332, -1.528, 69.594};
21 mainPoint[1] = {444.950, -407.117, 389.011, -5.546, -2.196, 65.279};
22 mainPoint[2] = {445.168, -463.605, 355.759, -1.544, -10.886, 57.104};
23 mainPoint[3] = {507.529, -485.385, 343.013, -0.786, -4.834, 61.799};
24 mainPoint[4] = {554.390, -442.647, 367.701, -4.761, -10.181, 64.925};
25 mainPoint[5] = {532.552, -394.003, 396.467, -13.732, -13.592, 67.411};
26 mainExaxisPos[0] = { -29.996, 0.000, 0.000, 0.000 };
27 mainExaxisPos[1] = { -29.996, 0.000, 0.000, 0.000 };
28 mainExaxisPos[2] = { -29.996, 0.000, 0.000, 0.000 };
29 mainExaxisPos[3] = { -29.996, 0.000, 0.000, 0.000 };
30 mainExaxisPos[4] = { -29.996, 0.000, 0.000, 0.000 };
31 mainExaxisPos[5] = { -29.996, 0.000, 0.000, 0.000 };
32 piecePoint[0] = { 505.571, -192.408, 316.759, 38.098, 37.051, 139.447 };
33 piecePoint[1] = {533.837, -201.558, 332.340, 34.644, 42.339, 137.748};
34 piecePoint[2] = {530.386, -225.085, 373.808, 35.431, 45.111, 137.560};
35 piecePoint[3] = {485.646, -229.195, 383.778, 33.870, 45.173, 137.064};
36 piecePoint[4] = {460.551, -212.161, 354.256, 28.856, 45.602, 135.930};
37 piecePoint[5] = {474.217, -197.124, 324.611, 42.469, 41.133, 148.167};
38 pieceExaxisPos[0] = { -29.996, -0.000, 0.000, 0.000 };
39 pieceExaxisPos[1] = { -29.996, -0.000, 0.000, 0.000 };
40 pieceExaxisPos[2] = { -29.996, -0.000, 0.000, 0.000 };
41 pieceExaxisPos[3] = { -29.996, -0.000, 0.000, 0.000 };
42 pieceExaxisPos[4] = { -29.996, -0.000, 0.000, 0.000 };
43 pieceExaxisPos[5] = { -29.996, -0.000, 0.000, 0.000 };
44 exaxisPos[0] = {-29.996, -0.000, 0.000, 0.000};
45 exaxisPos[1] = {-44.994, 90.000, 0.000, 0.000};
46 exaxisPos[2] = {-59.992, 0.002, 0.000, 0.000};
47 exaxisPos[3] = {-44.994, -89.997, 0.000, 0.000};
48 int tool = 2;
49 int wobj = 0;
50 double vel = 100.0;
51 double acc = 100.0;
52 double ovl = 12.0;
53 double oacc = 12.0;
54 int moveType = 1;
55 int moveDirection = 1;
56 rtn = robot.MoveToIntersectLineStart(mainPoint, mainExaxisPos, piecePoint, pieceExaxisPos, extAxisFlag, exaxisPos[0], tool, wobj, vel, acc, ovl, oacc, moveType, moveDirection, offset);
57 printf("MoveToIntersectLineStart rtn is %d\n", rtn);
58 rtn = robot.MoveIntersectLine(mainPoint, mainExaxisPos, piecePoint, pieceExaxisPos, extAxisFlag, exaxisPos, tool, wobj, vel, acc, 5.0, 5.0, moveDirection, offset);
59 printf("MoveIntersectLine rtn is %d\n", rtn);
60 robot.CloseRPC();
61 return ;
62}
4.71. Ruch w miejscu (bez przemieszczania)
1/**
2* @brief Ruch w miejscu (bez przemieszczania)
3* @return Kod błędu
4*/
5errno_t MoveStationary();
4.72. Przykład kodu ruchu w miejscu
1int TestLaserStationary(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 rtn = robot.LaserSensorRecordandReplay(0, 10, 1, 0, 0.1, 1, 0, 10, 100);
14 printf("LaserSensorRecordandReplay rtn is %d\n", rtn);
15 rtn = robot.MoveStationary();
16 printf("MoveStationary rtn is %d\n", rtn);
17 rtn = robot.LaserSensorRecord1(0, 10);
18 printf("LaserSensorRecordandReplay rtn is %d\n", rtn);
19 robot.CloseRPC();
20 robot.Sleep(9999999);
21 return 0;
22}
4.73. Rozpoczęcie wahadła punktowego
1/**
2* @brief Rozpoczęcie wahadła punktowego
3* @param [in] weaveNum Numer wahadła [0-7]
4* @param [in] mode 0-układ współrzędnych narzędzia; 1-punkt odniesienia
5* @param [in] refPoint Współrzędne kartezjańskie punktu odniesienia [x, y, z, a, b, c]
6* @param [in] weaveTime Czas wahadła [s]
7* @return Kod błędu
8*/
9errno_t OriginPointWeaveStart(int weaveNum, int mode, DescPose refPoint, double weaveTime);
4.74. Zakończenie wahadła punktowego
1/**
2* @brief Zakończenie wahadła punktowego
3* @return Kod błędu
4*/
5errno_t OriginPointWeaveEnd();
4.75. Przykład kodu SDK dla wahadła punktowego
1int TestOriginPointWeave()
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 JointPos j(39.886, -98.580, -124.032, -47.393, 90.000, 40.842);
14 ExaxisPos epos(0, 0, 0, 0);
15 DescPose offset_pos(0, 0, 0, 0, 0, 0);
16 DescPose refPoint = { 400.021,300.022,299.996,179.997,-0.003,-90.956 };
17 robot.MoveJ(&j, 1, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
18 robot.OriginPointWeaveStart(0, 0, refPoint, 3);
19 robot.MoveStationary();
20 robot.OriginPointWeaveEnd();
21 robot.Sleep(2000);
22 robot.MoveJ(&j, 1, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
23 robot.OriginPointWeaveStart(0, 1, refPoint, 3);
24 robot.MoveStationary();
25 robot.OriginPointWeaveEnd();
26 robot.CloseRPC();
27 robot.Sleep(1000);
28 return 0;
29}
4.76. Przykład kodu wahadła punktowego (z czujnikiem laserowym i osią rozszerzoną)
1int TestOriginPointWeave()
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 JointPos j(39.886, -98.580, -124.032, -47.393, 90.000, 40.842);
14 ExaxisPos epos1(0, 0, 0, 0);
15 DescPose offset_pos(0, 0, 0, 0, 0, 0);
16 ExaxisPos epos2(5, 0.000, 0.000, 0.000);
17 DescPose refPoint(400.021, 300.022, 299.996, 179.997, -0.003, -90.956);
18 robot.LaserTrackingSensorConfig("192.168.58.20", 5020);
19 robot.LaserTrackingSensorSamplePeriod(20);
20 robot.LoadPosSensorDriver(101);
21 robot.ExtDevLoadUDPDriver();
22 rtn = robot.SetExAxisCmdDoneTime(5000.0);
23 printf("SetExAxisCmdDoneTime rtn is %d\n" , rtn);
24 rtn = robot.ExtAxisServoOn(1, 1);
25 printf("ExtAxisServoOn axis id 1 rtn is %d\n" , rtn);
26 rtn = robot.ExtAxisServoOn(2, 1);
27 printf("ExtAxisServoOn axis id 2 rtn is %d\n" , rtn);
28 robot.Sleep(2000);
29 robot.ExtAxisSetHoming(1, 0, 10, 2);
30 rtn = robot.LaserTrackingLaserOnOff(1, 0);
31 printf("LaserTrackingLaserOnOff id 2 rtn is %d\n", rtn);
32 robot.LaserTrackingTrackOnOff(1, 4);
33 robot.Sleep(200);
34 robot.OriginPointWeaveStart(0, 0, refPoint, 10);
35 robot.MoveStationary();
36 robot.OriginPointWeaveEnd();
37 robot.LaserTrackingTrackOnOff(0, 4);
38
39 robot.Sleep(2000);
40
41 robot.ExtAxisMove(epos1, 100, -1);
42 robot.LaserTrackingTrackOnOff(1, 4);
43 robot.OriginPointWeaveStart(0, 0, refPoint, 20);
44 robot.ExtAxisMove(epos2, 100, -1);
45 robot.OriginPointWeaveEnd();
46 robot.LaserTrackingTrackOnOff(0, 4);
47 robot.CloseRPC();
48 robot.Sleep(1000);
49 return 0;
50}
4.77. Ruch w trybie serwo prędkościowym w przestrzeni stawów
1/**
2* @brief Ruch w trybie serwo prędkościowym w przestrzeni stawów
3* @param [in] jointVel 6 docelowych prędkości stawów, jednostka deg/s
4* @param [in] axisVel 4 prędkości osi zewnętrznych, jednostka deg/s
5* @param [in] acc Procent przyspieszenia, zakres [0~100], tymczasowo niedostępne, domyślnie 0
6* @param [in] vel Procent prędkości, zakres [0~100], tymczasowo niedostępne, domyślnie 0
7* @param [in] cmdT Okres wysyłania instrukcji, jednostka s, zalecany zakres [0.001~0.0016]
8* @param [in] filterT Czas filtracji, jednostka s, tymczasowo niedostępny, domyślnie 0
9* @param [in] gain Wzmacniacz proporcjonalności pozycji docelowej, tymczasowo niedostępny, domyślnie 0
10* @param [in] id ID instrukcji servoJ, domyślnie 0
11* @param[in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
12* @return Kod błędu
13*/
14errno_t ServoJV(double jointVel[6], double exisVel[4], float acc, float vel, float cmdT, float filterT, float gain, int id = 0, int comType = 0);
4.78. Przykład kodu ruchu w trybie serwo prędkościowym w przestrzeni stawów
1int ServoJVtest()
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 robot.SetReConnectParam(true, 300000, 500);
8 int rtn = robot.RPC("192.168.58.2");
9 if (rtn != 0)
10 {
11 return -1;
12 }
13 double joint_vel[6] = { 10.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14 double exis_vel[4] { 0.0, 0.0, 0.0, 0.0 };
15 float acc = 0.0f;
16 float vel = 0.0f;
17 float cmdT = 0.008f;
18 float filterT = 0.0f;
19 float gain = 0.0f;
20 int cnt = 0;
21 while (cnt < 200)
22 {
23 int error = robot.ServoJV(joint_vel, exis_vel, acc, vel, cmdT, filterT, gain);
24 printf("ServoJV rtn is %d\n", error);
25 cnt++;
26 }
27 robot.CloseRPC();
28 robot.Sleep(1000000);
29 return 0;
30}
4.79. Rozpoczęcie sterowania MIT stawów
1/**
2* @brief Rozpoczęcie sterowania MIT stawów
3* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
4* @return Kod błędu
5*/
6errno_t ServoMITStart(int comType = 0);
4.80. Zakończenie sterowania MIT stawów
1/**
2* @brief Zakończenie sterowania MIT stawów
3* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
4* @return Kod błędu
5*/
6errno_t ServoMITEnd(int comType = 0);
4.81. Sterowanie MIT stawów
1/**
2* @brief Sterowanie MIT stawów
3* @param [in] posGain Wzmocnienie pozycji stawów j1~j6
4* @param [in] desPos Oczekiwana pozycja stawów j1~j6, jednostka: deg
5* @param [in] velGain Wzmocnienie prędkości stawów j1~j6
6* @param [in] desVel Oczekiwana prędkość stawów j1~j6, jednostka: deg/s
7* @param [in] torque_ff Moment wyprzedzający j1~j6, jednostka: Nm
8* @param [in] interval Okres instrukcji, jednostka s, zakres [0.001~0.008]
9* @param [in] comType Typ wysyłania instrukcji; 0-xmlrpc; 1-UDP (odpowiadający portowi 20007 robota)
10* @return Kod błędu
11*/
12errno_t ServoMIT(double posGain[6], double desPos[6], double velGain[6], double desVel[6], double torque_ff[6], double interval, int comType = 0);
4.82. Przykład kodu sterowania MIT stawów robota
1int ServoMITtest()
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.SetCmdRpyCallback(UDPFrameCallBack);
9 printf("SetCmdRpyCallback rtn is %d\n", rtn);
10 rtn = robot.RPC("192.168.58.2");
11 if (rtn != 0)
12 {
13 return -1;
14 }
15 while (true)
16 {
17 robot.ResetAllError();
18 robot.Sleep(500);
19 double posGain[6] = { 0.0 };
20 double desPos[6] = { 0.0 };
21 double velGain[6] = { 0.0 };
22 double desVel[6] = { 0.0 };
23 double torques[6] = { 0.0 };
24 float curTorque[6] = { 0.0 };
25 robot.GetJointTorques(1, curTorque);
26 for (int i = 0; i < 6; i++)
27 {
28 torques[i] = curTorque[i];
29 }
30 robot.ServoMITStart(0);
31 ROBOT_STATE_PKG pkg = {};
32 robot.DragTeachSwitch(1);
33 double intev = 0.008;
34 double jPowerLimit[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
35 double jVelLimit[6] = { 50, 50, 50, 50, 50, 50 };
36 int error = 0;
37 while (true)
38 {
39 torques[5] = 0.02;
40 error = robot.ServoMIT(posGain, desPos, velGain, desVel, torques, intev, 0);
41 robot.Sleep(1);
42 robot.GetRobotRealTimeState(&pkg);
43 printf("pkg.jt_cur_pos[5]: %f\n", pkg.jt_cur_pos[5]);
44 if (pkg.jt_cur_pos[5] > 30)
45 {
46 break;
47 }
48 }
49 while (true)
50 {
51 torques[5] = -0.02;
52 error = robot.ServoMIT(posGain, desPos, velGain, desVel, torques, intev, 0);
53 robot.Sleep(1);
54 robot.GetRobotRealTimeState(&pkg);
55 printf("pkg.jt_cur_pos[5]:%f\n", pkg.jt_cur_pos[5]);
56 if (pkg.jt_cur_pos[5] < 0)
57 {
58 break;
59 }
60 }
61 robot.DragTeachSwitch(0);
62 error = robot.ServoMITEnd(0);
63 }
64 robot.CloseRPC();
65 robot.Sleep(1000000);
66 return 0;
67}