12. Sterowanie siłą robota

12.1. Konfiguracja czujnika siły

1/**
2* @brief  Konfiguruje czujnik siły
3* @param  [in] company  Producent czujnika siły, 17-Kunwei Technology
4* @param  [in] device  Numer urządzenia, tymczasowo nieużywane, domyślnie 0
5* @param  [in] softvesion  Numer wersji oprogramowania, tymczasowo nieużywane, domyślnie 0
6* @param  [in] bus Pozycja magistrali, na której zawieszono urządzenie, tymczasowo nieużywane, domyślnie 0
7* @return  Kod błędu
8*/
9int FT_SetConfig(int company, int device, int softvesion, int bus);

12.2. Pobranie konfiguracji czujnika siły

1/**
2* @brief Pobiera konfigurację czujnika siły
3* @param [out] deviceID Numer czujnika siły
4* @param [out] company Producent czujnika siły, 17-Kunwei Technology, 19-Chińskie Akademia Technologii Kosmicznych, 20-ATI sensor, 21-Zhongke Midian, 22-Weihang Minxin
5* @param [out] device  Numer urządzenia, Kunwei(0-KWR75B), Chińska Akademia Technologii Kosmicznych(0-MCS6A-200-4), ATI(0-AXIA80-M8), Zhongke Midian(0-MST2010), Weihang Minxin(0-WHC6L-YB-10A)
6* @param [out] softvesion Numer wersji oprogramowania, tymczasowo nieużywane, domyślnie 0
7* @return Kod błędu
8*/
9int FT_GetConfig(ref int deviceID, ref int company, ref int device, ref int softvesion);

12.3. Aktywacja czujnika siły

1/**
2* @brief  Aktywacja czujnika siły
3* @param  [in] act  0-reset, 1-aktywacja
4* @return  Kod błędu
5*/
6int FT_Activate(byte act);

12.4. Zerowanie czujnika siły

1/**
2* @brief  Zerowanie czujnika siły
3* @param  [in] act  0-usunięcie zera, 1-korekta zera
4* @return  Kod błędu
5*/
6int FT_SetZero(byte act);

12.5. Ustawienie odniesienia układu współrzędnych czujnika siły

1/**
2* @brief  Ustawia odniesienie układu współrzędnych czujnika siły
3* @param  [in] ref  0-układ współrzędnych narzędzia, 1-układ współrzędnych podstawy
4* @return  Kod błędu
5*/
6int FT_SetRCS(byte type);

12.6. Ustawienie masy ładunku pod czujnikiem siły

1/**
2* @brief  Ustawia masę ładunku pod czujnikiem siły
3* @param  [in] weight Masa ładunku, kg
4* @return  Kod błędu
5*/
6int SetForceSensorPayLoad(double weight);

12.7. Ustawienie środka ciężkości ładunku pod czujnikiem siły

1/**
2* @brief  Ustawia środek ciężkości ładunku pod czujnikiem siły
3* @param  [in] x Środek ciężkości ładunku x, mm
4* @param  [in] y Środek ciężkości ładunku y, mm
5* @param  [in] z Środek ciężkości ładunku z, mm
6* @return  Kod błędu
7*/
8int SetForceSensorPayLoadCog(double x, double y, double z);

12.8. Pobranie masy ładunku pod czujnikiem siły

1/**
2* @brief  Pobiera masę ładunku pod czujnikiem siły
3* @param  [in] weight Masa ładunku, kg
4* @return  Kod błędu
5*/
6int GetForceSensorPayLoad(ref double weight);

12.9. Pobranie środka ciężkości ładunku pod czujnikiem siły

1/**
2* @brief  Pobiera środek ciężkości ładunku pod czujnikiem siły
3* @param  [out] x Środek ciężkości ładunku x, mm
4* @param  [out] y Środek ciężkości ładunku y, mm
5* @param  [out] z Środek ciężkości ładunku z, mm
6* @return  Kod błędu
7*/
8int GetForceSensorPayLoadCog(ref double x, ref double y, ref double z);

12.10. Automatyczne zerowanie czujnika siły

1/**
2* @brief  Automatyczne zerowanie czujnika siły
3* @param  [out] weight Masa czujnika, kg
4* @param  [out] pos Środek ciężkości czujnika, mm
5* @return  Kod błędu
6*/
7int ForceSensorAutoComputeLoad(ref double weight, ref DescTran pos);

12.11. Pobranie danych siły/momentu w odniesieniu do układu współrzędnych

1/**
2* @brief  Pobiera dane siły/momentu w odniesieniu do układu współrzędnych
3* @param  [out] ft  Siła/moment, fx, fy, fz, tx, ty, tz
4* @return  Kod błędu
5*/
6int FT_GetForceTorqueRCS(byte flag, ref ForceTorque ft);

12.12. Pobranie surowych danych siły/momentu czujnika siły

1/**
2* @brief  Pobiera surowe dane siły/momentu czujnika siły
3* @param  [out] ft  Siła/moment, fx, fy, fz, tx, ty, tz
4* @return  Kod błędu
5*/
6int FT_GetForceTorqueOrigin(byte flag, ref ForceTorque ft);

12.13. Przykład kodu konfiguracji czujnika siły i automatycznego zerowania

 1private void button54_Click(object sender, EventArgs e)
 2{
 3    int company = 24;
 4    int device = 0;
 5    int softversion = 0;
 6    int bus = 1;
 7    int index = 1;
 8
 9    robot.FT_SetConfig(company, device, softversion, bus);
10    Thread.Sleep(1000);
11    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
12    Console.WriteLine($"FT config:{company},{device},{softversion},{bus}");
13    Thread.Sleep(1000);
14
15    robot.FT_Activate(0);
16    Thread.Sleep(1000);
17    robot.FT_Activate(1);
18    Thread.Sleep(1000);
19
20    Thread.Sleep(1000);
21    robot.FT_SetZero(0);
22    Thread.Sleep(1000);
23
24    ForceTorque ft = new ForceTorque(0, 0, 0, 0, 0, 0);
25    robot.FT_GetForceTorqueOrigin(0, ref ft);
26    Console.WriteLine($"ft origin:{ft.fx},{ft.fy},{ft.fz},{ft.tx},{ft.ty},{ft.tz}");
27    robot.FT_SetZero(1);
28    Thread.Sleep(1000);
29
30    DescPose ftCoord = new DescPose(0, 0, 0, 0, 0, 0);
31    robot.FT_SetRCS(0, ftCoord);
32
33    robot.SetForceSensorPayLoad(0.824);
34    robot.SetForceSensorPayLoadCog(0.778, 2.554, 48.765);
35    double weight = 0;
36    double x = 0, y = 0, z = 0;
37    robot.GetForceSensorPayLoad(ref weight);
38    robot.GetForceSensorPayLoadCog(ref x, ref y, ref z);
39    Console.WriteLine($"the FT load is {weight}, {x} {y} {z}");
40
41    robot.SetForceSensorPayLoad(0);
42    robot.SetForceSensorPayLoadCog(0, 0, 0);
43
44    double computeWeight = 0;
45    DescTran tran = new DescTran(0, 0, 0);
46    robot.ForceSensorAutoComputeLoad(ref weight, ref tran);
47    Console.WriteLine($"the result is weight {weight} pos is {tran.x} {tran.y} {tran.z}");
48
49}

12.14. Rejestracja identyfikacji masy ładunku

1/**
2* @brief  Rejestracja identyfikacji masy ładunku
3* @param  [in] id  Numer układu współrzędnych czujnika, zakres [1~14]
4* @return  Kod błędu
5*/
6int FT_PdIdenRecord(int id);

12.15. Obliczenie identyfikacji masy ładunku

1/**
2* @brief  Obliczenie identyfikacji masy ładunku
3* @param  [out] weight  Masa ładunku, jednostka kg
4* @return  Kod błędu
5*/
6int FT_PdIdenCompute(ref double weight);

12.16. Rejestracja identyfikacji środka ciężkości ładunku

1/**
2* @brief  Rejestracja identyfikacji środka ciężkości ładunku
3* @param  [in] id  Numer układu współrzędnych czujnika, zakres [1~14]
4* @param  [in] index Numer punktu, zakres [1~3]
5* @return  Kod błędu
6*/
7int FT_PdCogIdenRecord(int id, int index);

12.17. Obliczenie identyfikacji środka ciężkości ładunku

1/**
2* @brief  Obliczenie identyfikacji środka ciężkości ładunku
3* @param  [out] cog  Środek ciężkości ładunku, jednostka mm
4* @return  Kod błędu
5*/
6int FT_PdCogIdenCompute(ref DescTran cog);

12.18. Przykład kodu identyfikacji obciążenia czujnika siły

 1private void btnFTPdCog_Click(object sender, EventArgs e)
 2{
 3    int company = 24, device = 0, softversion = 0, bus = 1;
 4
 5    robot.FT_SetConfig(company, device, softversion, bus);
 6    Thread.Sleep(1000);
 7    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
 8    Console.WriteLine($"FT config: {company}, {device}, {softversion}, {bus}");
 9    Thread.Sleep(1000);
10
11    robot.FT_Activate(0);
12    Thread.Sleep(1000);
13    robot.FT_Activate(1);
14    Thread.Sleep(1000);
15
16    Thread.Sleep(1000);
17    robot.FT_SetZero(0);
18    Thread.Sleep(1000);
19
20    ForceTorque ft = new ForceTorque(0,0,0,0,0,0);
21    robot.FT_GetForceTorqueOrigin(0, ref ft);
22    Console.WriteLine($"ft origin: {ft.fx}, {ft.fy}, {ft.fz}, {ft.tx}, {ft.ty}, {ft.tz}");
23    robot.FT_SetZero(1);
24    Thread.Sleep(1000);
25
26    DescPose tcoord = new DescPose(0, 0, 35.0, 0, 0, 0);
27    robot.SetToolCoord(10, tcoord, 1, 0, 0, 0);
28
29    robot.FT_PdIdenRecord(10);
30    Thread.Sleep(1000);
31
32    double weight = 0.0f;
33    robot.FT_PdIdenCompute(ref weight);
34    Console.WriteLine($"payload weight: {weight}");
35
36    DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
37    DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
38    DescPose desc_p3 = new DescPose(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
39
40    robot.MoveCart( desc_p1, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
41    Thread.Sleep(1000);
42    robot.FT_PdCogIdenRecord(10, 1);
43    robot.MoveCart( desc_p2, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
44    Thread.Sleep(1000);
45    robot.FT_PdCogIdenRecord(10, 2);
46    robot.MoveCart( desc_p3, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
47    Thread.Sleep(1000);
48    robot.FT_PdCogIdenRecord(10, 3);
49
50    DescTran cog = new DescTran(0,0,0);
51    robot.FT_PdCogIdenCompute(ref cog);
52    Console.WriteLine($"cog: {cog.x}, {cog.y}, {cog.z}");
53}

12.19. Ochrona przed kolizją

 1/**
 2* @brief  Ochrona przed kolizją
 3* @param  [in] flag 0-wyłącz ochronę przed kolizją, 1-włącz ochronę przed kolizją
 4* @param  [in] sensor_id Numer czujnika siły
 5* @param  [in] select  Wybór, czy sześć stopni swobody ma wykrywać kolizję, 0-nie wykrywa, 1-wykrywa
 6* @param  [in] ft  Siła/moment kolizji, fx, fy, fz, tx, ty, tz
 7* @param  [in] max_threshold Maksymalny próg
 8* @param  [in] min_threshold Minimalny próg
 9* @note   Zakres wykrywania siły/momentu: (ft - min_threshold, ft + max_threshold)
10* @return  Kod błędu
11*/
12int FT_Guard(int flag, int sensor_id, int[] select, ForceTorque ft, double[] max_threshold, double[] min_threshold);

12.20. Przykład kodu ochrony przed kolizją

 1private void btnFTGuard_Click(object sender, EventArgs e)
 2{
 3    int company = 24, device = 0, softversion = 0, bus = 1;
 4
 5    robot.FT_SetConfig(company, device, softversion, bus);
 6    Thread.Sleep(1000);
 7    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
 8    Console.WriteLine($"FT config: {company}, {device}, {softversion}, {bus}");
 9    Thread.Sleep(1000);
10
11    robot.FT_Activate(0);
12    Thread.Sleep(1000);
13    robot.FT_Activate(1);
14    Thread.Sleep(1000);
15
16    Thread.Sleep(1000);
17    robot.FT_SetZero(0);
18    Thread.Sleep(1000);
19
20    byte sensor_id = 1;
21    int[] select = { 1, 1, 1, 1, 1, 1 };
22    double[] max_threshold = { 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f };
23    double[] min_threshold = { 5.0f, 5.0f, 5.0f, 5.0f, 5.0f, 5.0f };
24
25    ForceTorque ft = new ForceTorque();
26    DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
27    DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
28    DescPose desc_p3 = new DescPose(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
29
30    robot.FT_Guard(1, sensor_id, select,  ft, max_threshold, min_threshold);
31    robot.MoveCart( desc_p1, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
32    robot.MoveCart( desc_p2, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
33    robot.MoveCart( desc_p3, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
34
35    robot.FT_Guard(0, sensor_id, select, ft, max_threshold, min_threshold);
36}

12.21. Sterowanie stałą siłą

Nowe w wersji C#SDK-V1.1.9: Web-3.8.7

 1/**
 2* @brief  Sterowanie stałą siłą
 3* @param  [in] flag 0-wyłącz sterowanie stałą siłą, 1-włącz sterowanie stałą siłą
 4* @param  [in] sensor_id Numer czujnika siły
 5* @param  [in] select  Wybór, czy sześć stopni swobody ma wykrywać kolizję, 0-nie wykrywa, 1-wykrywa
 6* @param  [in] ft  Siła/moment kolizji, fx, fy, fz, tx, ty, tz
 7* @param  [in] ft_pid Parametry pid siły, parametry pid momentu
 8* @param  [in] adj_sign Sterowanie uruchamianiem/zatrzymywaniem adaptacji, 0-wyłączone, 1-włączone
 9* @param  [in] ILC_sign Sterowanie uruchamianiem/zatrzymywaniem ILC, 0-zatrzymaj, 1-trening, 2-praktyka
10* @param  [in] max_dis Maksymalna odległość regulacji, jednostka mm
11* @param  [in] max_ang Maksymalny kąt regulacji, jednostka deg
12* @param  [in] M Parametry masy rx, ry [0.1-10], domyślnie 2
13* @param  [in] B Parametry tłumienia rx, ry [0.1-50], domyślnie 8
14* @param  [in] threshold Próg uruchomienia rx, ry [0-10], domyślnie 0.2
15* @param  [in] adjustCoeff Współczynnik regulacji momentu rx, ry [0-1], domyślnie 1
16* @param  [in] polishRadio Promień szlifowania, jednostka mm
17* @param  [in] filter_Sign Znacznik włączenia filtrowania 0-wył.; 1-wł., domyślnie wyłączone
18* @param  [in] posAdapt_sign Znacznik włączenia zgodności orientacji 0-wył.; 1-wł., domyślnie wyłączone
19* @param  [in] isNoBlock Znacznik blokowania, 0-blokujące; 1-nieblokujące
20* @return  Kod błędu
21*/
22public int FT_Control(byte flag, int sensor_id, byte[] select, ForceTorque ft, float[] ft_pid,byte adj_sign, byte ILC_sign, float max_dis, float max_ang,double[] M, double[] B, double[] threshold, double[] adjustCoeff,double polishRadio, int filter_Sign, int posAdapt_sign, int isNoBlock)

12.22. Przykład kodu sterowania stałą siłą z tłumieniem

Nowe w wersji C#SDK-V1.1.9: Web-3.8.7

 1public void TestFTControlWithAdjustCoeff()
 2{
 3    int rtn;
 4    int sensor_id = 10;
 5    byte[] select = new byte[6] { 0, 0, 1, 0, 0, 0 };
 6    float[] ft_pid = new float[6] { 0.0008f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
 7    byte adj_sign = 0;
 8    byte ILC_sign = 0;
 9    float max_dis = 1000.0f;
10    float max_ang = 20.0f;
11    ForceTorque ft = new ForceTorque();
12    ft.fz = -10.0f;
13    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
14    JointPos j1 = new JointPos(80.765f, -98.795f, 106.548f, -97.734f, -89.999f, 94.842f);
15    JointPos j2 = new JointPos(43.067f, -84.429f, 92.620f, -98.175f, -90.011f, 57.144f);
16    DescPose desc_p1 = new DescPose(5.009f, -547.463f, 262.053f, -179.999f, -0.019f, 75.923f);
17    DescPose desc_p2 = new DescPose(-347.966f, -547.463f, 262.048f, -180.000f, -0.019f, 75.923f);
18    DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
19    double[] M = new double[2] { 2.0, 2.0 };
20    double[] B = new double[2] { 15.0, 15.0 };
21    double[] threshold = new double[2] { 1.0, 1.0 };
22    double[] adjustCoeff = new double[2] { 1.0, 0.8 };
23    double polishRadio = 0.0;
24    int filter_Sign = 0;
25    int posAdapt_sign = 1;
26    int isNoBlock = 0;
27    while (true)
28    {
29        rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
30        Console.WriteLine($"FT_Control start rtn is {rtn}");
31        robot.MoveL(j1, desc_p1, 1, 0, 100, 100, 100, -1, 0, epos, 0, 0, offset_pos, 0, 0, 10);
32        robot.MoveL(j2, desc_p2, 1, 0, 100, 100, 100, -1, 0, epos, 0, 0, offset_pos, 0, 0, 10);
33        rtn = robot.FT_Control(0, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
34        Console.WriteLine($"FT_Control end rtn is {rtn}");
35    }
36}

12.23. Wstawianie obrotowe

 1/**
 2* @brief Wstawianie obrotowe
 3* @param [in] rcs Układ odniesienia, 0-układ współrzędnych narzędzia, 1-układ współrzędnych podstawy
 4* @param [in] angVelRot Prędkość kątowa obrotu, jednostka deg/s
 5* @param [in] ft Próg siły/momentu, fx, fy, fz, tx, ty, tz, zakres [0~100]
 6* @param [in] max_angle Maksymalny kąt obrotu, jednostka deg
 7* @param [in] orn Kierunek siły/momentu, 1-wzdłuż osi Z, 2-wokół osi Z
 8* @param [in] max_angAcc Maksymalne przyspieszenie kątowe, jednostka deg/s², tymczasowo nieużywane, domyślnie 0
 9* @param [in] rotorn Kierunek obrotu, 1-zgodnie z ruchem wskazówek zegara, 2-przeciwnie do ruchu wskazówek zegara
10* @param [in] strategy Strategia postępowania przy niewykryciu siły/momentu, 0-zgłoś błąd; 1-ostrzeżenie, kontynuuj ruch
11* @return Kod błędu
12*/
13public int FT_RotInsertion(int rcs, double angVelRot, double ft, double max_angle, int orn, double max_angAcc, int rotorn, int strategy)

12.24. Wstawianie Liniowe

 1/**
 2* @brief  Wstawianie liniowe
 3* @param  [in] rcs Referencyjny układ współrzędnych, 0-układ narzędzia, 1-układ podstawy
 4* @param  [in] ft  Próg siły/momentu obrotowego, fx,fy,fz,tx,ty,tz, zakres [0~100]
 5* @param  [in] lin_v Prędkość liniowa, jednostka mm/s
 6* @param  [in] lin_a Przyspieszenie liniowe, jednostka mm/s^2, tymczasowo nieużywane
 7* @param  [in] max_dis Maksymalna odległość wstawiania, jednostka mm
 8* @param  [in] linorn  Kierunek wstawiania, 0-kierunek ujemny, 1-kierunek dodatni
 9* @return  Kod błędu
10*/
11public int FT_LinInsertion(int rcs, float ft, float lin_v, float lin_a, float max_dis, byte linorn)

12.25. Przykład Kodu Wstawiania Rotacyjnego z Czujnikiem Siły

 1public void TestRotInsert()
 2{
 3    ROBOT_STATE_PKG pkg = new ROBOT_STATE_PKG();
 4    int rtn;
 5
 6    float forceInsertion = 5.0f; // Próg siły lub momentu (0~100), jednostka N lub Nm
 7    int angleMax = 300; // Maksymalny kąt obrotu, jednostka °
 8    byte orn = 1; // Kierunek siły, 1-fz, 2-mz
 9    float angAccmax = 0; // Maksymalne przyspieszenie kątowe obrotu, jednostka °/s^2, tymczasowo nieużywane
10    byte status = 1;  // Flaga włączenia sterowania stałą siłą, 0-wył., 1-wł.
11    int sensor_num = 11; // Numer czujnika siły
12    float[] gain = { 0.0001f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };  // Próg maksymalny
13    byte adj_sign = 0;  // Status start/stop adaptacyjny, 0-wył., 1-wł.
14    byte ILC_sign = 0;  // Status start/stop sterowania ILC, 0-stop, 1-trening, 2-operacyjny
15    float max_dis = 1000.0f;  // Maksymalna odległość regulacji
16    float max_ang = 20.0f;  // Maksymalny kąt regulacji
17    ForceTorque ft = new ForceTorque();
18    int rcs = 0;  // Referencyjny układ współrzędnych, 0-układ narzędzia, 1-układ podstawy
19    float angVelRot = 1.0f;  // Prędkość kątowa obrotu, jednostka °/s
20    byte rotorn = 1; // Kierunek obrotu, 1-zgodnie z ruchem wskazówek zegara, 2-przeciwnie
21    JointPos j1 = new JointPos(100.968, -108.678, 126.166, -106.630, -93.253, 19.584);
22    DescPose desc_p1 = new DescPose(159.473, -316.570, 334.560, -179.718, -3.352, 171.400);
23    ExaxisPos epos = new ExaxisPos(0.0f, 0.0f, 0.0f, 0.0f);
24    DescPose offset_pos = new DescPose(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
25
26    robot.MoveL(j1, desc_p1, 2, 0, 100.0f, 180.0f, 100.0f, -1.0f, 0, epos, (byte)0, (byte)1, offset_pos);
27
28    byte[] select3 = { 0, 0, 1, 0, 0, 0 };
29    ft.fz = -5.0f;
30    gain[0] = 0.0001f;
31    status = 1;
32    robot.FT_Control(status, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
33    rtn = robot.FT_LinInsertion(rcs, 10, 1, 1, 100, 1);
34    Console.WriteLine("FT_LinInsertion rtn is " + rtn);
35    robot.FT_Control(0, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
36
37    ft.fz = -30.0f;
38    robot.FT_Control(1, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
39    rtn = robot.FT_RotInsertion(rcs, angVelRot, forceInsertion, angleMax, orn, angAccmax, rotorn, 0);
40    Console.WriteLine("FT_RotInsertion rtn is " + rtn);
41    robot.FT_Control(0, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
42
43    rtn = robot.FT_LinInsertion(0, 40, 3, 0, 100, 1);
44    Console.WriteLine("FT_LinInsertion retract rtn is " + rtn);
45
46    Thread.Sleep(1000);
47    robot.GetRobotRealTimeState(ref pkg);
48    Console.WriteLine("robot errcode " + pkg.main_code + "  " + pkg.sub_code);
49}

12.26. Przykład kodu wstawiania obrotowego czujnika siły robota

 1public void TestMove()
 2{
 3    int rtn;
 4    JointPos j1 = new JointPos(-11.904f, -99.669f, 117.473f, -108.616f, -91.726f, 74.256f);
 5    JointPos j2 = new JointPos(-45.615f, -106.172f, 124.296f, -107.151f, -91.282f, 74.255f);
 6    JointPos j3 = new JointPos(-29.777f, -84.536f, 109.275f, -114.075f, -86.655f, 74.257f);
 7    JointPos j4 = new JointPos(-31.154f, -95.317f, 94.276f, -88.079f, -89.740f, 74.256f);
 8    DescPose desc_pos1 = new DescPose(-419.524f, -13.000f, 351.569f, -178.118f, 0.314f, 3.833f);
 9    DescPose desc_pos2 = new DescPose(-321.222f, 185.189f, 335.520f, -179.030f, -1.284f, -29.869f);
10    DescPose desc_pos3 = new DescPose(-487.434f, 154.362f, 308.576f, 176.600f, 0.268f, -14.061f);
11    DescPose desc_pos4 = new DescPose(-443.165f, 147.881f, 480.951f, 179.511f, -0.775f, -15.409f);
12    DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
13    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
14    int tool = 0;
15    int user = 0;
16    float vel = 100.0f;
17    float acc = 100.0f;
18    float ovl = 100.0f;
19    float oacc = 100.0f;
20    float blendT = 0.0f;
21    float blendR = 0.0f;
22    byte flag = 0;
23    byte search = 0;
24    int blendMode = 0;
25    int velAccMode = 0;
26    robot.SetSpeed(20);
27    rtn = robot.MoveJ(j1, desc_pos1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
28    Console.WriteLine($"movej errcode:{rtn}");
29    rtn = robot.MoveL(j2, desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, oacc, velAccMode,0,10);
30    Console.WriteLine($"movel errcode:{rtn}");
31    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);
32    Console.WriteLine($"movec errcode:{rtn}");
33    rtn = robot.MoveJ(j2, desc_pos2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
34    Console.WriteLine($"movej errcode:{rtn}");
35    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);
36    Console.WriteLine($"circle errcode:{rtn}");
37    rtn = robot.MoveCart(desc_pos4, tool, user, vel, acc, ovl, blendT, -1);
38    Console.WriteLine($"MoveCart errcode:{rtn}");
39    rtn = robot.MoveJ(j1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
40    Console.WriteLine($"movej errcode:{rtn}");
41    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, -1, velAccMode);
42    Console.WriteLine($"movel errcode:{rtn}");
43    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);
44    Console.WriteLine($"movec errcode:{rtn}");
45    rtn = robot.MoveJ(j2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
46    Console.WriteLine($"movej errcode:{rtn}");
47    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);
48    Console.WriteLine($"circle errcode:{rtn}");
49}

12.27. Rozpoczęcie sterowania podatnego

1/**
2* @brief  Rozpoczęcie sterowania podatnego
3* @param  [in] p Współczynnik regulacji pozycji lub współczynnik podatności
4* @param  [in] force Próg siły włączenia podatności, jednostka N
5* @return  Kod błędu
6*/
7int FT_ComplianceStart(float p, float force);

12.28. Zatrzymanie sterowania podatnego

1/**
2* @brief  Zatrzymanie sterowania podatnego
3* @return  Kod błędu
4*/
5int FT_ComplianceStop();

12.29. Przykład kodu sterowania podatnego

 1private void btnComplience_Click(object sender, EventArgs e)
 2{
 3    int company = 24, device = 0, softversion = 0, bus = 1;
 4    robot.FT_SetConfig(company, device, softversion, bus);
 5    Thread.Sleep(1000);
 6    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
 7    Console.WriteLine($"FT config: {company}, {device}, {softversion}, {bus}");
 8    Thread.Sleep(1000);
 9
10    robot.FT_Activate(0);
11    Thread.Sleep(1000);
12    robot.FT_Activate(1);
13    Thread.Sleep(1000);
14
15    robot.FT_SetZero(0);
16    Thread.Sleep(1000);
17
18    byte flag = 1;
19    int sensor_id = 1;
20    int[] select = { 1, 1, 1, 0, 0, 0 };
21    double[] ft_pid = { 0.0005f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
22    byte adj_sign = 0, ILC_sign = 0;
23    float max_dis = 100.0f, max_ang = 0.0f;
24
25    ForceTorque ft = new ForceTorque { fx = -10.0, fy = -10.0, fz = -10.0 };
26    DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
27    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
28
29    JointPos j1 = new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
30    JointPos j2 = new JointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
31    DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
32    DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
33
34    robot.FT_Control(flag, (byte)sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
35    float p = 0.00005f;
36    float force = 30.0f;
37    int rtn = robot.FT_ComplianceStart(p, force);
38    Console.WriteLine($"FT_ComplianceStart rtn is {rtn}");
39
40    int count = 5;
41    while (count-- > 0)
42    {
43    robot.MoveL(j1, desc_p1, 0, 0, 100.0f, 180.0f, 100.0f, -1.0f, epos, 0, 1, offset_pos);
44    robot.MoveL(j2, desc_p2, 0, 0, 100.0f, 180.0f, 100.0f, -1.0f, epos, 0, 0, offset_pos);
45    }
46
47    robot.FT_ComplianceStop();
48    Console.WriteLine($"FT_ComplianceStop rtn is {rtn}");
49
50    flag = 0;
51    robot.FT_Control(flag, (byte)sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
52}

12.30. Inicjalizacja filtra dynamicznego identyfikacji obciążenia

Nowe w wersji C#SDK-v1.0.4.

1/**
2* @brief Inicjalizacja filtra dynamicznego identyfikacji obciążenia
3* @return Kod błędu
4*/
5int LoadIdentifyDynFilterInit();

12.31. Inicjalizacja zmiennych dynamicznych identyfikacji obciążenia

Nowe w wersji C#SDK-v1.0.4.

1/**
2* @brief Inicjalizacja zmiennych dynamicznych identyfikacji obciążenia
3* @return Kod błędu
4*/
5int LoadIdentifyDynVarInit();

12.32. Główny program identyfikacji obciążenia

Nowe w wersji C#SDK-v1.0.4.

1/**
2* @brief Główny program identyfikacji obciążenia
3* @param [in] joint_torque Moment obrotowy stawu
4* @param [in] joint_pos Pozycja stawu
5* @param [in] t Okres próbkowania
6* @return Kod błędu
7*/
8int LoadIdentifyMain(double[] joint_torque, double[] joint_pos, double t);

12.33. Pobranie wyniku identyfikacji obciążenia

Nowe w wersji C#SDK-v1.0.4.

1/**
2* @brief Pobiera wynik identyfikacji obciążenia
3* @param [in] gain  Współczynnik członu grawitacyjnego double[6], współczynnik członu odśrodkowego double[6]
4* @param [out] weight Masa ładunku
5* @param [out] cog Środek ciężkości ładunku
6* @return Kod błędu
7*/
8int LoadIdentifyGetResult(double[] gain, ref double weight, ref DescTran cog);

12.34. Przykład kodu identyfikacji obciążenia robota

 1private void button74_Click(object sender, EventArgs e)
 2{
 3    int rtn;
 4    int retval = 0;
 5
 6    retval = robot.LoadIdentifyDynFilterInit();
 7    Console.WriteLine("LoadIdentifyDynFilterInit retval is: " + retval);
 8
 9    retval = robot.LoadIdentifyDynVarInit();
10    Console.WriteLine("LoadIdentifyDynVarInit retval is: " + retval);
11
12    JointPos posJ = new JointPos(0,0,0,0,0,0);
13    DescPose posDec = new DescPose(0, 0, 0, 0, 0, 0);
14    double[] joint_toq = new double[6] { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
15    robot.GetActualJointPosDegree(0, ref posJ);
16    posJ.jPos[1] = posJ.jPos[1] + 10;
17    robot.GetJointTorques(0, joint_toq);
18    joint_toq[1] = joint_toq[1] + 2;
19
20    double[] tmpTorque = new double[6] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21    for (int i = 0; i < 6; i++)
22    {
23        tmpTorque[i] = joint_toq[i];
24    }
25
26    retval = robot.LoadIdentifyMain(tmpTorque, posJ.jPos, 1);
27    Console.WriteLine("LoadIdentifyMain retval is: " + retval);
28
29    double[] gain = new double[12] { 0, 0.05, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 0 };
30    double weight = 0;
31    DescTran load_pos = new DescTran(0, 0, 0);
32    retval = robot.LoadIdentifyGetResult(gain, ref weight, ref load_pos);
33    Console.WriteLine("LoadIdentifyGetResult retval is: {0}; weight is {1} cog is {2} {3} {4}", retval, weight, load_pos.x, load_pos.y, load_pos.z);
34}

12.35. Przeciąganie wspomagane czujnikiem siły

Nowe w wersji C#SDK-V1.1.4: Web-3.8.3

 1/**
 2* @brief  Przeciąganie wspomagane czujnikiem siły
 3* @param  [in] status Stan sterowania, 0-wyłączone; 1-włączone
 4* @param  [in] asaptiveFlag Znacznik włączenia adaptacji, 0-wyłączony; 1-włączony
 5* @param  [in] interfereDragFlag Znacznik przeciągania w strefie interferencji, 0-wyłączony; 1-włączony
 6* @param  [in] ingularityConstraintsFlag Strategia punktu osobliwego, 0-unikanie; 1-przechodzenie
 7* @param  [in] forceCollisionFlag Znacznik wykrywania kolizji robota podczas przeciągania wspomaganego; 0-wyłączony; 1-włączony
 8* @param  [in] M Współczynnik bezwładności
 9* @param  [in] B Współczynnik tłumienia
10* @param  [in] K Współczynnik sztywności
11* @param  [in] F Sześcioosiowy próg siły przeciągania
12* @param  [in] Fmax Maksymalne ograniczenie siły przeciągania Nm
13* @param  [in] Vmax Maksymalne ograniczenie prędkości stawu °/s
14* @return  Kod błędu
15*/
16int EndForceDragControl(int status, int asaptiveFlag, int interfereDragFlag,int ingularityConstraintsFlag,int forceCollisionFlag, double[] M, double[] B, double[] K, double[] F, double Fmax, double Vmax);

12.36. Pobranie stanu przełącznika przeciągania czujnika siły

1/**
2* @brief  Pobiera stan przełącznika przeciągania czujnika siły
3* @param  [out] dragState Stan sterowania przeciąganiem wspomaganym czujnikiem siły, 0-wyłączone; 1-włączone
4* @param  [out] sixDimensionalDragState Stan sterowania przeciąganiem wspomaganym sześcioosiową siłą, 0-wyłączone; 1-włączone
5* @return  Kod błędu
6*/
7int GetForceAndTorqueDragState(ref int dragState, ref int sixDimensionalDragState);

12.37. Automatyczne włączanie czujnika siły po wyczyszczeniu błędu

1/**
2* @brief  Automatyczne włączanie czujnika siły po wyczyszczeniu błędu
3* @param  [in] status Stan sterowania, 0-wyłączone; 1-włączone
4* @return  Kod błędu
5*/
6int SetForceSensorDragAutoFlag(int status);

12.38. Przykład kodu przeciągania wspomaganego czujnikiem siły

 1private void button61_Click(object sender, EventArgs e)
 2{
 3    robot.SetForceSensorDragAutoFlag(1);
 4    double[] M = { 15.0, 15.0, 15.0, 0.5, 0.5, 0.1 };
 5    double[] B = { 150.0, 150.0, 150.0, 5.0, 5.0, 1.0 };
 6    double[] K = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 7    double[] F = { 10.0, 10.0, 10.0, 1.0, 1.0, 1.0 };
 8
 9    robot.EndForceDragControl(1, 0, 0, 0, M, B, K, F, 50, 100);
10    robot.WaitMs(5000);
11
12    int dragState = 0;
13    int sixDimensionalDragState = 0;
14    robot.GetForceAndTorqueDragState(ref dragState, ref sixDimensionalDragState);
15    Console.WriteLine($"the drag state is {dragState} {sixDimensionalDragState}");
16
17    robot.EndForceDragControl(0, 0, 0, 0, M, B, K, F, 50, 100);
18}

12.39. Ustawienie przełącznika i parametrów mieszanego przeciągania z użyciem 6-osiowej siły i impedancji stawu

 1/**
 2* @brief  Ustawienie przełącznika i parametrów mieszanego przeciągania z użyciem 6-osiowej siły i impedancji stawu
 3* @param  [in] status Stan sterowania, 0-wyłączone; 1-włączone
 4* @param  [in] impedanceFlag Znacznik włączenia impedancji, 0-wyłączony; 1-włączony
 5* @param  [in] lamdeDain Wzmocnienie przeciągania
 6* @param  [in] KGain Wzmocnienie sztywności
 7* @param  [in] BGain Wzmocnienie tłumienia
 8* @param  [in] dragMaxTcpVel Maksymalne ograniczenie prędkości liniowej końcówki podczas przeciągania
 9* @param  [in] dragMaxTcpOriVel Maksymalne ograniczenie prędkości kątowej końcówki podczas przeciągania
10* @return  Kod błędu
11*/
12int ForceAndJointImpedanceStartStop(int status, int impedanceFlag, double[] lamdeDain, double[] KGain, double[] BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);

12.40. Przykład kodu przeciągania wspomaganego czujnikiem siły

 1private void button62_Click(object sender, EventArgs e)
 2{
 3    robot.DragTeachSwitch(1);
 4    double[] lambdaGain = { 3.0, 2.0, 2.0, 2.0, 2.0, 3.0 };
 5    double[] kGain = { 0, 0, 0, 0, 0, 0 };
 6    double[] bGain = { 150, 150, 150, 5.0, 5.0, 1.0 };
 7    int rtn = robot.ForceAndJointImpedanceStartStop(1, 0, lambdaGain, kGain, bGain, 1000, 180);
 8    Console.WriteLine($"ForceAndJointImpedanceStartStop rtn is {rtn}");
 9    Thread.Sleep(5000);
10    robot.DragTeachSwitch(0);
11    rtn = robot.ForceAndJointImpedanceStartStop(0, 0, lambdaGain, kGain, bGain, 1000, 180);
12    Console.WriteLine($"ForceAndJointImpedanceStartStop rtn is {rtn}");
13}

12.41. Sterowanie uruchamianiem/zatrzymywaniem impedancji

Nowe w wersji C#SDK-V1.1.8: Web-3.8.6

 1/**
 2* @brief Sterowanie uruchamianiem/zatrzymywaniem impedancji
 3* @param [in] status 0: wyłączone; 1-włączone
 4* @param [in] workSpace 0-przestrzeń stawów; 1-przestrzeń kartezjańska
 5* @param [in] forceThreshold Próg siły wyzwalającej (N)
 6* @param [in] m Parametr masy
 7* @param [in] b Parametr tłumienia
 8* @param [in] k Parametr sztywności
 9* @param [in] maxV Maksymalna prędkość liniowa (mm/s)
10* @param [in] maxVA Maksymalne przyspieszenie liniowe (mm/s²)
11* @param [in] maxW Maksymalna prędkość kątowa (°/s)
12* @param [in] maxWA Maksymalne przyspieszenie kątowe (°/s²)
13* @return Kod błędu
14*/
15public int ImpedanceControlStartStop(int status, int workSpace, double[] forceThreshold, double[] m, double[] b, double[] k, double maxV, double maxVA, double maxW, double maxWA)

12.42. Przykład kodu sterowania uruchamianiem/zatrzymywaniem impedancji robota

Nowe w wersji C#SDK-V1.1.8: Web-3.8.6

 1public void TestImpedanceControl()
 2{
 3    int[] ctrl = new int[20];
 4    int state;
 5    int pressValue;
 6    int error;
 7    int rtn;
 8    JointPos j1 = new JointPos(102.622, -135.990, 120.769, -73.950, -90.848, 35.507);
 9    JointPos j2 = new JointPos(93.674, -80.062, 82.947, -92.199, -90.967, 26.559);
10    DescPose desc_pos1 = new DescPose(136.552, -149.799, 449.532, 179.817, -1.172, 157.123);
11    DescPose desc_pos2 = new DescPose(136.540, -561.048, 449.542, 179.819, -1.172, 157.122);
12
13    DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
14    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
15
16    int tool = 0;
17    int user = 0;
18    float vel = 100.0f;
19    float acc = 200.0f;
20    float ovl = 100.0f;
21    float blendT = -1.0f;
22    float blendR = -1.0f;
23
24    byte flag = 0;
25
26    byte search = 0;
27    robot.SetSpeed(20);
28    int company = 17;
29    int device = 0;
30    int softversion = 0;
31    int bus = 1;
32    robot.FT_SetConfig(company, device, softversion, bus);
33    Thread.Sleep(1000);
34    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
35    Console.WriteLine($"FT config:{company},{device},{softversion},{bus}");
36    Thread.Sleep(1000);
37
38    robot.FT_Activate(0);
39    Thread.Sleep(1000);
40    robot.FT_Activate(1);
41    Thread.Sleep(1000);
42    Thread.Sleep(1000);
43    robot.FT_SetZero(0);
44    Thread.Sleep(1000);
45    robot.FT_SetZero(1);
46    Thread.Sleep(1000);
47
48    double[] forceThreshold = new double[] { 30, 30, 30, 5, 5, 5 };
49    double[] m = new double[] { 0.1, 0.1, 0.1, 0.02, 0.02, 0.02 };
50    double[] b = new double[] { 1, 1, 1, 0.08, 0.08, 0.08 };
51    double[] k = new double[] { 0, 0, 0, 0, 0, 0 };
52    rtn = robot.ImpedanceControlStartStop(1, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
53    Console.WriteLine($"ImpedanceControlStartStop errcode:{rtn}");
54    rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
55    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
56    rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
57    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
58    rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
59    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
60    Console.WriteLine($"movel errcode:{rtn}");
61    robot.ImpedanceControlStartStop(0, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
62}

12.43. Włączenie funkcji kompensacji momentu obrotowego i współczynnika kompensacji

1/**
2* @brief Włączenie funkcji kompensacji momentu obrotowego i współczynnika kompensacji
3* @param [in] status Przełącznik, 0-wyłączony; 1-włączony
4* @param [in] torqueCoeff Współczynniki kompensacji momentu J1-J6 [0-1]
5* @return Kod błędu
6*/
7public int SetCoderCompenParams(int status, double[] torqueCoeff)

12.44. Pozycjonowanie Powierzchni

 1/**
 2* @brief  Pozycjonowanie powierzchni
 3* @param  [in] rcs Referencyjny układ współrzędnych, 0-układ narzędzia, 1-układ podstawy
 4* @param  [in] dir  Kierunek ruchu, 1-kierunek dodatni, 2-kierunek ujemny
 5* @param  [in] axis Oś ruchu, 1-oś x, 2-oś y, 3-oś z
 6* @param  [in] lin_v Prędkość liniowa poszukiwania, jednostka mm/s
 7* @param  [in] lin_a Przyspieszenie liniowe poszukiwania, jednostka mm/s^2, tymczasowo nieużywane, domyślnie 0
 8* @param  [in] max_dis Maksymalna odległość poszukiwania, jednostka mm
 9* @param  [in] ft  Próg siły/momentu zakończenia ruchu, fx,fy,fz,tx,ty,tz
10* @return  Kod błędu
11*/
12public int FT_FindSurface(int rcs, byte dir, byte axis, float lin_v, float lin_a, float max_dis, float ft)

12.45. Rozpoczęcie Obliczania Pozycji Płaszczyzny Środkowej

1/**
2* @brief  Rozpoczęcie obliczania pozycji płaszczyzny środkowej
3* @return  Kod błędu
4*/
5public int FT_CalCenterStart()

12.46. Zakończenie Obliczania Pozycji Płaszczyzny Środkowej

1/**
2* @brief  Zakończenie obliczania pozycji płaszczyzny środkowej
3* @param  [out] pos Pozycja płaszczyzny środkowej
4* @return  Kod błędu
5*/
6public int FT_CalCenterEnd(ref DescPose pos)

12.47. Przykład Kodu Pozycjonowania Powierzchni

 1private void button59_Click(object sender, EventArgs e)
 2{
 3    int company = 22;
 4    int device = 0;
 5    int softversion = 0;
 6    int bus = 1;
 7
 8    robot.FT_SetConfig(company, device, softversion, bus);
 9    Thread.Sleep(1000);
10    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
11    Console.WriteLine("FT config:" + company + "," + device + "," + softversion + "," + bus);
12    Thread.Sleep(1000);
13
14    robot.FT_Activate(0);
15    Thread.Sleep(1000);
16    robot.FT_Activate(1);
17    Thread.Sleep(1000);
18
19    Thread.Sleep(1000);
20    robot.FT_SetZero(0);
21    Thread.Sleep(1000);
22
23    int rcs = 0;
24    byte dir = 1;
25    byte axis = 1;
26    float lin_v = 15.0f;
27    float lin_a = 0.0f;
28    float maxdis = 500.0f;
29    float ft_goal = 2.0f;
30    DescPose desc_pos = new DescPose(-419.524f, -13.000f, 351.569f, -178.118f, 0.314f, 3.833f);
31    DescPose xcenter = new DescPose(0, 0, 0, 0, 0, 0);
32    DescPose ycenter = new DescPose(0, 0, 0, 0, 0, 0);
33
34    ForceTorque ft = new ForceTorque();
35
36    ft.fx = -2.0f;
37
38    robot.MoveCart(desc_pos, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
39
40    robot.FT_CalCenterStart();
41    robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
42    robot.MoveCart(desc_pos, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
43    robot.WaitMs(1000);
44
45    dir = 2;
46    robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
47    robot.FT_CalCenterEnd(ref xcenter);
48    Console.WriteLine("xcenter:" + xcenter.tran.x + "," + xcenter.tran.y + "," + xcenter.tran.z + "," + xcenter.rpy.rx + "," + xcenter.rpy.ry + "," + xcenter.rpy.rz);
49    robot.MoveCart(xcenter, 1, 0, 60.0f, 50.0f, 50.0f, -1.0f, -1);
50
51    robot.FT_CalCenterStart();
52    dir = 1;
53    axis = 2;
54    lin_v = 6.0f;
55    maxdis = 150.0f;
56    robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
57    robot.MoveCart(desc_pos, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
58    robot.WaitMs(1000);
59
60    dir = 2;
61    robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
62    robot.FT_CalCenterEnd(ref ycenter);
63    Console.WriteLine("ycenter:" + ycenter.tran.x + "," + ycenter.tran.y + "," + ycenter.tran.z + "," + ycenter.rpy.rx + "," + ycenter.rpy.ry + "," + ycenter.rpy.rz);
64    robot.MoveCart(ycenter, 1, 0, 60.0f, 50.0f, 50.0f, 0.0f, -1);
65
66}

12.48. Ustawianie Przesunięcia Splotu w Czasie Rzeczywistym

1/**
2* @brief  Ustawianie przesunięcia splotu w czasie rzeczywistym
3* @param [in] offset Przesunięcie w czasie rzeczywistym [mm, °]
4* @return  Kod błędu
5*/
6public int SetWeaveOffsetRT(DescPose offset)

12.49. Przykład Kodu Prędkości i Przesunięcia Splotu w Czasie Rzeczywistym

 1public void TestWeaveSpeedAndOffset()
 2{
 3    Console.WriteLine("============================================================");
 4    Console.WriteLine("  Weave Speed and Offset Test");
 5    Console.WriteLine("============================================================");
 6
 7    if (robot == null)
 8    {
 9        Console.WriteLine("ERROR: Robot not connected!");
10        return;
11    }
12
13    int rtn;
14    ROBOT_STATE_PKG pkg = new ROBOT_STATE_PKG();
15    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
16    DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
17
18    JointPos j1 = new JointPos(5.027, -84.331, -75.139, -103.690, 86.379, 20.794);
19    DescPose d1 = new DescPose(324.752, -83.339, 366.314, -172.321, -0.936, -106.047);
20
21    JointPos j2 = new JointPos(-35.335, -117.598, -57.174, -95.234, 90.001, -19.560);
22    DescPose d2 = new DescPose(324.999, -355.439, 260.000, 179.995, 0.003, -105.775);
23
24    JointPos j3 = new JointPos(59.787, -117.594, -57.183, -95.222, 90.006, 75.562);
25    DescPose d3 = new DescPose(324.998, 355.441, 260.002, 179.995, 0.003, -105.775);
26
27    // ---- Step 1: MoveJ to start point ----
28    Console.WriteLine("\nStep 1: MoveJ to start point");
29    rtn = robot.MoveJ(j1, d1, 1, 0, 100, 100, 50, epos, -1, 0, offset_pos);
30    Console.WriteLine("  MoveJ(j1) rtn={0}", rtn);
31    Thread.Sleep(500);
32
33    // ---- Step 2: MoveJ to weave entry ----
34    Console.WriteLine("\nStep 2: MoveJ to weave entry point");
35    rtn = robot.MoveJ(j2, d2, 1, 0, 100, 100, 50, epos, -1, 0, offset_pos);
36    Console.WriteLine("  MoveJ(j2) rtn={0}", rtn);
37    Thread.Sleep(500);
38
39    // ---- Step 3: WeaveStart, launch weave MoveL thread ----
40    Console.WriteLine("\nStep 3: WeaveStart + MoveL in background thread");
41    robot.WeaveStart(0);
42
43    bool weaveRunning = true;
44    Thread weaveThread = new Thread(() =>
45    {
46        rtn = robot.MoveL(j3, d3, 1, 0, 100, 100, 5, -1, 0, epos, 0, 0, offset_pos, 5, 0, 0, 10);
47        Console.WriteLine("  MoveL(weave) thread finished, rtn={0}", rtn);
48        weaveRunning = false;
49    });
50    weaveThread.IsBackground = true;
51    weaveThread.Start();
52    Thread.Sleep(500);  // Wait for motion to start
53
54    // ---- Step 4: Speed test (main thread, weave MoveL in background) ----
55    Console.WriteLine("\nStep 4: SetSpeed test during weaving");
56    int[] speedValues = { 20, 50, 80, 30, 60, 10 };
57    foreach (int speed in speedValues)
58    {
59        if (!weaveRunning) break;
60        rtn = robot.SetSpeedInstant(speed);
61        robot.GetRobotRealTimeState(ref pkg);
62        Console.WriteLine("  SetSpeed({0}) -> rtn={1}, TCP_CmpSpeed={2}", speed, rtn, pkg.target_TCP_CmpSpeed);
63        Thread.Sleep(5000);
64    }
65
66
67    Thread.Sleep(5000);
68    // ---- Step 5: SetWeaveOffsetRT offset test (main thread, weave MoveL in background) ----
69    Console.WriteLine("\nStep 5: SetWeaveOffsetRT test (50 iterations, delta=0.1)");
70    double accumOffset = 0.0;
71    for (int i = 0; i < 50 && weaveRunning; i++)
72    {
73        accumOffset += 0.1;
74        DescPose weaveOffset = new DescPose(0, 0, accumOffset, 0, 0, 0);
75        rtn = robot.SetWeaveOffsetRT(weaveOffset);
76        robot.GetRobotRealTimeState(ref pkg);
77        Console.WriteLine("  [{0}/50] SetWeaveOffsetRT(x={1:F1}) -> rtn={2}, TCP_pos=({3:F2},{4:F2},{5:F2})",
78            i + 1, accumOffset, rtn,
79            pkg.tl_cur_pos[0], pkg.tl_cur_pos[1], pkg.tl_cur_pos[2]);
80        Thread.Sleep(100);
81    }
82
83    // ---- Step 6: Wait for weave MoveL, then WeaveEnd ----
84    Console.WriteLine("\nStep 6: Wait for weave MoveL, then WeaveEnd");
85    weaveThread.Join();
86    robot.WeaveEnd(0);
87    Thread.Sleep(500);
88
89    // ---- Step 7: MoveL back to start ----
90    Console.WriteLine("\nStep 7: MoveL back to start");
91    rtn = robot.MoveL(j1, d1, 1, 0, 100, 100, 50, -1, 0, epos, 0, 0, offset_pos, 50, 0, 0, 10);
92    Console.WriteLine("  MoveL(back) rtn={0}", rtn);
93
94    robot.GetRobotRealTimeState(ref pkg);
95    Console.WriteLine("\n  Final robot state: main_code={0}, sub_code={1}", pkg.main_code, pkg.sub_code);
96    Console.WriteLine("============================================================");
97    Console.WriteLine("  Weave Speed and Offset Test Complete");
98    Console.WriteLine("============================================================");
99}