📄 compass_communication.frm
字号:
End
Begin VB.TextBox txtPitch
Alignment = 1 'Right Justify
Height = 270
Left = 7680
TabIndex = 5
Text = "0"
Top = 1320
Width = 1575
End
Begin VB.TextBox txtRoll
Alignment = 1 'Right Justify
Height = 270
Left = 3960
TabIndex = 3
Text = "0"
Top = 1320
Width = 1575
End
Begin VB.TextBox txtYaw
Alignment = 1 'Right Justify
Height = 270
Left = 240
TabIndex = 1
Text = "0"
Top = 1320
Width = 1575
End
Begin MSCommLib.MSComm MSComm1
Left = 9360
Top = 7680
_ExtentX = 1005
_ExtentY = 1005
_Version = 393216
DTREnable = -1 'True
RThreshold = 1
BaudRate = 19200
End
Begin VB.Label Label30
Caption = "270"
Height = 255
Left = 4680
TabIndex = 59
Top = 3120
Width = 495
End
Begin VB.Label Label29
Caption = "180"
Height = 255
Left = 3480
TabIndex = 58
Top = 4080
Width = 375
End
Begin VB.Label Label28
Caption = "90"
Height = 255
Left = 4680
TabIndex = 57
Top = 5280
Width = 375
End
Begin VB.Label Label27
Caption = "0"
Height = 255
Left = 5880
TabIndex = 56
Top = 4080
Width = 375
End
Begin VB.Shape Shape3
Height = 495
Left = 4320
Top = 3840
Width = 1215
End
Begin VB.Label Label13
Caption = "VD"
Height = 375
Left = 6960
TabIndex = 21
Top = 4440
Width = 495
End
Begin VB.Label Label12
Caption = "VI:"
Height = 375
Left = 6960
TabIndex = 20
Top = 4080
Width = 615
End
Begin VB.Label Label11
Caption = "VP:"
Height = 375
Left = 6960
TabIndex = 19
Top = 3720
Width = 615
End
Begin VB.Label Label9
Caption = "PD"
Height = 495
Left = 120
TabIndex = 15
Top = 4440
Width = 615
End
Begin VB.Label Label8
Caption = "PI"
Height = 375
Left = 120
TabIndex = 14
Top = 4080
Width = 495
End
Begin VB.Label Label7
Caption = "PP:"
Height = 375
Left = 120
TabIndex = 13
Top = 3720
Width = 615
End
Begin VB.Label Label10
Caption = "controlU"
Height = 375
Left = 3120
TabIndex = 12
Top = 120
Width = 1335
End
Begin VB.Label Label6
Caption = "Angle to rotate:"
Height = 255
Left = 240
TabIndex = 10
Top = 2040
Width = 2535
End
Begin VB.Label Label5
Caption = "Time:"
Height = 255
Left = 240
TabIndex = 7
Top = 120
Width = 735
End
Begin VB.Label Label4
Caption = "单位:度"
Height = 495
Left = 7800
TabIndex = 6
Top = 480
Width = 855
End
Begin VB.Label Label3
Caption = "Pitch:"
Height = 255
Left = 6120
TabIndex = 4
Top = 960
Width = 1455
End
Begin VB.Label Label2
Caption = "Roll:"
Height = 255
Left = 3240
TabIndex = 2
Top = 960
Width = 1455
End
Begin VB.Label Label1
BackColor = &H80000004&
Caption = "Yaw:"
Height = 255
Left = 240
TabIndex = 0
Top = 960
Width = 1455
End
End
Attribute VB_Name = "Form1"
Attribute VB_GlobalNameSpace = False
Attribute VB_Creatable = False
Attribute VB_PredeclaredId = True
Attribute VB_Exposed = False
Option Explicit
Private Declare Function timeGetTime Lib "winmm.dll" () As Long
Public Yaw, Pitch, Roll, lastYaw As Single
Public singleEP, singleSumEP, singleSpeed, singleLastEP As Single
Public singleV, singleEV, singleSumEV, singleLastEV, singleU As Single
Private strReceiveBuff As String
Private intTime, intTime0 As Integer
Private Sub Form_Load()
If MSComm3.PortOpen = False Then
MSComm3.PortOpen = True
End If
If MSComm1.PortOpen = False Then
MSComm1.PortOpen = True
End If
strReceiveBuff = ""
singleLastEP = 0
singleEP = 0
singleSpeed = 0
singleSumEP = 0
lastYaw = 0
singleSumEV = 0
singleLastEV = 0
singleU = 0
intTime0 = 0
intTime = 125
Timer1.Enabled = True
End Sub
Private Sub Form_Unload(Cancel As Integer)
If MSComm1.PortOpen = True Then MSComm1.PortOpen = False
If MSComm3.PortOpen = True Then MSComm3.PortOpen = False
End Sub
Private Sub HScroll1_change()
singleEP = HScroll1.Value - Val(txtYaw.Text)
If singleEP > 180 Then singleEP = singleEP - 360
If singleEP < -180 Then singleEP = singleEP + 360
Text1.Text = CStr(HScroll1.Value)
Text2.Text = CStr(CInt(Yaw))
Text3.Text = CStr(CInt(singleEP))
Label6.Caption = "Angle to Rotate:" & CStr(HScroll1.Value)
Line1.X2 = 840 * Cos(HScroll1.Value * 6.283 / 360) + 840
Line1.Y2 = 840 * Sin(HScroll1.Value * 6.283 / 360) + 840
End Sub
Private Sub HScroll1_Scroll()
singleEP = HScroll1.Value - Val(txtYaw.Text)
If singleEP > 180 Then singleEP = singleEP - 360
If singleEP < -180 Then singleEP = singleEP + 360
Text1.Text = CStr(HScroll1.Value)
Text2.Text = CStr(CInt(Yaw))
Text3.Text = CStr(CInt(singleEP))
Label6.Caption = "Angle to Rotate:" & CStr(HScroll1.Value)
Line1.X2 = 840 * Cos(HScroll1.Value * 6.283 / 360) + 840
Line1.Y2 = 840 * Sin(HScroll1.Value * 6.283 / 360) + 840
End Sub
Private Sub HScrollPD_Change()
Text9.Text = CStr(HScrollPD.Value / 10)
End Sub
Private Sub HScrollPI_Change()
Text8.Text = CStr(HScrollPI.Value)
singleSumEP = 0
End Sub
Private Sub HScrollpp_Change()
Text7.Text = CStr(HScrollpp.Value / 5)
End Sub
Private Sub HScrollVP_Change()
Text10.Text = CStr(HScrollVP.Value / 5)
End Sub
Private Sub HScrollVI_Change()
Text11.Text = CStr(HScrollVI.Value)
singleSumEV = 0
End Sub
Private Sub HScrollVD_Change()
Text12.Text = CStr(HScrollVD.Value / 10)
End Sub
Private Sub Speed_Change()
If Speed.Value > 0 Then
If Speed.Value < 16 Then
MSComm3.Output = "!A0" & Hex(Abs(Speed.Value)) & Chr(13)
Else
MSComm3.Output = "!A" & Hex(Abs(Speed.Value)) & Chr(13)
End If
Else
If Speed.Value > -16 Then
MSComm3.Output = "!a0" & Hex(Abs(Speed.Value)) & Chr(13)
Else
MSComm3.Output = "!a" & Hex(Abs(Speed.Value)) & Chr(13)
End If
End If
End Sub
Private Sub MSComm1_OnComm()
Dim strBuff, strBuff1 As String
Dim i, j, k As Integer
strBuff = MSComm1.Input
k = Len(strBuff)
For i = 1 To k
strBuff1 = Mid(strBuff, i, 1)
Select Case strBuff1
Case Chr(13)
Case Chr(10)
'********** receive one Chr **************
Doevent
'***************** **********
strReceiveBuff = ""
Case Else
strReceiveBuff = strReceiveBuff & strBuff1
End Select
Next
End Sub
Private Sub Doevent()
Dim i, j, k, ilen As Integer
Dim str1 As String
Dim lngT As Long
ilen = Len(Trim(strReceiveBuff))
i = InStr(1, strReceiveBuff, ",")
j = InStr(i + 1, strReceiveBuff, ",")
If (i = 0 Or j = 0) Then Exit Sub
str1 = Mid(strReceiveBuff, 1, i - 1)
Yaw = Val(str1)
Line2.X2 = 840 * Cos(Yaw * 6.283 / 360) + 840
Line2.Y2 = 840 * Sin(Yaw * 6.283 / 360) + 840
str1 = Mid(strReceiveBuff, i + 1, j - i - 1)
Pitch = Val(str1)
str1 = Mid(strReceiveBuff, j + 1, ilen)
Roll = Val(str1)
txtYaw.Text = Format(Yaw, "0.0")
txtPitch.Text = Format(Pitch, "0.0")
txtRoll.Text = Format(Roll, "0.0")
lngT = timeGetTime
txtTimeBox.Text = CStr(lngT)
End Sub
Private Sub Timer1_Timer()
Dim i As Integer
singleEP = singleEP - singleU * Timer1.Interval * 0.002
singleSpeed = PIDP(singleEP, HScrollpp.Value, HScrollPI.Value, HScrollPD.Value)
Text4.Text = CStr(CInt(singleSpeed))
singleLastEP = singleEP
singleEV = singleSpeed - singleV
Text6.Text = CStr(CInt(singleEV))
singleU = PIDV(singleEV, HScrollVP.Value, HScrollVI.Value, HScrollVD.Value)
singleLastEV = singleEV
'*******Forward feed*********
If Abs(singleEP) < 2 Then
i = 2
ElseIf Abs(singleEP) < 10 Then
i = 1
ElseIf Abs(singleEP) < 30 Then
i = 0.5
ElseIf Abs(singleEP) < 90 Then
i = 0.1
Else
i = 0.05
End If
singleU = singleU + singleEP * i
'****************************
If Abs(singleEP) > 6 Then
If singleU > 0 Then singleU = singleU + 15
If singleU < 0 Then singleU = singleU - 15
End If
If singleU > 100 Then singleU = 100
If singleU < -100 Then singleU = -100
Label10.Caption = -CInt(singleU)
Speed.Value = -CInt(singleU)
End Sub
Private Sub txtYaw_Change()
singleEP = HScroll1.Value - Val(txtYaw.Text)
If singleEP > 180 Then singleEP = singleEP - 360
If singleEP < -180 Then singleEP = singleEP + 360
Text1.Text = CStr(HScroll1.Value)
Text2.Text = CStr(CInt(Yaw))
Text3.Text = CStr(CInt(singleEP))
singleV = ((Val(txtYaw.Text) - lastYaw) \ 1) * 8
If Abs(singleV) > 300 Then
If singleV > 0 Then singleV = 300 Else singleV = -300
End If
Text5.Text = CStr(CInt(singleV))
lastYaw = Val(txtYaw.Text)
If Abs(singleEP) <= 20 Then
singleSumEP = singleSumEP + singleEP
End If
If Abs(singleEV) <= 20 Then
singleSumEV = singleSumEV + singleEV
End If
Text13.Text = CStr(CInt(singleSumEV))
End Sub
Private Function PIDP(ByVal E As Single, ByVal P As Integer, ByVal Ti As Integer, ByVal Td As Integer) As Single
PIDP = (P / 5) * (E + singleSumEP / Ti + Td / 10 * (E - singleLastEP))
End Function
Private Function PIDV(ByVal E As Single, ByVal P As Integer, ByVal Ti As Integer, ByVal Td As Integer) As Single
PIDV = (P / 5) * (E + singleSumEV / Ti + Td / 10 * (E - singleLastEV))
End Function
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -