1/**
2* @brief Przykład użycia interfejsu konfiguracji stanu w czasie rzeczywistym CNDE
3*/
4public static void TestRealtimeStateConfig(Robot robot)
5{
6
7 // 1. Utworzenie początkowej listy stanów
8 List<RobotState> stateList1 = new ArrayList<>();
9 stateList1.add(RobotState.ProgramState);
10 stateList1.add(RobotState.RobotState);
11 stateList1.add(RobotState.JointCurPos);
12 stateList1.add(RobotState.ToolCurPos);
13
14 // 2. Pierwsze wywołanie SetRobotRealtimeStateConfig w celu skonfigurowania stanów i okresu
15 int period1 = 100; // okres 100 ms
16 int rtn = robot.SetRobotRealtimeStateConfig(stateList1, period1);
17 System.out.printf("1. SetRobotRealtimeStateConfig (lista początkowa, okres=%d) rtn: %d%n", period1, rtn);
18
19 if (rtn == 0) {
20 // 3. Dodanie dodatkowego stanu
21 rtn = robot.AddRobotRealtimeState(RobotState.RobotTime);
22 System.out.printf("2. AddRobotRealtimeState (RobotTime) rtn: %d%n", rtn);
23
24 // 4. Ponowne wywołanie SetRobotRealtimeStateConfig w celu ponownej konfiguracji (inna lista stanów)
25 List<RobotState> stateList2 = new ArrayList<>();
26 stateList2.add(RobotState.ProgramState);
27 stateList2.add(RobotState.RobotState);
28 stateList2.add(RobotState.MainCode);
29 stateList2.add(RobotState.SubCode);
30 stateList2.add(RobotState.JointCurPos);
31 stateList2.add(RobotState.ToolCurPos);
32 stateList2.add(RobotState.ActualJointTorque);
33
34 int period2 = 50; // okres 50 ms
35 rtn = robot.SetRobotRealtimeStateConfig(stateList2, period2);
36 System.out.printf("3. SetRobotRealtimeStateConfig (zaktualizowana lista, okres=%d) rtn: %d%n", period2, rtn);
37
38 // 5. Modyfikacja okresu
39 int newPeriod = 80; // okres 80 ms
40 rtn = robot.SetRobotRealtimeStatePeriod(newPeriod);
41 System.out.printf("4. SetRobotRealtimeStatePeriod (okres=%d) rtn: %d%n", newPeriod, rtn);
42
43 // 6. Pobranie bieżącej konfiguracji i jej wydrukowanie
44 Robot.StateConfigResult configResult = robot.GetRobotRealtimeStateConfig();
45 System.out.println("5. Wynik GetRobotRealtimeStateConfig:");
46 System.out.printf(" - Okres: %d ms%n", configResult.period);
47 System.out.println(" - Skonfigurowane stany:");
48 for (int i = 0; i < configResult.stateList.size(); i++) {
49 System.out.printf(" [%d] %s%n", i, configResult.stateList.get(i));
50 }
51
52 rtn = robot.RPC("192.168.58.2");
53 if (rtn == 0) {
54 System.out.println("rpc połączenie success");
55 } else {
56 System.out.println("rpc połączenie fail");
57 return;
58 }
59 // Oczekiwanie na nawiązanie połączenia CNDE
60 System.out.println("Oczekiwanie na nawiązanie połączenia CNDE...");
61 while (robot.CNDEGetStateData() == null) {
62 robot.Sleep(100);
63 }
64 System.out.println("Połączenie CNDE nawiązano, rozpoczęcie odbierania danych...");
65
66 // 7. Cykliczne odczytywanie stanu w czasie rzeczywistym w celu weryfikacji, czy konfiguracja jest aktywna
67 System.out.println("6. Odczytywanie stanów w czasie rzeczywistym...");
68 while(true) {
69 robot.Sleep(1000);
70 // Pobieranie danych stanu przez CNDE
71 ROBOT_STATE_PKG pkg = robot.CNDEGetStateData();
72 if (pkg == null) {
73 System.out.println("Dane stanu są puste, połączenie CNDE przerwane, oczekiwanie na ponowne połączenie");
74 continue; // Kontynuuj pętlę w przypadku przerwania połączenia, oczekuj na ponowne połączenie
75 }
76 System.out.println("\n--- Czas robota ---");
77 if (pkg.robotTime != null) {
78 System.out.println("robotTime: " + pkg.robotTime.year + "-" + pkg.robotTime.month + "-" + pkg.robotTime.day +
79 " " + pkg.robotTime.hour + ":" + pkg.robotTime.minute + ":" + pkg.robotTime.second +
80 "." + pkg.robotTime.millisecond);
81 }
82
83 System.out.println(" --- Informacje o stanie ---");
84 System.out.printf(" program_state: %d%n", pkg.program_state);
85 System.out.printf(" robot_state: %d%n", pkg.robot_state);
86 System.out.printf(" main_code: %d%n", pkg.main_code);
87 System.out.printf(" sub_code: %d%n", pkg.sub_code);
88 System.out.println(" --- Pozycje stawów (actual_joint_pos) ---");
89 System.out.printf(" jt_cur_pos[0-2]: %.2f, %.2f, %.2f%n",
90 pkg.jt_cur_pos[0], pkg.jt_cur_pos[1], pkg.jt_cur_pos[2]);
91 System.out.printf(" jt_cur_pos[3-5]: %.2f, %.2f, %.2f%n",
92 pkg.jt_cur_pos[3], pkg.jt_cur_pos[4], pkg.jt_cur_pos[5]);
93 System.out.println(" --- Pozycja TCP (actual_TCP_pos) ---");
94 System.out.printf(" tl_cur_pos[0-2]: %.2f, %.2f, %.2f%n",
95 pkg.tl_cur_pos[0], pkg.tl_cur_pos[1], pkg.tl_cur_pos[2]);
96 System.out.printf(" tl_cur_pos[3-5]: %.2f, %.2f, %.2f%n",
97 pkg.tl_cur_pos[3], pkg.tl_cur_pos[4], pkg.tl_cur_pos[5]);
98 System.out.println(" --- Momenty stawów (actual_joint_torque) ---");
99 System.out.printf(" jt_cur_tor[0-2]: %.2f, %.2f, %.2f%n",
100 pkg.jt_cur_tor[0], pkg.jt_cur_tor[1], pkg.jt_cur_tor[2]);
101 System.out.printf(" jt_cur_tor[3-5]: %.2f, %.2f, %.2f%n",
102 pkg.jt_cur_tor[3], pkg.jt_cur_tor[4], pkg.jt_cur_tor[5]);
103 robot.Sleep(500);
104 }
105 } else {
106 System.out.printf("SetRobotRealtimeStateConfig nie powiodło się z błędem: %d%n", rtn);
107 }
108}