效果:通過can總線直接向機械手控制器發送控制指令,實現機械手閉合控制,同時實現六維力傳感器數據實時監測。
環境:ubuntu14.04+ROS indigo+pcan驅動(插入pcan後,能夠使用ls /dev/pcan*顯示出端口名稱)。
[正文]
通過libpcan庫操作can總線實現數據發送、讀取,按照機械手控制器的數據格式要求,即可實現機械手和六維力傳感器的驅動。可以通過兩種方法獲得機械手和六維力傳感器個數據格式:其一是通過在控制機械手同時,外接一根pcan usb到windows下PcanView軟件觀察總結機械手控制時的數據格式,然後進行模仿,該方法較爲直接,但是需要將數據和機械手控制所用的角速度、角度等信息進行對應,因此需要一定的經驗。其二是從barrett的節點文件barrett_hand-indigo-devel/bhand_controller/src/bhand_controller/下的bhand_node.py,pyHand_api.py入手,找到其與機械手操作相關的函數或類的數據格式定義。而六維力傳感器的數據格式及初始化操作要從MonitorForceTorque.cpp中分析得到。
3.2.1 顯式調用libpcan.so 動態鏈接庫實現can總線操作
下面給出一個libcan庫的操作例子,本例博文地址:http://blog.csdn.net/ hookie1990/article/details/52629043。
1,添加載入需要的頭文件及文件中自己使用的頭文件。載入相關的頭文件爲dlfcn.h,fcntl.h,由於目標使用libpcan函數,所以libpcan.h也要加入。
l dlfcn.h : Linux動態庫的顯式調用庫,包括dlopen(),dlclose()等。
l fcntl.h :fcntl.h定義了很多宏和open,fcntl函數原型,包括close open等關閉文件的系列操作。本文中用到了其打開文件的宏定義,即打開文件的控制模式。
int open(const char*pathname, int oflag, ... /* mode_t mode */);
返回值:成功則返回文件描述符,否則返回 -1
對於open函數來說,第三個參數(...)僅當創建新文件時(即使用了O_CREAT 時)才使用,用於指定文件的訪問權限位(access permission bits)。pathname是待打開/創建文件的路徑名(如 C:/cpp/a.cpp);oflag用於指定文件的打開/創建模式,這個參數可由以下常量(定義於 fcntl.h)通過邏輯或構成。
O_RDONLY 只讀模式
O_WRONLY 只寫模式
O_RDWR 讀寫模式
打開/創建文件時,至少得使用上述三個常量中的一個。以下常量是選用的:
O_APPEND 每次寫操作都寫入文件的末尾
O_CREAT 如果指定文件不存在,則創建這個文件
O_EXCL 如果要創建的文件已存在,則返回 -1,並且修改errno 的值
O_TRUNC 如果文件存在,並且以只寫/讀寫方式打開,則清空文件全部內容(即將其長度截短爲0)
O_NOCTTY 如果路徑名指向終端設備,不要把這個設備用作控制終端。
O_NONBLOCK 如果路徑名指向FIFO/塊文件/字符文件,則把文件的打開和後繼 I/O設置爲非阻塞模式(nonblockingmode)
以下三個常量同樣是選用的,它們用於同步輸入輸出
O_DSYNC 等待物理 I/O 結束後再write。在不影響讀取新寫入的數據的前提下,不等待文件屬性更新。
O_RSYNC read 等待所有寫入同一區域的寫操作完成後再進行
O_SYNC 等待物理 I/O 結束後再write,包括更新文件屬性的 I/O open 返回的文件描述符一定是最小的未被使用的描述符。
2,根據目標使用的函數及其參數形式定義目標指針。
此處暫時使用兩個函數,一個CAN_Init,一個LINUX_CAN_Open函數。
下面爲libpcan.h頭文件中的定義。
1. DWORD CAN_Init(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
2. HANDLE LINUX_CAN_Open(const char *szDeviceName, int nFlag);
根據上面定義我們聲明兩個函數指針的類型,方便後文使用
1. //define mapping function according to target function in libpcan.h
2. typedef DWORD (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
3. typedef HANDLE (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
然後用聲明的類型定義映射函數名
1. //define function pointer,there is a one-to-one mapping between target function and your defined function
2. funCAN_Init_TYPE fun_CAN_Init;
3. funLINUX_CAN_Open_TYPE funLINUX_CAN_Open;
3,定義文件訪問訪問的指針,實現文件加載。
1. void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
2. // dlopen 函數還會自動解析共享庫中的依賴項。這樣,如果您打開了一個依賴於其他共享庫的對象,它就會自動加載它們。
3. // 函數返回一個句柄,該句柄用於後續的 API 調用
4. libm_handle = dlopen("libpcan.so", RTLD_LAZY );
5. // 如果返回 NULL 句柄,表示無法找到對象文件,過程結束。否則的話,將會得到對象的一個句柄,可以進一步詢問對象
6. if (!libm_handle){
7. // 如果返回 NULL 句柄,通過dlerror方法可以取得無法訪問對象的原因
8. printf("Open Error:%s.\n",dlerror());
9. return 0;
10. }
其中用到的dlopen的參數解釋如下:
void *dlopen(const char *filename, intflag);
其中flag有:RTLD_LAZYRTLD_NOW RTLD_GLOBAL,其含義分別爲:
RTLD_LAZY:在dlopen返回前,對於動態庫中存在的未定義的變量(如外部變量extern,也可以是函數)不執行解析,就是不解析這個變量的地址。
RTLD_NOW:與上面不同,他需要在dlopen返回前,解析出每個未定義變量的地址,如果解析不出來,在dlopen會返回NULL,錯誤爲:undefined symbol: xxxx.......
RTLD_GLOBAL:它的含義是它的含義是使得庫中的解析的定義變量在隨後的隨後其它的鏈接庫中變得可以使用。
4,實現動態庫函數到目標函數的映射。
1. // 使用 dlsym 函數,嘗試解析新打開的對象文件中的符號。您將會得到一個有效的指向該符號的指針,或者是得到一個 NULL 並返回一個錯誤
2. //one-to-one mapping
3. char *errorInfo;//error information pointer
4. fun_CAN_Init =(funCAN_Init_TYPE)dlsym(libm_handle,"CAN_Init");
5. funLINUX_CAN_Open = (funLINUX_CAN_Open_TYPE)dlsym(libm_handle,"LINUX_CAN_Open");
6. errorInfo = dlerror();// 調用dlerror方法,返回錯誤信息的同時,內存中的錯誤信息被清空
7. if (errorInfo != NULL){
8. printf("Dlsym Error:%s.\n",errorInfo);
9. return 0;
10. }
5,定義訪問硬件的HANDLE(指針),實現自定義函數對於can總線的訪問,訪問完成要關閉對象。
此前文中已有定義。
1. #define DEFAULT_NODE "/dev/pcan0"
此處實現如下:
1. HANDLE pcan_handle =NULL;//void *pcan_handle
2.
3. const char *szDevNode = DEFAULT_NODE;//define const pointer point to device name
4. pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR | O_NONBLOCK);//use mapping function
5. //judge whether the call is success.if pcan_handle=null,the call would be failed
6. if(pcan_handle){
7. can_set_init();
8. printf("receivetest: %s have been opened\n", szDevNode);
9. }
10. else
11. printf("receivetest: can't open %s\n", szDevNode);
12.
13. // 調用 ELF 對象中的目標函數後,通過調用 dlclose 來關閉對它的訪問
14. dlclose(libm_handle);
最終映射使用的函數如下:
funCAN_Init =(funCAN_Init_TYPE) dlsym(libm_handle,"CAN_Init");
funLINUX_CAN_Open =(funLINUX_CAN_Open_TYPE) dlsym(libm_handle,"LINUX_CAN_Open");
funCAN_Close =(funCAN_Close_TYPE) dlsym(libm_handle,"CAN_Close");
funCAN_VersionInfo =(funCAN_VersionInfo_TYPE) dlsym(libm_handle,"CAN_VersionInfo");
funLINUX_CAN_Read =(funLINUX_CAN_Read_TYPE) dlsym(libm_handle,"LINUX_CAN_Read");
funCAN_Status =(funCAN_Status_TYPE) dlsym(libm_handle,"CAN_Status");
funnGetLastError =(funnGetLastError_TYPE) dlsym(libm_handle,"nGetLastError");
funCAN_Write =(funCAN_Write_TYPE) dlsym(libm_handle,"CAN_Write");
3.2.2 機械手位置控制實現
根據我們對於機械手復位的檢測,發現在復位過程中上位機軟件想機械手發送了一組數據,通過對發送過程的簡單模仿,可使Barrett機械手復位。
15. void barett_hand_init()
16. {
17. TPCANMsg test_data;
18. //query the bhand state.
19. for(chari=0;i<=3;i++)
20. {
21. test_data.ID=0xcb+i;
22. test_data.LEN=1;
23. test_data.MSGTYPE=MSGTYPE_STANDARD;
24. test_data.DATA[0]=0x05;
25. funCAN_Write(pcan_handle,&test_data);
26. }
27.
28. for(chari=0;i<=3;i++)
29. {
30. test_data.ID=0xcb+i;
31. test_data.LEN=6;
32. test_data.MSGTYPE=MSGTYPE_STANDARD;
33. test_data.DATA[0]=0xb2;
34. for(charj=1;j<6;j++)test_data.DATA[j]=0;
35. funCAN_Write(pcan_handle,&test_data);
36. }
37.
38. for(chari=0;i<=3;i++)
39. {
40. test_data.ID=0xcb+i;
41. test_data.LEN=6;
42. test_data.MSGTYPE=MSGTYPE_STANDARD;
43. for(charj=0;j<6;j++)test_data.DATA[j]=0;
44. test_data.DATA[0]=0xce;
45. test_data.DATA[2]=0xfa;
46. funCAN_Write(pcan_handle,&test_data);
47. }
48.
49. //query the bhand state.
50. for(chari=0;i<=3;i++)
51. {
52. test_data.ID=0xcb+i;
53. test_data.LEN=1;
54. test_data.MSGTYPE=MSGTYPE_STANDARD;
55. test_data.DATA[0]=0x00;
56. funCAN_Write(pcan_handle,&test_data);
57. }
58. //query the bhand state.
59. for(chari=0;i<=3;i++)
60. {
61. test_data.ID=0xcb+i;
62. test_data.LEN=1;
63. test_data.MSGTYPE=MSGTYPE_STANDARD;
64. test_data.DATA[0]=0x5a;
65. funCAN_Write(pcan_handle,&test_data);
66. }
67. //query the bhand state.
68. for(chari=0;i<=3;i++)
69. {
70. test_data.ID=0xcb+i;
71. test_data.LEN=1;
72. test_data.MSGTYPE=MSGTYPE_STANDARD;
73. test_data.DATA[0]=0x08;
74. funCAN_Write(pcan_handle,&test_data);
75. }
76. //init the bhand.
77. for(chari=0;i<=3;i++)
78. {
79. test_data.ID=0xcb+i;
80. test_data.LEN=4;
81. test_data.MSGTYPE=MSGTYPE_STANDARD;
82. test_data.DATA[0]=0x9d;
83. test_data.DATA[1]=0x00;
84. test_data.DATA[2]=0x0d;
85. test_data.DATA[3]=0x00;
86. funCAN_Write(pcan_handle,&test_data);
87. }
88. sleep(5);
89. ROS_INFO("BHandInit successfully!");
90.
91. }
解析出來對於機械手位置控制的實現,其中參數範圍如下式所示,此處還沒有轉換爲角度,後期在sia_bhand_controller中已轉換爲對應的0-2.5的弧度值。
Position=x*10 0000, 0<=x<=2.5
11. void bhand_position_control(long f1_pos,long f2_pos,longf3_pos,long sp_pos)
12. {
13. TPCANMsg CANMsg;
14.
15.
16. CANMsg.LEN = 6;
17. CANMsg.MSGTYPE =MSGTYPE_STANDARD;
18.
19. CANMsg.DATA[0] = 0xba;
20. CANMsg.DATA[1] = 0;
21. CANMsg.DATA[2] = 0;
22. CANMsg.DATA[3] = 0;
23. CANMsg.DATA[4] = 0;
24. CANMsg.DATA[5] = 0;
25.
26. CANMsg.DATA[2] = f1_pos & 0xff;
27. CANMsg.DATA[3] =(f1_pos>>8) & 0xff;
28. CANMsg.DATA[4] =(f1_pos>>16) & 0xff;
29.
30. CANMsg.ID = 0xcb;
31. funCAN_Write(pcan_handle,&CANMsg);
32. usleep(1000);
33. CANMsg.DATA[2] = f2_pos& 0xff;
34. CANMsg.DATA[3] =(f2_pos>>8) & 0xff;
35. CANMsg.DATA[4] =(f2_pos>>16) & 0xff;
36.
37. CANMsg.ID = 0xcc;
38. funCAN_Write(pcan_handle,&CANMsg);
39. usleep(1000);
40.
41. CANMsg.DATA[2] = f3_pos& 0xff;
42. CANMsg.DATA[3] =(f3_pos>>8) & 0xff;
43. CANMsg.DATA[4] =(f3_pos>>16) & 0xff;
44.
45. CANMsg.ID = 0xcd;
46. funCAN_Write(pcan_handle,&CANMsg);
47. usleep(1000);
48.
49. CANMsg.DATA[2] = sp_pos& 0xff;
50. CANMsg.DATA[3] =(sp_pos>>8) & 0xff;
51. CANMsg.DATA[4] =(sp_pos>>16) & 0xff;
52.
53. CANMsg.ID = 0xce;
54. funCAN_Write(pcan_handle,&CANMsg);
55. usleep(1000);
56. }
由於本例中我們選擇對於pcan的處理是非阻塞的模式,因此必須有一個函數處理接收到的無意義數據或者請求。其中的if(m.Msg.ID == 0x50a || m.Msg.ID == 0x50b)判斷則是標示該幀頭開始數據爲六維力傳感器的數據。
57. int read_loop(HANDLE h,ros::Publisher pub, bool display_on,boolpublish_on)
58. {
59. TPCANRdMsg m;
60. __u32 status;
61.
62. if (funLINUX_CAN_Read(h,&m)) {
63. //perror("receivetest:LINUX_CAN_Read()");
64. return errno;
65. }
66. if(m.Msg.ID == 0x50a ||m.Msg.ID == 0x50b)
67. {
68. if (display_on)
69. print_message_ex(&m);
70. if (publish_on)
71. publish_forcedata(&m,pub,force_display_on);
72.
73. // check if a CANstatus is pending
74. if (m.Msg.MSGTYPE& MSGTYPE_STATUS) {
75. status =funCAN_Status(h);
76. if ((int)status< 0) {
77. errno =funnGetLastError();
78. perror("receivetest:CAN_Status()");
79. returnerrno;
80. }
81.
82. printf("receivetest: pendingCAN status 0x%04x read.\n",
83. (__u16)status);
84. }
85. }
86. return 0;
87. }
3.2.3 Barrett Hand腕部六維力傳感器操作
從MonitorForceTorque.cpp中我們可以查到六維力傳感器的初始化步驟:由兩個函數完成,wavePuck()和setPropertySlow().完成設備喚醒和啓動校準(非初始受力置零)。
88. //wake puck
89. wavePuck(0,8);
90. //tare
91. setPropertySlow(0,FTSENSORID,FTDATAPROPERTY,0,0);
函數的原型如下:
11.int wavePuck(int bus,int id)
12.{
13.
14. TPCANMsg msgOut;
15. DWORD err;
16.
17. //Generate the outbound message
18. msgOut.ID = id;
19. msgOut.MSGTYPE = MSGTYPE_STANDARD;
20. msgOut.LEN = 4;
21. msgOut.DATA[0] = 0x85; // Set Status
22. msgOut.DATA[1] = 0x00;
23. msgOut.DATA[2] = 0x02; // Status = Ready
24. msgOut.DATA[3] = 0x00;
25.
26. //Send the message
27. //err = CAN_Write( &msgOut );
28. err=funCAN_Write(pcan_handle,&msgOut);
29. sleep(1);
30.
31. return(err);
32.}
33.
34.int compile(
35. intproperty /** The property beingcompiled (use the enumerations in btcan.h) */,
36. longlongVal /** The value to set theproperty to */,
37. unsigned char *data /** A pointer to acharacter buffer in which to build the data payload */,
38. int*dataLen /** A pointer to thetotal length of the data payload for this packet */,
39. intisForSafety /** A flag indicatingwhether this packet is destined for the safety circuit or not */)
40.{
41. inti;
42.
43. //Check the property
44. //if(property > PROP_END)
45. //{
46. // syslog(LOG_ERR,"compile(): Invalid property = %d", property);
47. // return(1);
48. //}
49.
50. /*Insert the property */
51. data[0]= property;
52. data[1] = 0; /* To align the values for thetater's DSP */
53.
54. /*Append the value */
55. for(i = 2; i < 6; i++)
56. {
57. data[i] = (char)(longVal &0x000000FF);
58. longVal >>= 8;
59. }
60.
61. /*Record the proper data length */
62. *dataLen = 6; //(dataType[property] &0x0007) + 2;
63.
64. return (0);
65.}
66.
67.int setPropertySlow(int bus, int id, intproperty, int verify, long value)
68.{
69. TPCANMsg msgOut, msgIn;
70. DWORD err;
71. intdataHeader, i;
72. longresponse;
73. //unsigned char data[8];
74. int len;
75.
76. //syslog(LOG_ERR, "About to compilesetProperty, property = %d", property);
77. //Compile 'set' packet
78. err= compile(property, value, msgOut.DATA, &len, 0);
79.
80. //Generate the outbound message
81. msgOut.ID = id;
82. msgOut.MSGTYPE = MSGTYPE_STANDARD;
83. msgOut.LEN = len;
84.
85.
86.
87. //syslog(LOG_ERR, "After compilationdata[0] = %d", data[0]);
88. msgOut.DATA[0] |= 0x80; // Set the 'Set'bit
89.
90. //Send the message
91. //CAN_Write( &msgOut );
92. funCAN_Write(pcan_handle,&msgOut);
93.
94. //Sleep(250);
95.
96. //BUG: This will not verify properties from groups of pucks
97. return(0);
98.}
99.
100.int getPropertyFT(int bus,HANDLE h)
101.
102.{
103.
104.
105. TPCANMsg msgOut, msgInOne, msgInTwo;
106. DWORD err;
107.
108.
109. //Generate the outbound message
110. msgOut.ID = FTSENSORID;
111. msgOut.MSGTYPE = MSGTYPE_STANDARD;
112. msgOut.LEN = 1;
113. msgOut.DATA[0] = FTDATAPROPERTY; // Get theft data
114.
115.}
線程循環檢測有效數據即read_loop()所執行的功能,檢測到幀頭後要進行如下的數據解析,規則是每兩個字節表示一個浮點數,以x軸力爲例表示一個完整的處理如下:
x_force=mr->Msg.DATA[1];
x_force<<=8;
x_force|=mr->Msg.DATA[0];
force_x_float=x_force/256.0;
得到受力或力矩的數值後便需要根據力和力矩的數值通知機械臂或主控制系統進行進一步的處理,該部分工作由bhand_axis_force_warning()函數實現。完整代碼參看src下對應cpp文件或者個人博客。
116.void publish_forcedata(TPCANRdMsg*mr,ros::Publisher axis_force_pub,bool display_on)
117.{
118. short int x_force,y_force,z_force;
119. short int torque_x,torque_y,torque_z;
120.
121.
122. if(0x50a==mr->Msg.ID )
123. {
124. x_force=mr->Msg.DATA[1];
125. x_force<<=8;
126. x_force|=mr->Msg.DATA[0];
127.
128. y_force=mr->Msg.DATA[3];
129. y_force<<=8;
130. y_force|=mr->Msg.DATA[2];
131.
132. z_force=mr->Msg.DATA[5];
133. z_force<<=8;
134. z_force|=mr->Msg.DATA[4];
135. force_x_float=x_force/256.0;
136. force_y_float=y_force/256.0;
137. force_z_float=z_force/256.0;
138. if(display_on) ROS_INFO(" Forcex:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
139. }
140.
141. if(0x50b==mr->Msg.ID )
142. {
143. torque_x=mr->Msg.DATA[1];
144. torque_x<<=8;
145. torque_x|=mr->Msg.DATA[0];
146.
147. torque_y=mr->Msg.DATA[3];
148. torque_y<<=8;
149. torque_y|=mr->Msg.DATA[2];
150.
151. torque_z=mr->Msg.DATA[5];
152. torque_z<<=8;
153. torque_z|=mr->Msg.DATA[4];
154. torque_x_float=torque_x/256.0;
155. torque_y_float=torque_y/256.0;
156. torque_z_float=torque_z/256.0;
157. if(display_on) ROS_INFO("Torquex:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
158. }
159.
160.}
3.3 launch文件及節點數配置
實際參數、節點的配置均由launch文件實現,launch文件可以接收int,string,double類型的數據,如下爲一個示例。
161.<launch>
162. <nodepkg="beginner_tutorials" type="bhand_axis_force_limit"name="bhand_axis_force" output="screen">
163. <paramname="dev_name" type="string"value="/dev/pcanusb0" />
164. <paramname="bhand_open_pos" type="int" value="6" />
165. <paramname="bhand_close_pos" type="int" value="14"/>
166. <paramname="force_display_on" type="bool" value="false"/>
167. <paramname="force_threshold" type="double" value="18.0"/>
168. <paramname="torque_threshold" type="double"value="20.0" />
169. <paramname="force_danger" type="double" value="25.0"/>
170. <paramname="torque_danger" type="double" value="40.0"/>
171. <paramname="left_torque_x_threshold" type="double" value="-2.0" />
172. <paramname="right_torque_x_threshold" type="double"value="2.0" />
173. </node>
174.</launch>
節點代碼中的接受處理流程與launch文件一一對應,如下:
175. string dev_name;
176. ros::param::get("~dev_name",dev_name);
177. ROS_INFO("PCANDevice:%s",dev_name.c_str());
178.
179. intbhand_open_pos=5;
180. ros::param::get("~bhand_open_pos",bhand_open_pos);
181. ROS_INFO("Bhand open position:%d",bhand_open_pos);
182.
183. intbhand_close_pos=13;
184. ros::param::get("~bhand_close_pos",bhand_close_pos);
185. ROS_INFO("Bhand close position:%d",bhand_close_pos);
186.
187. ros::param::get("~force_display_on",force_display_on);
188. ROS_INFO("ForceDatadisplay:%s",force_display_on==false?"false":"true");
189.
190. ros::param::get("~force_threshold",force_threshold);
191. ROS_INFO("Force threshold :%f",force_threshold);
192. ros::param::get("~torque_threshold",torque_threshold);
193. ROS_INFO("Torque threshold :%f",torque_threshold);
194.
195. ros::param::get("~force_danger",force_danger);
196. ROS_WARN(" Force dangerous threshold:%f",force_danger);
197. ros::param::get("~torque_danger",torque_danger);
198. ROS_WARN("Torque dangerous threshold:%f",torque_danger);
199.
200. ros::param::get("~left_torque_x_threshold",left_torque_x_threshold);
201. ROS_WARN(" left torque x threshold:%f",left_torque_x_threshold);
202. ros::param::get("~right_torque_x_threshold",right_torque_x_threshold);
203. ROS_WARN("right torque x threshold:%f",right_torque_x_threshold);
204.
下附本節完整代碼bhand_axis_force_limit.cpp
#include <ros/ros.h>
#include <stdio.h>
#include <dlfcn.h>
#include <libpcan.h>
#include <fcntl.h>
#include <errno.h>
#include <signal.h>
#include "id_data_msgs/ID_Data.h"//using for notie event
#include "ros/callback_queue.h"
#include "string"
using namespace std;
//define mapping function according to target function in libpcan.h
typedef DWORD (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
typedef HANDLE (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
typedef DWORD (*funCAN_VersionInfo_TYPE)(HANDLE hHandle, LPSTR lpszTextBuff);
typedef DWORD (*funCAN_Close_TYPE)(HANDLE hHandle);
typedef DWORD (*funLINUX_CAN_Read_TYPE)(HANDLE hHandle, TPCANRdMsg* pMsgBuff);
typedef DWORD (*funCAN_Status_TYPE)(HANDLE hHandle);
typedef int (*funnGetLastError_TYPE)(void);
typedef DWORD (*funCAN_Write_TYPE)(HANDLE hHandle, TPCANMsg* pMsgBuff);
//the target device name
#define DEFAULT_NODE "/dev/pcan0"
#define FTSENSORID (8)
#define FTDATAPROPERTY (54)
//GLOBALS
void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
HANDLE pcan_handle =NULL;//void *pcan_handle
id_data_msgs::ID_Data axis_force_msg;
bool force_display_on=true;
float force_threshold=4.0;
float torque_threshold=10.0;
double force_danger=15.0;
double torque_danger=30.0;
double left_torque_x_threshold=-1.0;
double right_torque_x_threshold=1.0;
//globals
bool close_hand_flag=false;
bool open_hand_flag=false;
float torque_x_float,torque_y_float,torque_z_float;
float force_x_float,force_y_float,force_z_float;
void notice_data_clear(id_data_msgs::ID_Data *test);
int wavePuck(int bus,int id);
int compile(
int property /** The property being compiled (use the enumerations in btcan.h) */,
long longVal /** The value to set the property to */,
unsigned char *data /** A pointer to a character buffer in which to build the data payload */,
int *dataLen /** A pointer to the total length of the data payload for this packet */,
int isForSafety /** A flag indicating whether this packet is destined for the safety circuit or not */);
int setPropertySlow(int bus, int id, int property, int verify, long value);
//define function pointer,there is a one-to-one mapping between target function and your defined function
funCAN_Init_TYPE funCAN_Init;
funLINUX_CAN_Open_TYPE funLINUX_CAN_Open;
funCAN_VersionInfo_TYPE funCAN_VersionInfo;
funCAN_Close_TYPE funCAN_Close;
funLINUX_CAN_Read_TYPE funLINUX_CAN_Read;
funCAN_Status_TYPE funCAN_Status;
funnGetLastError_TYPE funnGetLastError;
funCAN_Write_TYPE funCAN_Write;
class notice_pub_sub
{
public:
boost::function<void (const id_data_msgs::ID_Data::ConstPtr&)> notice_pub_sub_msgCallbackFun;
notice_pub_sub();
void notice_pub_sub_listener();
void notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data);
void notice_display(id_data_msgs::ID_Data notice_msg,bool set);
void notice_sub_spinner(char set);
private:
ros::NodeHandle notice_handle;
ros::Subscriber notice_subscriber;
ros::Publisher notice_publisher;
ros::SubscribeOptions notice_ops;
ros::AsyncSpinner *notice_spinner;
ros::CallbackQueue notice_callbackqueue;
void notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg);
};
notice_pub_sub::notice_pub_sub()
{
notice_pub_sub_msgCallbackFun=boost::bind(¬ice_pub_sub::notice_msgCallback,this,_1);
notice_ops=ros::SubscribeOptions::create<id_data_msgs::ID_Data>(
"/notice",
1,
notice_pub_sub_msgCallbackFun,
ros::VoidPtr(),
¬ice_callbackqueue
);
notice_subscriber=notice_handle.subscribe(notice_ops);
notice_spinner=new ros::AsyncSpinner(1,¬ice_callbackqueue);
notice_publisher=notice_handle.advertise<id_data_msgs::ID_Data>("/notice",1);
}
void notice_pub_sub::notice_pub_sub_listener()
{
}
void notice_pub_sub::notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data)
{
notice_publisher.publish(id_data);
}
void notice_pub_sub::notice_display(id_data_msgs::ID_Data notice_msg,bool set)
{
if(set)
{
printf("REC Notice message,ID: %d,Data: ",notice_msg.id);
for(char i=0;i<8;i++)
{
printf("%d ",notice_msg.data[i]);
if(i==7) printf("\n");
}
}
}
void notice_pub_sub::notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg)
{
id_data_msgs::ID_Data notice_message;
notice_message.id=0;
for(char i=0;i<8;i++)notice_message.data[i]=0;
notice_message.id=notice_msg->id;
for(char i=0;i<8;i++)notice_message.data[i]=notice_msg->data[i];
notice_pub_sub::notice_display(notice_message,true);
if(notice_message.id==1 && notice_message.data[0]==1)//close flag
{
close_hand_flag=true;
notice_data_clear(¬ice_message);
notice_message.id=1;
notice_message.data[0]=14;
notice_publisher.publish(notice_message);
}
if(notice_message.id==1 && notice_message.data[0]==0)//open flag
{
open_hand_flag=true;
notice_data_clear(¬ice_message);
notice_message.id=1;
notice_message.data[0]=14;
notice_publisher.publish(notice_message);
}
}
void notice_pub_sub::notice_sub_spinner(char set)
{
if(set==1)
notice_spinner->start();
if(set==0)
notice_spinner->stop();
}
//function declaration
int read_loop(HANDLE h,ros::Publisher pub,bool display_on,bool publish_on);
void print_message(TPCANMsg *m);
void print_message_ex(TPCANRdMsg *mr);
void do_exit(void *file,HANDLE h,int error);
void signal_handler(int signal);
void init();
void publish_forcedata(TPCANRdMsg *mr, ros::Publisher axis_force_pub, bool display_on);
void barett_hand_init();
void bhand_position_control(long f1_pos,long f2_pos,long f3_pos,long sp_pos);
void bhand_axis_force_warning(notice_pub_sub *notice_test,float force_x,float force_y,float force_z,float torque_x,float torque_y,float torque_z);
//
int sys_count=0;
int main(int argc,char **argv)
{
ros::init(argc,argv,"bhand_axis_force");
ros::NodeHandle handle_test;
init();
string dev_name;
ros::param::get("~dev_name",dev_name);
ROS_INFO("PCAN Device:%s",dev_name.c_str());
int bhand_open_pos=5;
ros::param::get("~bhand_open_pos",bhand_open_pos);
ROS_INFO("Bhand open position :%d",bhand_open_pos);
int bhand_close_pos=13;
ros::param::get("~bhand_close_pos",bhand_close_pos);
ROS_INFO("Bhand close position :%d",bhand_close_pos);
ros::param::get("~force_display_on",force_display_on);
ROS_INFO("ForceData display:%s",force_display_on==false?"false":"true");
ros::param::get("~force_threshold",force_threshold);
ROS_INFO("Force threshold :%f",force_threshold);
ros::param::get("~torque_threshold",torque_threshold);
ROS_INFO("Torque threshold :%f",torque_threshold);
ros::param::get("~force_danger",force_danger);
ROS_WARN(" Force dangerous threshold :%f",force_danger);
ros::param::get("~torque_danger",torque_danger);
ROS_WARN("Torque dangerous threshold :%f",torque_danger);
ros::param::get("~left_torque_x_threshold",left_torque_x_threshold);
ROS_WARN(" left torque x threshold :%f",left_torque_x_threshold);
ros::param::get("~right_torque_x_threshold",right_torque_x_threshold);
ROS_WARN("right torque x threshold :%f",right_torque_x_threshold);
//load libpcan.so using dlopen function,return handle for further use
libm_handle = dlopen("libpcan.so", RTLD_LAZY );
if (!libm_handle){
printf("Open Error:%s.\n",dlerror());//if file can't be loaded,return null,get reason using dlerror function
return 0;
}
char *errorInfo;//error information pointer
//one-to-one mapping using dlsym function,if return null,mapping would be failed
funCAN_Init =(funCAN_Init_TYPE) dlsym(libm_handle,"CAN_Init");
funLINUX_CAN_Open =(funLINUX_CAN_Open_TYPE) dlsym(libm_handle,"LINUX_CAN_Open");
funCAN_Close =(funCAN_Close_TYPE) dlsym(libm_handle,"CAN_Close");
funCAN_VersionInfo =(funCAN_VersionInfo_TYPE) dlsym(libm_handle,"CAN_VersionInfo");
funLINUX_CAN_Read =(funLINUX_CAN_Read_TYPE) dlsym(libm_handle,"LINUX_CAN_Read");
funCAN_Status =(funCAN_Status_TYPE) dlsym(libm_handle,"CAN_Status");
funnGetLastError =(funnGetLastError_TYPE) dlsym(libm_handle,"nGetLastError");
funCAN_Write =(funCAN_Write_TYPE) dlsym(libm_handle,"CAN_Write");
errorInfo = dlerror();//get error using dlerror function,and clear the error list in memory
if (errorInfo != NULL){
printf("Dlsym Error:%s.\n",errorInfo);
return 0;
}
char txt[VERSIONSTRING_LEN]; //store information of can version
unsigned short wBTR0BTR1 = CAN_BAUD_1M; //set the communicate baud rate of can bus
int nExtended = CAN_INIT_TYPE_ST; //set can message int standard model
const char *szDevNode = DEFAULT_NODE; //define const pointer point to device name
if(dev_name.c_str() !="")
pcan_handle = funLINUX_CAN_Open(dev_name.c_str(), O_RDWR | O_NONBLOCK);
else
{
pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR | O_NONBLOCK);//use mapping function
dev_name=DEFAULT_NODE;
}
//pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR | O_NONBLOCK );//use mapping function
//judge whether the call is success.if pcan_handle=null,the call would be failed
if(pcan_handle){
printf("CAN Bus test: %s have been opened\n", szDevNode);
errno = funCAN_VersionInfo(pcan_handle, txt);
if (!errno)
printf("CAN Bus test: driver version = %s\n", txt);
else {
perror("CAN Bus test: CAN_VersionInfo()");
}
if (wBTR0BTR1) {
errno = funCAN_Init(pcan_handle, wBTR0BTR1, nExtended);
if (errno) {
perror("CAN Bus test: CAN_Init()");
}
else
printf("Device Info: %s; CAN_BAUD_1M; CAN_INIT_TYPE_ST\n", szDevNode);
}
}
else
printf("CAN Bus test: can't open %s\n", szDevNode);
//test of can send data
barett_hand_init();
bhand_position_control(bhand_open_pos*10000,bhand_open_pos*10000,bhand_open_pos*10000,0);//position: (0--2.5)*10 0000
sleep(5);//waiting for bhand ready...
ros::NodeHandle axis_force_handle;
ros::Publisher axis_force_pub=axis_force_handle.advertise<id_data_msgs::ID_Data>("/notice",1);//advertise axis force to the topic named "/notice"
ros::Rate loop_rate(100);
//data receive test
//wake puck
wavePuck(0,8);
//tare
setPropertySlow(0,FTSENSORID,FTDATAPROPERTY,0,0);
TPCANMsg test_data;
//query the axis sensor.
test_data.ID=0x008;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x36;
notice_pub_sub notice_test;
id_data_msgs::ID_Data notice_data_pub;
while(ros::ok())
{
sys_count++;
if(sys_count%20==0)funCAN_Write(pcan_handle,&test_data);//query the axis sensor.
read_loop(pcan_handle,axis_force_pub,false,true);
bhand_axis_force_warning(¬ice_test,force_x_float,force_y_float,force_z_float,torque_x_float,torque_y_float,torque_z_float);
//HANDLE h,ros::Publisher pub, bool display_on,bool publish_on
if(close_hand_flag)
{
open_hand_flag=false;
bhand_position_control(bhand_close_pos*10000,bhand_close_pos*10000,bhand_close_pos*10000,0);//position: (0-2.5)*10 0000
sleep(2);
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=1;
notice_data_pub.data[0]=2;
notice_test.notice_pub_sub_pulisher(notice_data_pub);
close_hand_flag=false;
}
if(open_hand_flag)
{
close_hand_flag=false;
bhand_position_control((bhand_open_pos-2)*10000,(bhand_open_pos-2)*10000,(bhand_open_pos-2)*10000,0);//position: (0-2.5)*10 0000
sleep(2);
bhand_position_control(bhand_open_pos*10000,bhand_open_pos*10000,bhand_open_pos*10000,0);//position: (0-2.5)*10 0000
sleep(1);
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=1;
notice_data_pub.data[0]=2;
notice_test.notice_pub_sub_pulisher(notice_data_pub);
open_hand_flag=false;
ROS_INFO("Open hand successfully!\n");
}
notice_test.notice_sub_spinner(1);
loop_rate.sleep();
}
return 0;
}
int wavePuck(int bus,int id)
{
TPCANMsg msgOut;
DWORD err;
// Generate the outbound message
msgOut.ID = id;
msgOut.MSGTYPE = MSGTYPE_STANDARD;
msgOut.LEN = 4;
msgOut.DATA[0] = 0x85; // Set Status
msgOut.DATA[1] = 0x00;
msgOut.DATA[2] = 0x02; // Status = Ready
msgOut.DATA[3] = 0x00;
// Send the message
//err = CAN_Write( &msgOut );
err=funCAN_Write(pcan_handle,&msgOut);
sleep(1);
return(err);
}
int compile(
int property /** The property being compiled (use the enumerations in btcan.h) */,
long longVal /** The value to set the property to */,
unsigned char *data /** A pointer to a character buffer in which to build the data payload */,
int *dataLen /** A pointer to the total length of the data payload for this packet */,
int isForSafety /** A flag indicating whether this packet is destined for the safety circuit or not */)
{
int i;
// Check the property
//if(property > PROP_END)
//{
// syslog(LOG_ERR,"compile(): Invalid property = %d", property);
// return(1);
//}
/* Insert the property */
data[0] = property;
data[1] = 0; /* To align the values for the tater's DSP */
/* Append the value */
for (i = 2; i < 6; i++)
{
data[i] = (char)(longVal & 0x000000FF);
longVal >>= 8;
}
/* Record the proper data length */
*dataLen = 6; //(dataType[property] & 0x0007) + 2;
return (0);
}
int setPropertySlow(int bus, int id, int property, int verify, long value)
{
TPCANMsg msgOut, msgIn;
DWORD err;
int dataHeader, i;
long response;
//unsigned char data[8];
int len;
//syslog(LOG_ERR, "About to compile setProperty, property = %d", property);
// Compile 'set' packet
err = compile(property, value, msgOut.DATA, &len, 0);
// Generate the outbound message
msgOut.ID = id;
msgOut.MSGTYPE = MSGTYPE_STANDARD;
msgOut.LEN = len;
//syslog(LOG_ERR, "After compilation data[0] = %d", data[0]);
msgOut.DATA[0] |= 0x80; // Set the 'Set' bit
// Send the message
//CAN_Write( &msgOut );
funCAN_Write(pcan_handle,&msgOut);
//Sleep(250);
// BUG: This will not verify properties from groups of pucks
return(0);
}
int getPropertyFT(int bus,HANDLE h)
{
TPCANMsg msgOut, msgInOne, msgInTwo;
DWORD err;
// Generate the outbound message
msgOut.ID = FTSENSORID;
msgOut.MSGTYPE = MSGTYPE_STANDARD;
msgOut.LEN = 1;
msgOut.DATA[0] = FTDATAPROPERTY; // Get the ft data
}
void barett_hand_init()
{
TPCANMsg test_data;
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x05;
funCAN_Write(pcan_handle,&test_data);
}
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=6;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0xb2;
for(char j=1;j<6;j++)test_data.DATA[j]=0;
funCAN_Write(pcan_handle,&test_data);
}
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=6;
test_data.MSGTYPE=MSGTYPE_STANDARD;
for(char j=0;j<6;j++)test_data.DATA[j]=0;
test_data.DATA[0]=0xce;
test_data.DATA[2]=0xfa;
funCAN_Write(pcan_handle,&test_data);
}
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x00;
funCAN_Write(pcan_handle,&test_data);
}
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x5a;
funCAN_Write(pcan_handle,&test_data);
}
//query the bhand state.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=1;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x08;
funCAN_Write(pcan_handle,&test_data);
}
//init the bhand.
for(char i=0;i<=3;i++)
{
test_data.ID=0xcb+i;
test_data.LEN=4;
test_data.MSGTYPE=MSGTYPE_STANDARD;
test_data.DATA[0]=0x9d;
test_data.DATA[1]=0x00;
test_data.DATA[2]=0x0d;
test_data.DATA[3]=0x00;
funCAN_Write(pcan_handle,&test_data);
}
sleep(5);
ROS_INFO("BHand Init successfully!");
}
void bhand_position_control(long f1_pos,long f2_pos,long f3_pos,long sp_pos)
{
TPCANMsg CANMsg;
CANMsg.LEN = 6;
CANMsg.MSGTYPE =MSGTYPE_STANDARD;
CANMsg.DATA[0] = 0xba;
CANMsg.DATA[1] = 0;
CANMsg.DATA[2] = 0;
CANMsg.DATA[3] = 0;
CANMsg.DATA[4] = 0;
CANMsg.DATA[5] = 0;
CANMsg.DATA[2] = f1_pos & 0xff;
CANMsg.DATA[3] = (f1_pos>>8) & 0xff;
CANMsg.DATA[4] = (f1_pos>>16) & 0xff;
CANMsg.ID = 0xcb;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
CANMsg.DATA[2] = f2_pos & 0xff;
CANMsg.DATA[3] = (f2_pos>>8) & 0xff;
CANMsg.DATA[4] = (f2_pos>>16) & 0xff;
CANMsg.ID = 0xcc;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
CANMsg.DATA[2] = f3_pos & 0xff;
CANMsg.DATA[3] = (f3_pos>>8) & 0xff;
CANMsg.DATA[4] = (f3_pos>>16) & 0xff;
CANMsg.ID = 0xcd;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
CANMsg.DATA[2] = sp_pos & 0xff;
CANMsg.DATA[3] = (sp_pos>>8) & 0xff;
CANMsg.DATA[4] = (sp_pos>>16) & 0xff;
CANMsg.ID = 0xce;
funCAN_Write(pcan_handle,&CANMsg);
usleep(1000);
}
int read_loop(HANDLE h,ros::Publisher pub, bool display_on,bool publish_on)
{
TPCANRdMsg m;
__u32 status;
if (funLINUX_CAN_Read(h, &m)) {
//perror("receivetest: LINUX_CAN_Read()");
return errno;
}
if(m.Msg.ID == 0x50a || m.Msg.ID == 0x50b)
{
if (display_on)
print_message_ex(&m);
if (publish_on)
publish_forcedata(&m,pub,force_display_on);
// check if a CAN status is pending
if (m.Msg.MSGTYPE & MSGTYPE_STATUS) {
status = funCAN_Status(h);
if ((int)status < 0) {
errno = funnGetLastError();
perror("receivetest: CAN_Status()");
return errno;
}
printf("receivetest: pending CAN status 0x%04x read.\n",
(__u16)status);
}
}
return 0;
}
void publish_forcedata(TPCANRdMsg *mr,ros::Publisher axis_force_pub,bool display_on)
{
short int x_force,y_force,z_force;
short int torque_x,torque_y,torque_z;
if(0x50a==mr->Msg.ID )
{
x_force=mr->Msg.DATA[1];
x_force<<=8;
x_force|=mr->Msg.DATA[0];
y_force=mr->Msg.DATA[3];
y_force<<=8;
y_force|=mr->Msg.DATA[2];
z_force=mr->Msg.DATA[5];
z_force<<=8;
z_force|=mr->Msg.DATA[4];
force_x_float=x_force/256.0;
force_y_float=y_force/256.0;
force_z_float=z_force/256.0;
if(display_on) ROS_INFO(" Force x:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
}
if(0x50b==mr->Msg.ID )
{
torque_x=mr->Msg.DATA[1];
torque_x<<=8;
torque_x|=mr->Msg.DATA[0];
torque_y=mr->Msg.DATA[3];
torque_y<<=8;
torque_y|=mr->Msg.DATA[2];
torque_z=mr->Msg.DATA[5];
torque_z<<=8;
torque_z|=mr->Msg.DATA[4];
torque_x_float=torque_x/256.0;
torque_y_float=torque_y/256.0;
torque_z_float=torque_z/256.0;
if(display_on) ROS_INFO("Torque x:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
}
}
void bhand_axis_force_warning(notice_pub_sub *notice_test,float force_x,float force_y,float force_z,float torque_x,float torque_y,float torque_z)
{
id_data_msgs::ID_Data notice_data_pub;
if(fabs(force_x)>force_threshold || fabs(force_y)>force_threshold || fabs(force_z)>force_threshold)
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=13;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN(" Force threshhold:%8.4f BHAND COLLISION!",force_threshold);
//ROS_INFO(" Force x:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
if(torque_x<=left_torque_x_threshold )
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=12;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN("LEFT arm Axis force sensor detect BHAND COLLISION!");
usleep(10000);
}
if(torque_x>=right_torque_x_threshold )
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=11;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN("RIGHT Axis force sensor detect BHAND COLLISION!");
usleep(10000);
}
}
if(fabs(torque_x)>torque_threshold || fabs(torque_y)>torque_threshold || fabs(torque_z)>torque_threshold)
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=13;
//notice_test->notice_pub_sub_pulisher(notice_data_pub);
//ROS_WARN("Torque threshhold:%8.4f BHAND COLLISION!",torque_threshold);
//ROS_INFO("Torque x:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
}
if(fabs(torque_x)>torque_danger || fabs(torque_y)>torque_danger || fabs(torque_z)>torque_danger || fabs(force_x)>force_danger || fabs(force_y)>force_danger || fabs(force_z)>force_danger)
{
notice_data_clear(¬ice_data_pub);
notice_data_pub.id=5;
notice_data_pub.data[0]=14;
notice_test->notice_pub_sub_pulisher(notice_data_pub);
ROS_ERROR("Axis force sensor detect BHAND DANGER!");
ROS_INFO(" Force x:%8.4f, y:%8.4f, z:%8.4f",force_x_float,force_y_float,force_z_float);
ROS_INFO("Torque x:%8.4f, y:%8.4f, z:%8.4f",torque_x_float,torque_y_float,torque_z_float);
}
}
void print_message(TPCANMsg *m)
{
int i;
//print RTR, 11 or 29, CAN-Id and datalength
printf("receivetest: %c %c 0x%08x %1d ",
((m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm') -
((m->MSGTYPE ) ? 0x20 : 0),
(m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',
m->ID,
m->LEN);
//don't print any telegram contents for remote frames
if (!(m->MSGTYPE & MSGTYPE_RTR))
for (i = 0; i < m->LEN; i++)
printf("%02x ", m->DATA[i]);
//printf("%3d ", m->DATA[i]);//decimal format print.
printf("\n");
}
void print_message_ex(TPCANRdMsg *mr)
{
printf("%u.%3u ", mr->dwTime, mr->wUsec);
print_message(&mr->Msg);
}
// exit handler
void do_exit(void *file,HANDLE h,int error)
{
//Must close h handle firstly,then close file using dlclose
if (h) {
funCAN_Close(h);
}
printf("\nCAN Bus test: finished (%d).\n\n", error);
//after call the target function in ELF object,close it using dlclose
dlclose(file);
exit(error);
}
// the signal handler for manual break Ctrl-C
void signal_handler(int signal)
{
do_exit(libm_handle,pcan_handle,0);
}
// what has to be done at program start
void init()
{
/* install signal handlers */
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
}
void notice_data_clear(id_data_msgs::ID_Data *test)
{
test->id=0;
for(int i=0;i<8;i++) test->data[i]=0;
}
P.S. 文中或代碼中有關/notice話題的部分詳見博客內該博文http://blog.csdn.net/hookie1990/article/details/54138234