3. Fondamenti del Robot
3.1. Creare un’istanza del Robot (Istanziazione)
1/**
2* @brief Costruttore della classe interfaccia del robot
3*/
4FRRobot();
3.2. Stabilire la comunicazione con il controller
1/**
2* @brief Stabilisce la comunicazione con il controller del robot
3* @param [in] ip Indirizzo IP del controller, predefinito di fabbrica: 192.168.58.2
4* @return Codice di errore
5*/
6errno_t RPC(const char *ip);
3.3. Chiudere la comunicazione con il controller
1/**
2 * @brief Chiude la comunicazione con il controller del robot
3 * @return Codice di errore
4 */
5errno_t CloseRPC();
3.4. Interrogare il numero di versione dell’SDK
1/**
2* @brief Interroga il numero di versione dell'SDK
3* @param [out] version Numero di versione dell'SDK
4* @return Codice di errore
5*/
6errno_t GetSDKVersion(char *version);
3.5. Ottenere l’IP del controller
1/**
2* @brief Ottiene l'IP del controller
3* @param [out] ip IP del controller
4* @return Codice di errore
5*/
6errno_t GetControllerIP(char *ip);
3.6. Controllare l’ingresso o l’uscita del robot dalla modalità di insegnamento tramite trascinamento (drag & teach)
1/**
2* @brief Controlla l'ingresso o l'uscita del robot dalla modalità di insegnamento tramite trascinamento
3* @param [in] state 0-Esci dalla modalità insegnamento tramite trascinamento, 1-Entra nella modalità insegnamento tramite trascinamento
4* @return Codice di errore
5*/
6errno_t DragTeachSwitch(uint8_t state);
3.7. Interrogare se il robot è in modalità trascinamento
1/**
2* @brief Interroga se il robot è in modalità di insegnamento tramite trascinamento
3* @param [out] state 0-Non in modalità insegnamento tramite trascinamento, 1-In modalità insegnamento tramite trascinamento
4* @return Codice di errore
5*/
6errno_t IsInDragTeach(uint8_t *state);
3.8. Controllare l’abilitazione (enable) o la disabilitazione (disable) del robot
1/**
2* @brief Controlla l'abilitazione o la disabilitazione del robot. Dopo l'accensione, il robot è automaticamente abilitato per impostazione predefinita.
3* @param [in] state 0-Disabilita (disable), 1-Abilitata (enable)
4* @return Codice di errore
5*/
6errno_t RobotEnable(uint8_t state);
3.9. Controllare la commutazione della modalità manuale/automatica del robot
1/**
2* @brief Controlla la commutazione della modalità manuale/automatica del robot
3* @param [in] mode 0-Modalità automatica, 1-Modalità manuale
4* @return Codice di errore
5*/
6errno_t Mode(int mode);
3.10. Spegnere il sistema operativo del robot
Nuovo nella versione C++SDK-v2.2.1-3.8.1.
1/**
2* @brief Spegne il sistema operativo del robot
3* @return Codice di errore
4*/
5errno_t ShutDownRobotOS();
3.11. Impostare i parametri di riconnessione per la comunicazione con il robot
Nuovo nella versione C++SDK-v2.1.5.0.
1/**
2* @brief Imposta i parametri di riconnessione per la comunicazione con il robot
3* @param [in] enable Abilita la riconnessione in caso di guasto di rete true-Abilitata false-Disabilitata
4* @param [in] reconnectTime Tempo di riconnessione, unità ms
5* @param [in] period Periodo di riconnessione, unità ms
6* @return Codice di errore
7*/
8errno_t SetReConnectParam(bool enable, int reconnectTime = 30000, int period = 50);
3.12. Spegnere il sistema operativo del robot
1/**
2* @brief Spegne il sistema operativo del robot
3* @return Codice di errore
4*/
5int ShutDownRobotOS();
3.13. Inizializzare i parametri del registro (log)
Nuovo nella versione C++SDK-v2.1.2.0.
1/**
2* @brief Inizializza i parametri del registro (log);
3* @param output_model:Modalità di output, 0-Output diretto;1-Output bufferizzato;2-Output asincrono;
4* @param file_path: Percorso+Nome del file di salvataggio, lunghezza massima 256, il nome deve essere nel formato xxx.log, ad esempio /home/fr/linux/fairino.log;
5* @param file_num:Numero di file per l'archiviazione a rotazione (rolling), da 1 a 20 file. Dimensione massima per singolo file 50M;
6* @return errno_t Codice di errore;
7*/
8 errno_t LoggerInit(int output_model = 0, std::string file_path = "", int file_num = 5);
3.14. Impostare il livello di filtro del registro (log)
Nuovo nella versione C++SDK-v2.1.2.0.
1/**
2* @brief Imposta il livello di filtro del registro (log);
3* @param lvl: Valore del livello di filtro, valori più bassi producono meno log in output, valore predefinito è 1. 1-error, 2-warning, 3-inform, 4-debug;
4*/
5void SetLoggerLevel(int lvl = 1);
3.15. Esempio di codice per il controllo di base del robot
1int TestRobotCtrl(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 robot.SetReConnectParam(true, 30000, 500);
9 char ip[64] = "";
10 char version[64] = "";
11 uint8_t state;
12 robot.GetSDKVersion(version);
13 printf("SDK version:%s\n", version);
14 robot.GetControllerIP(ip);
15 printf("controller ip:%s\n", ip);
16 robot.Mode(1);
17 robot.Sleep(1000);
18 robot.DragTeachSwitch(1);
19 robot.Sleep(1000);
20 robot.IsInDragTeach(&state);
21 printf("drag state :%u\n", state);
22 robot.Sleep(3000);
23 robot.DragTeachSwitch(0);
24 robot.Sleep(1000);
25 robot.IsInDragTeach(&state);
26 printf("drag state :%u\n", state);
27 robot.Sleep(3000);
28 robot.RobotEnable(0);
29 robot.Sleep(3000);
30 robot.RobotEnable(1);
31 robot.Mode(0);
32 robot.Sleep(1000);
33 robot.Mode(1);
34 robot.Sleep(3000);
35 robot.ShutDownRobotOS();
36 robot.CloseRPC();
37 return 0;
38}
3.16. Esempio di codice per ottenere la versione software del robot
1/**
2* @brief Ottiene la versione software del robot
3* @param[out] robotModel Modello del robot
4* @param[out] webversion Versione web
5* @param[out] controllerVersion Versione del controller
6* @return Codice di errore
7*/
8errno_t GetSoftwareVersion(char robotModel[64], char webVersion[64], char controllerVersion[64]);
3.17. Ottenere la versione hardware del robot
Nuovo nella versione C++SDK-v2.1.1.0.
1/**
2* @brief Ottiene la versione hardware del robot
3* @param[out] ctrlBoxBoardversion Versione hardware della scheda principale del control box
4* @param[out] driver1version Versione hardware del driver 1
5* @param[out] driver2version Versione hardware del driver 2
6* @param[out] driver3version Versione hardware del driver 3
7* @param[out] driver4version Versione hardware del driver 4
8* @param[out] driver5version Versione hardware del driver 5
9* @param[out] driver6version Versione hardware del driver 6
10* @param[out] endBoardversion Versione hardware della scheda terminale (end board)
11*/
12errno_t GetHardwareVersion(char ctrlBoxBoardversion[128], char driver1version[128], char driver2version[128], char driver3version[128], char driver4version[128], char driver5version[128], char driver6version[128], char endBoardversion[128]);
3.18. Ottenere la versione firmware del robot
Nuovo nella versione C++SDK-v2.1.1.0.
1/**
2* @brief Ottiene la versione firmware del robot
3* @param[out] ctrlBoxBoardversion Versione firmware della scheda principale del control box
4* @param[out] driver1version Versione firmware del driver 1
5* @param[out] driver2version Versione firmware del driver 2
6* @param[out] driver3version Versione firmware del driver 3
7* @param[out] driver4version Versione firmware del driver 4
8* @param[out] driver5version Versione firmware del driver 5
9* @param[out] driver6version Versione firmware del driver 6
10* @param[out] endBoardversion Versione firmware della scheda terminale (end board)
11*/
12errno_t GetFirmwareVersion(char ctrlBoxBoardversion[128], char driver1version[128], char driver2version[128], char driver3version[128], char driver4version[128], char driver5version[128], char driver6version[128], char endBoardversion[128]);
3.19. Esempio di codice per ottenere le versioni software/firmware del robot
1int TestGetVersions(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 char robotModel[64] = { 0 };
14 char webversion[64] = { 0 };
15 char controllerVersion[64] = { 0 };
16 char ctrlBoxBoardversion[128] = { 0 };
17 char driver1version[128] = { 0 };
18 char driver2version[128] = { 0 };
19 char driver3version[128] = { 0 };
20 char driver4version[128] = { 0 };
21 char driver5version[128] = { 0 };
22 char driver6version[128] = { 0 };
23 char endBoardversion[128] = { 0 };
24 rtn = robot.GetSoftwareVersion(robotModel, webversion, controllerVersion);
25 printf("Getsoftwareversion rtn is: %d\n", rtn);
26 printf("robotmodel is: %s, webversion is: %s, controllerVersion is: %s \n\n", robotModel, webversion, controllerVersion);
27 rtn = robot.GetHardwareVersion(ctrlBoxBoardversion, driver1version, driver2version, driver3version, driver4version, driver5version, driver6version, endBoardversion);
28 printf("GetHardwareversion rtn is: %d\n", rtn);
29 printf("GetHardwareversion get hardware versoin is: %s, %s, %s, %s, %s, %s, %s, %s\n\n", ctrlBoxBoardversion, driver1version, driver2version, driver3version, driver4version, driver5version, driver6version, endBoardversion);
30 rtn = robot.GetFirmwareVersion(ctrlBoxBoardversion, driver1version, driver2version, driver3version, driver4version, driver5version, driver6version, endBoardversion);
31 printf("GetFirmwareversion rtn is: %d\n", rtn);
32 printf("GetHardwareversion get hardware versoin is: %s, %s, %s, %s, %s, %s, %s, %s\n\n", ctrlBoxBoardversion, driver1version, driver2version, driver3version, driver4version, driver5version, driver6version, endBoardversion);
33 robot.CloseRPC();
34 return 0;
35 }