Attribute VB_Name = "Module1"


Public Enum movitionModule
    respM
    servoM
    banlanceM
    overshootM
End Enum
Public brake1 As Boolean, brake2 As Boolean


'************改進的PID算法，遇限消弱積分和不完全微分***************************
Public Sub improvedPID()
    Dim singleDU As Single
    If Abs(form1.singleU) < 100 Or (form1.singleU > 100 And form1.singleEP < 0) Or (form1.singleU < -100 And form1.singleEP > 0) Then
        form1.singleSumEP = form1.singleSumEP + form1.singleEP
    End If
    singleDU = 0.8 * form1.singleU + (20 * form1.HScrollPD.Value / form1.Timer2.Interval) * (form1.singleEP - form1.singleLastEP)
    form1.singleU = form1.HScrollpp.Value * (singleDU + form1.singleEP + (1 / form1.HScrollPI.Value) * form1.singleSumEP) / 50
    form1.singleLastEPP = form1.singleLastEP
    form1.singleLastEP = form1.singleEP
    
End Sub
'*************判斷系統當前的運動模式********************************
Public Function moduleIden(ByVal SV As Single) As movitionModule
    
    If (form1.singleEP * form1.singleLastEP >= 0) And (form1.singleLastEP * form1.singleLastEPP) > 0 Then
        If form1.singleEP >= 0 Then
            If form1.singleEP < form1.singleLastEP And SV > 0 Then moduleIden = respM
        Else
            If form1.singleEP > form1.singleLastEP And SV < 0 Then moduleIden = respM
        End If
    ElseIf form1.singleEP * SV < 0 And Abs(form1.singleEP) >= 1 Then
         moduleIden = overshootM
    'Else
        'If Abs(form1.singleLastEP) < 2 And Abs(form1.singleEP) < 2 Then
            'moduleIden = balanceM
        'End If
    End If
       
    If Abs(form1.singleLastEPP) < 3.5 And Abs(form1.singleLastEP) < 3.5 And form1.lastV * form1.lastVV > 0 Then
        'moduleIden = servoM
    End If
End Function

'**************實時測速函數，實時測得電機速度以作位置預測之用*********************
Public Function getVelocity() As Single
    Dim answer As String
    Dim i As Integer, temp As Integer
    Dim k
    answer = ""
    form1.MSComm1.Output = "?p" & Chr(13)
    
    Do
        answer = answer & form1.MSComm1.Input
        i = InStr(1, answer, "?p")
    Loop Until i > 0
    
    k = Mid(answer, i + 3, 1)
    If k >= "A" And k <= "F" Then
        temp = 16 * (Asc(k) - 55)
    ElseIf k >= "0" And k <= "9" Then
        temp = 16 * Val(k)
    Else
        getVelocity = lastV
        MsgBox "Invalid communiction!", vbCritical, "Erro"
        Exit Function
    End If
    k = Mid(answer, i + 4, 1)
    If k >= "A" And k <= "F" Then
        temp = temp + (Asc(k) - 55)
    ElseIf k >= "0" And k <= "9" Then
        temp = temp + Val(k)
    Else
        getVelocity = form1.lastV
        MsgBox "Invalid communiction!", vbCritical, "Erro"
        Exit Function
    End If
    If temp <= 127 Then
        form1.txtPitch.Text = CStr(temp)
    Else
        form1.txtPitch.Text = CStr(temp - 256)
        temp = temp - 256
    End If
    form1.txtPitch.Text = temp
    getVelocity = temp
End Function
'*******************加了模式識別的控制算法************************
Public Sub getU()
    Dim coeff As Single
    Dim sglV As Single
    Dim module As movitionModule
    If Abs(Val(form1.txtRoll.Text)) > 55 Then
        form1.singleU = 0
        Exit Sub
    End If
    sglV = getVelocity()
    coeff = CSng(form1.Timer2.Interval) * 0.00078
    form1.singleEP = form1.singleEP - 0.5 * (sglV + form1.lastV) * coeff
    form1.lastVV = form1.lastV
    form1.lastV = sglV
    If Abs(form1.singleLastEP) < 1 And Abs(form1.singleLastEPP) < 1 And Abs(form1.singleEP) >= 2 Then
        If form1.singleSumEP > 0 Then
            If form1.singleEP < 0 Then form1.singleSumEP = form1.singleSumEP + 5 * form1.singleEP
        ElseIf form1.singleSumEP = 0 Then
            form1.singleSumEP = form1.singleSumEP + 4 * form1.singleEP
        Else
            If form1.singleEP > 0 Then form1.singleSumEP = form1.singleSumEP + 5 * form1.singleEP
        End If
    End If
    module = moduleIden(sglV)
    'If module = respM Then
        'If Abs(form1.singleEP) > 5 Then
            'brake1 = True
            'brake2 = True
            'Call improvedPID
        'Else
            'If brake1 = True Then
                'form1.singleU = 0
                'brake1 = False
            'ElseIf brake2 = True Then
                'form1.singleU = 0
                'brake2 = False
            'Else
                'Call improvedPID
            'End If
        'End If
    'ElseIf module = servoM Then
        'If Abs(form1.singleEP) > 4 Then
            'If form1.singleEP > 0 Then
                'form1.singleU = 100
            'Else
                'form1.singleU = -100
                'form1.singleSumEP = form1.singleEP * 0.5
            'End If
        'Else
            'Call improvedPID
        'End If
    'ElseIf module = banlanceM Then
        'form1.singleU = 0
        'form1.singleSumEP = 0
    If module = overshootM Then
        form1.singleSumEP = 5 * form1.singleEP
        'form1.singleU = form1.singleEP * 20
        Call improvedPID
    Else
        Call improvedPID
    End If
    
    'Call improvedPID
    'form1.singleU = initiCoeff * form1.singleU
    If Abs(form1.singleU) <= 2 Then form1.singleU = 0
    If form1.singleU > 120 Then form1.singleU = 120
    If form1.singleU < -120 Then form1.singleU = -120
End Sub