串口转can通信控制伺服电机

ssdgh

$regfile = "m128def.dat"                                    '单片机型号头文件
. P" \, Y+ A4 |: |6 r$crystal = 8000000                                          '晶振频率
  I) h2 `6 d4 {5 V0 u7 ^$baud = 19200
$hwstack = 256
$swstack = 256
: D) |" \  E; y$ `, k  c$framesize = 256

. @6 h& V4 e: C+ `+ a! M* hDeclare Sub Send(byval Str_code As String)
' U9 d  p, ^. t5 k; O1 r; `
# D% ]  \! u6 W, r. T  f  uDeclare Function Can_data(byval Std_mode As Integer , Byval Data_length As Integer , _
                           Byval Func_code As String , Byval Node_id As String , _
1 u) X: q7 D7 b1 l% K# l                           Byval Str_data As String) As String
$ _) n8 v% C! n4 {, PDeclare Sub Set_enable(byval Node_id As String , Byval Ctrl_mode As Integer , Byval Value As Integer)
# c$ l  \7 V7 F8 Z
Config Portd = Output
" O2 K2 f/ E7 S. \6 c$ tConst Std_frame = 1                                         '标准帧
+ E2 G$ M! ~; oConst Ext_frame = 0                                         '扩展帧
& Q. t: r/ n$ a1 H7 k+ w' R! zConst Position_mode = 0                                     '位置模式
4 o6 u% ~7 V! IConst Speed_mode = 1                                        '速度模式
! a. h# V5 i/ D& X' i! M2 i, Q
Dim Can_code As String * 26

% |! L7 o+ L! Q  y! a5 Z) L. d9 y3 JPortd = &HFF
Wait 1
'1号伺服位置模式上使能
+ C9 F- m" j& E, f3 f4 }Set_enable "01" , Position_mode , 1
  Y( b# w6 M5 O5 j* L  O) VPortd.3 = 0
( T) V1 @9 L; b0 ?' w$ iWait 1
) Q' E# z% ]; o1 a'2号伺服速度模式上使能
* P3 c; A* U4 ~+ B- J8 WSet_enable "02" , Speed_mode , 1
Portd.4 = 0
Wait 10

'1号伺服掉使能
Set_enable "01" , Position_mode , 0
Portd.3 = 1
Wait 1
'2号伺服掉使能
Set_enable "02" , Speed_mode , 0
Portd.4 = 1
- u- F& g2 _  w: _Wait 1
0 j5 _9 t6 t3 `  m1 W6 U* {
. {7 l2 r% K( t% p# X( _4 rEnd

: m* w, k& ?1 _, z'按值发送
/ ]' x3 O2 K: p2 R# y: eSub Send(byval Str_code As String )
    Local I1 As Integer , Mystr As String * 4 , Mybyte As Byte , Length As Integer
    Length = Len(str_code)
6 Q9 s; f% L3 {6 r& ]+ L    For I1 = 1 To Length Step 2
        Mystr = Mid(str_code , I1 , 2)
( d  p. T  t  J4 m" j4 R& t        Mybyte = Hexval(mystr)
        Printbin Mybyte
    Next
End Sub
. T4 B& v' m# l" |2 S/ P* U
'Can帧数据Std_mode=1为标准帧=0为扩展帧，
4 ^8 E, J" A- e4 `$ r! \# y! g6 n8 t'Data_length为数据区长度最大8字节，
  E/ R" m" t5 N. m'Func_code为功能码一个字节，
2 j& @7 Z+ I1 \" h'Node_id为节点ID一个字节，
'Str_data数据区十六进制字符串，最大8个字节
Function Can_data(byval Std_mode As Integer , Byval Data_length As Integer , _
5 g2 o) s$ {6 |" M0 t1 Y                  Byval Func_code As String , Byval Node_id As String , _
                  Byval Str_data As String) As String

   Local Count As Integer , Str_code As String * 26 , I As Integer

   Select Case Std_mode
   Case Is = Std_frame
      Str_code = "0" + Str(data_length)                     '标准帧的第一个字符为0第二个字符为数据区的字节数（最大8字节）
   Case Is = Ext_frame
      Str_code = "8" + Str(data_length)                     '扩展帧的第一个字符为8第二个字符为数据区的字节数（最大8字节）
   End Select

   Str_code = Str_code + "0000"

& {1 Y$ k0 o- g) `' x9 }1 {   If Len(func_code) = 1 Then
      Str_code = Str_code + "0"
   End If
# s- ?1 p$ ^  K( U+ w   Str_code = Str_code + Func_code

. e6 C0 J3 g7 T1 @   If Len(node_id) = 1 Then Str_code = Str_code + "0"
   Str_code = Str_code + Node_id

   Count = Len(str_data)
   Count = 16 - Count
1 o) X: U3 L* H' D& B) _   Str_code = Str_code + Str_data
   For I = 1 To Count
6 u  v* R: ~4 y      Str_code = Str_code + "0"
! \( K+ Q4 W7 u  k   Next I
   Can_data = Str_code
End Function

2 \* c' f4 g- m0 G8 _6 c'伺服驱动器使能Node_id为驱动器节点ID，
'Ctrl_mode有两种模式，位置模式Position_mode = 0和速度模式 Speed_mode = 1
'Value值设置非0为上使能，设置为0为掉使能
Sub Set_enable(byval Node_id As String , Byval Ctrl_mode As Integer , Byval Value As Integer)
   If Value <> 0 Then
; `# M- a! h' c      '上使能
      '02 00 00 02 0A 00 00 00 00 00 00 00 00   '控制字清0
      Can_code = Can_data(std_frame , 2 , "02" , Node_id , "0000" )       '获得完整can报文
      Send Can_code                                         '发数据
      Waitms 5
4 w0 t, d( b0 W; D      Select Case Ctrl_mode
      Case Is = Position_mode
7 B1 g- s; S0 f" S! h        '01 00 00 03 0A 01 00 00 00 00 00 00 00    '速度模式
        Can_code = Can_data(std_frame , 1 , "03" , Node_id , "01" )       '获得完整can报文
! N" N8 B$ c1 P3 r' h0 r9 L; o        Send Can_code                                       '发数据
        Waitms 5
: [) n! t0 i+ U" L4 T2 Q      Case Is = Speed_mode
        '01 00 00 03 0A 03 00 00 00 00 00 00 00    '位置模式
/ a/ J( B; u+ O" K% Z        Can_code = Can_data(std_frame , 1 , "03" , Node_id , "03" )       '获得完整can报文
        Send Can_code                                       '发数据
: i& Z- j; c8 e* b        Waitms 5
7 ^2 g2 D3 Z, R0 j: C( ?& _  a      End Select
& V9 v  |8 H$ |# u      '02 00 00 02 0A 06 00 00 00 00 00 00 00   '始能第一步
3 |; y$ p% `1 {, l+ j      Can_code = Can_data(std_frame , 2 , "02" , Node_id , "0600" )       '获得完整can报文
      Send Can_code                                         '发数据
      Waitms 5
1 Q/ k" c, n6 @+ k9 O, ]      '02 00 00 02 0A 07 00 00 00 00 00 00 00   '始能第二步
      Can_code = Can_data(std_frame , 2 , "02" , Node_id , "0700" )       '获得完整can报文
      Send Can_code                                         '发数据
# h( j& ^0 Y. G6 ]. D      Waitms 5
; ]! U+ [' z+ B- B* a% o& `      '02 00 00 02 0A 0F 00 00 00 00 00 00 00   '始能第三步
      Can_code = Can_data(std_frame , 2 , "02" , Node_id , "0F00" )       '获得完整can报文
2 b0 g, z! B2 c+ C7 ^      Send Can_code                                         '发数据
      Waitms 5
   Else
      '掉使能02 00 00 02 0A 05 00 00 00 00 00 00 00
      Can_code = Can_data(std_frame , 2 , "02" , Node_id , "0500" )       '获得完整can报文
      Send Can_code                                         '发数据
! r2 u" h- z7 b/ \( D   End If
End Sub

nuiga

感觉清零有些繁琐

RNGxiaohu

学习了   感谢分享