[Bascom] Quadrocopter i problemy z pid i sposoby jego wykorzystania.

[zuba1]

Witam. Od pewnego czasu mam problem z moim quadrocopterem. Problem jest prosty. Przygotowuję dane o kącie wychylenia z filtru Kalmana, potem łączę go z danymi o chcianym wychyleniu i dodaję wartość do obecnej mocy silników. Potem ta wartość trafia do PID. PID stara się osiągnąć samą moc silników. Ale ta koncepcja jest błędna. Więc wymyśliłem inne rozwiązanie. Na PID podaję samą wartość wychylenia. PID stara się osiągnąć 0. Potem ta wartość z PIDa dodawana jest z mocą silników. Może mię ktoś nakierować jak ten problem rozwiązać?? Pid w obydwu przypadkach stroiłem metodą zieglera-nicholsa. Niestety nieskutecznie. 😐 . Podaję fragmet kodu z pierwszej koncepcji wraz z całym blokiem sterowania i nadawania kierunku lotu. Wszelka pomoc jest mile widziana. Pozdrawiam.

'1  2  3
'
'4  0  6
'
'7  8  9
'baza ruchów
If Kierunek = 0 Then
Xx = 0
Yy = 0
End If

If Kierunek = 1 Then
Xx = 4
Yy = 4
End If

If Kierunek = 2 Then
Xx = 5
Yy = 0
End If

If Kierunek = 3 Then
Xx = 4
Yy = -4
End If

If Kierunek = 4 Then
Xx = 0
Yy = 5
End If

If Kierunek = 6 Then
Xx = 0
Yy = -5
End If

If Kierunek = 7 Then
Xx = -4
Yy = 4
End If

If Kierunek = 8 Then
Xx = -5
Yy = 0
End If

If Kierunek = 9 Then
Xx = -4
Yy = -4
End If


'własciwe sterowanie silnikami i bloki operacyjne dla silników i pid

Angle_xx = Kat_x + Xx
Angle_yy = Kat_y + Yy

'wstępne dane do filtra pid
Pv1 = Moc + Angle_xx
Pv2 = Moc + Angle_yy
Pv3 = Moc - Angle_xx
Pv4 = Moc - Angle_yy
Spx1 = Moc
Spx2 = Moc
Spx3 = Moc
Spx4 = Moc


If Faza_lotu > 0 Then
Error1 = Spx1 - Pv1                                         ' Obliczanie uchybu
Error1 = Error1 * -1                                        ' Inwersja uchybu
Sum_error1 = Sum_error1 + Error1                            ' Suma uchybów
Iterm1 = Ki1 * Sum_error1                                   ' Integrated CV part (CAŁKUJĄCY)

     ' -------- First time startup
     If First_execution1 < 2 Then
        If First_execution1 = 0 Then
           Sum_error1 = 40 / Ki1
           First_execution1 = 1
           Initial_error1 = Error1
        End If
        Pterm1 = 0
        Dterm1 = 0
        If Initial_error1 > 0 And Error1 < 0 Then
           First_execution1 = 2
           Last_pv1 = Pv1
        End If
        If Initial_error1 < 0 And Error1 > 0 Then
           First_execution1 = 2
           Last_pv1 = Pv1
        End If
        Last_sp1 = Spx1

     ' -------- Normal calculation loop
     Else
        D_pv1 = Last_pv1 - Pv1
        Last_pv1 = Pv1
        Dterm1 = Kd1 * D_pv1                               ' Derivated CV part
        If Spx1 = Last_sp1 Then
           ' -------- Normal loop when setpoint not changed
           Pterm1 = Kp1 * Error1                           ' Proportional CV part
           ' -------- Loop when setpoint changed
           Else
           Pterm1 = 0
           Dterm1 = 0
           If Spx1 > Last_sp1 And Pv1 > Spx1 Then
              Last_sp1 = Spx1
              Last_pv1 = Pv1
           End If
           If Spx1 < Last_sp1 And Pv1 < Spx1 Then
              Last_sp1 = Spx1
              Last_pv1 = Pv1
           End If
        End If                                             ' Enf of SP change seperation                                   '
     End If                                                ' Enf of first time running seperation                                  '

     Cv1 = Pterm1 + Iterm1                                 ' Summing of the tree
     Cv1 = Cv1 + Dterm1                                    ' calculated terms
     ' --------End normal calculation loop



Error2 = Spx2 - Pv2                                         ' Obliczanie uchybu
Error2 = Error2 * -1                                        ' Inwersja uchybu
Sum_error2 = Sum_error2 + Error2                            ' Suma uchybów
Iterm2 = Ki2 * Sum_error2                                   ' Integrated CV part (CAŁKUJĄCY)

     ' -------- First time startup
     If First_execution2 < 2 Then
        If First_execution2 = 0 Then
           Sum_error2 = 40 / Ki2
           First_execution2 = 1
           Initial_error2 = Error2
        End If
        Pterm2 = 0
        Dterm2 = 0
        If Initial_error2 > 0 And Error2 < 0 Then
           First_execution2 = 2
           Last_pv2 = Pv2
        End If
        If Initial_error2 < 0 And Error2 > 0 Then
           First_execution2 = 2
           Last_pv2 = Pv2
        End If
        Last_sp2 = Spx2

     ' -------- Normal calculation loop
     Else
        D_pv2 = Last_pv2 - Pv2
        Last_pv2 = Pv2
        Dterm2 = Kd2 * D_pv2                               ' Derivated CV part
        If Spx2 = Last_sp2 Then
           ' -------- Normal loop when setpoint not changed
           Pterm2 = Kp2 * Error2                           ' Proportional CV part
           ' -------- Loop when setpoint changed
           Else
           Pterm2 = 0
           Dterm2 = 0
           If Spx2 > Last_sp2 And Pv2 > Spx2 Then
              Last_sp2 = Spx2
              Last_pv2 = Pv2
           End If
           If Spx2 < Last_sp2 And Pv2 < Spx2 Then
              Last_sp2 = Spx2
              Last_pv2 = Pv2
           End If
        End If                                             ' Enf of SP change seperation                                   '
     End If                                                ' Enf of first time running seperation                                  '

     Cv2 = Pterm2 + Iterm2                                 ' Summing of the tree
     Cv2 = Cv2 + Dterm2                                    ' calculated terms
     ' --------End normal calculation loop

Error3 = Spx3 - Pv3                                         ' Obliczanie uchybu
Error3 = Error3 * -1                                        ' Inwersja uchybu
Sum_error3 = Sum_error3 + Error3                            ' Suma uchybów
Iterm3 = Ki1 * Sum_error3                                   ' Integrated CV part (CAŁKUJĄCY)

     ' -------- First time startup
     If First_execution3 < 2 Then
        If First_execution3 = 0 Then
           Sum_error3 = 40 / Ki1
           First_execution3 = 1
           Initial_error3 = Error3
        End If
        Pterm3 = 0
        Dterm3 = 0
        If Initial_error3 > 0 And Error3 < 0 Then
           First_execution3 = 2
           Last_pv3 = Pv3
        End If
        If Initial_error3 < 0 And Error3 > 0 Then
           First_execution3 = 2
           Last_pv3 = Pv3
        End If
        Last_sp3 = Spx3

     ' -------- Normal calculation loop
     Else
        D_pv3 = Last_pv3 - Pv3
        Last_pv3 = Pv3
        Dterm3 = Kd1 * D_pv3                               ' Derivated CV part
        If Spx3 = Last_sp3 Then
           ' -------- Normal loop when setpoint not changed
           Pterm3 = Kp1 * Error3                           ' Proportional CV part
           ' -------- Loop when setpoint changed
           Else
           Pterm3 = 0
           Dterm3 = 0
           If Spx3 > Last_sp3 And Pv3 > Spx3 Then
              Last_sp3 = Spx3
              Last_pv3 = Pv3
           End If
           If Spx3 < Last_sp3 And Pv3 < Spx3 Then
              Last_sp3 = Spx3
              Last_pv3 = Pv3
           End If
        End If                                             ' Enf of SP change seperation                                   '
     End If                                                ' Enf of first time running seperation                                  '

     Cv3 = Pterm3 + Iterm3                                 ' Summing of the tree
     Cv3 = Cv3 + Dterm3                                    ' calculated terms
     ' --------End normal calculation loop


Error4 = Spx4 - Pv4                                         ' Obliczanie uchybu
Error4 = Error4 * -4                                        ' Inwersja uchybu
Sum_error4 = Sum_error4 + Error4                            ' Suma uchybów
Iterm4 = Ki2 * Sum_error4                                   ' Integrated CV part (CAŁKUJĄCY)

     ' -------- First time startup
     If First_execution4 < 2 Then
        If First_execution4 = 0 Then
           Sum_error4 = 40 / Ki2
           First_execution4 = 1
           Initial_error4 = Error4
        End If
        Pterm4 = 0
        Dterm4 = 0
        If Initial_error4 > 0 And Error4 < 0 Then
           First_execution4 = 2
           Last_pv4 = Pv2
        End If
        If Initial_error4 < 0 And Error4 > 0 Then
           First_execution4 = 2
           Last_pv4 = Pv4
        End If
        Last_sp4 = Spx4

     ' -------- Normal calculation loop
     Else
        D_pv4 = Last_pv4 - Pv4
        Last_pv4 = Pv4
        Dterm4 = Kd2 * D_pv4                               ' Derivated CV part
        If Spx4 = Last_sp4 Then
           ' -------- Normal loop when setpoint not changed
           Pterm4 = Kp2 * Error4                           ' Proportional CV part
           ' -------- Loop when setpoint changed
           Else
           Pterm4 = 0
           Dterm4 = 0
           If Spx4 > Last_sp4 And Pv4 > Spx4 Then
              Last_sp4 = Spx4
              Last_pv4 = Pv4
           End If
           If Spx4 < Last_sp4 And Pv4 < Spx4 Then
              Last_sp4 = Spx4
              Last_pv4 = Pv4
           End If
        End If                                             ' Enf of SP change seperation                                   '
     End If                                                ' Enf of first time running seperation                                  '

     Cv4 = Pterm4 + Iterm4                                 ' Summing of the tree
     Cv4 = Cv4 + Dterm4                                    ' calculated terms
     ' --------End normal calculation loop
End If


If Cv1 > 130 Then Cv1 = 130
If Cv2 > 130 Then Cv2 = 130
If Cv3 > 130 Then Cv3 = 130
If Cv4 > 130 Then Cv4 = 130

If Cv1 < 63 Then Cv1 = 63
If Cv2 < 63 Then Cv2 = 63
If Cv3 < 63 Then Cv3 = 63
If Cv4 < 63 Then Cv4 = 63

If Faza_lotu = 0 Then
Led3 = 0
Motor(1) = Moc
Motor(2) = Moc
Motor(3) = Moc
Motor(4) = Moc
End If

If Faza_lotu = 1 Then
Toggle Led3
Motor(1) = Cv1
Motor(2) = Cv2
Motor(3) = Cv3
Motor(4) = Cv4
End If


If Faza_lotu = 2 Then
Led3 = 1
Motor(1) = Cv1
Motor(2) = Cv2
Motor(3) = Cv3
Motor(4) = Cv4
End If

If Faza_lotu = 3 Then
Toggle Led3
Motor(1) = Cv1
Motor(2) = Cv2
Motor(3) = Cv3
Motor(4) = Cv4
End If

If Faza_lotu = 4 Then                                       'insrukcja tesoowa do odpalenia z konsoli
Toggle Led3
Motor(1) = Cv1
Motor(2) = Cv2
Motor(3) = Cv3
Motor(4) = Cv4
Print "kat_x " ; Angle_yy
Print "kat_y " ; Angle_xx
Print "ANGLE  " ; Angle
Print "wysokosc" ; Ob_wys
End If


'ładowanie danych do silników,
Servo(4) = Motor(4)
Servo(3) = Motor(3)
Servo(2) = Motor(2) - 6                                     'KOREKTA
Servo(1) = Motor(1)
Toggle Led1
Waitms 1
Loop

[Chumanista]

Skoro wszelka pomoc to kilka uwag do kodu:

1. Dlaczego Bascom? Moim zdaniem przejście na C to najlepsza możliwa decyzja.

2. Te kierunki zrób na ELSE IF bo jak widzę Bascom nie ma SWITCH.

If First_execution2 < 2 Then

3. Tego się nie da zrobić ładniej?

4. Koszmarna ilość IFów. Nie da się lepiej?

[zuba1]

Zamiast switch jest case. Nie w tym problem. tu chodzi bardziej o sam pid i jego spięcie z programem tak aby dawał się stroić i pracował sprawnie. Dziękuję za te cenne uwagi postaram się to poprawić. Ale na razie skupmy się na samym pidzie bo to on jest tu głównym problemem.