標(biāo)題: bascom avr利用can通信控制伺服電機(jī)上使能 [打印本頁]
作者: 紅日888    時(shí)間: 2021-6-20 18:36
標(biāo)題: bascom avr利用can通信控制伺服電機(jī)上使能
本帖最后由 紅日888 于 2021-6-20 21:34 編輯

'此列子為串口轉(zhuǎn)can通信控制伺服電機(jī)上使能的列子
$regfile = "m128def.dat"                                    '單片機(jī)型號(hào)頭文件
$crystal = 8000000                                          '晶振頻率
$baud = 19200
$hwstack = 256
$swstack = 256
$framesize = 256

Declare Sub Send(byval Str_code As String)

Declare Function Can_data(byval Std_mode As Integer , Byval Data_length As Integer , _
                           Byval Func_code As String , Byval Node_id As String , _
                           Byval Str_data As String) As String
Declare Sub Set_enable(byval Node_id As String , Byval Ctrl_mode As Integer , Byval Value As Integer)

Config Portd = Output
Const Std_frame = 1                                         '標(biāo)準(zhǔn)幀
Const Ext_frame = 0                                         '擴(kuò)展幀
Const Position_mode = 0                                     '位置模式
Const Speed_mode = 1                                        '速度模式

Dim Can_code As String * 26

Portd = &HFF
Wait 1
'1號(hào)伺服位置模式上使能
Set_enable "01" , Position_mode , 1
Portd.3 = 0
Wait 1
'2號(hào)伺服速度模式上使能
Set_enable "02" , Speed_mode , 1
Portd.4 = 0
Wait 10

'1號(hào)伺服掉使能
Set_enable "01" , Position_mode , 0
Portd.3 = 1
Wait 1
'2號(hào)伺服掉使能
Set_enable "02" , Speed_mode , 0
Portd.4 = 1
Wait 1

End

'按值發(fā)送
Sub Send(byval Str_code As String )
    Local I1 As Integer , Mystr As String * 4 , Mybyte As Byte , Length As Integer
    Length = Len(str_code)
    For I1 = 1 To Length Step 2
        Mystr = Mid(str_code , I1 , 2)
        Mybyte = Hexval(mystr)
        Printbin Mybyte
    Next
End Sub

'Can幀數(shù)據(jù)Std_mode=1為標(biāo)準(zhǔn)幀=0為擴(kuò)展幀，
'Data_length為數(shù)據(jù)區(qū)長(zhǎng)度最大8字節(jié)，
'Func_code為功能碼一個(gè)字節(jié)，
'Node_id為節(jié)點(diǎn)ID一個(gè)字節(jié)，
'Str_data數(shù)據(jù)區(qū)十六進(jìn)制字符串，最大8個(gè)字節(jié)
Function Can_data(byval Std_mode As Integer , Byval Data_length As Integer , _
                  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)                     '標(biāo)準(zhǔn)幀的第一個(gè)字符為0第二個(gè)字符為數(shù)據(jù)區(qū)的字節(jié)數(shù)（最大8字節(jié)）
   Case Is = Ext_frame
      Str_code = "8" + Str(data_length)                     '擴(kuò)展幀的第一個(gè)字符為8第二個(gè)字符為數(shù)據(jù)區(qū)的字節(jié)數(shù)（最大8字節(jié)）
   End Select

   Str_code = Str_code + "0000"

   If Len(func_code) = 1 Then
      Str_code = Str_code + "0"
   End If
   Str_code = Str_code + Func_code

   If Len(node_id) = 1 Then Str_code = Str_code + "0"
   Str_code = Str_code + Node_id

   Count = Len(str_data)
   Count = 16 - Count
   Str_code = Str_code + Str_data
   For I = 1 To Count
      Str_code = Str_code + "0"
   Next I
   Can_data = Str_code
End Function

'伺服驅(qū)動(dòng)器使能Node_id為驅(qū)動(dòng)器節(jié)點(diǎn)ID，
'Ctrl_mode有兩種模式，位置模式Position_mode = 0和速度模式 Speed_mode = 1
'Value值設(shè)置非0為上使能，設(shè)置為0為掉使能
Sub Set_enable(byval Node_id As String , Byval Ctrl_mode As Integer , Byval Value As Integer)
   If Value <> 0 Then
      '上使能
      '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報(bào)文
      Send Can_code                                         '發(fā)數(shù)據(jù)
      Waitms 5
      Select Case Ctrl_mode
      Case Is = Position_mode
        '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報(bào)文
        Send Can_code                                       '發(fā)數(shù)據(jù)
        Waitms 5
      Case Is = Speed_mode
        '01 00 00 03 0A 03 00 00 00 00 00 00 00    '位置模式
        Can_code = Can_data(std_frame , 1 , "03" , Node_id , "03" )       '獲得完整can報(bào)文
        Send Can_code                                       '發(fā)數(shù)據(jù)
        Waitms 5
      End Select
      '02 00 00 02 0A 06 00 00 00 00 00 00 00   '始能第一步
      Can_code = Can_data(std_frame , 2 , "02" , Node_id , "0600" )       '獲得完整can報(bào)文
      Send Can_code                                         '發(fā)數(shù)據(jù)
      Waitms 5
      '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報(bào)文
      Send Can_code                                         '發(fā)數(shù)據(jù)
      Waitms 5
      '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報(bào)文
      Send Can_code                                         '發(fā)數(shù)據(jù)
      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報(bào)文
      Send Can_code                                         '發(fā)數(shù)據(jù)
   End If
End Sub





歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1