Sprawdzenie kodu regulatora PB [AVR]

[Tolo]

W sumie pewnie nie pownienem zakładać tego tematu ponieważ problem wydaje się być oklepany i teoretycznie powinienem znaleźć gdzieś odpowiedzi na moje pytania w rzeczywistości nie udało mi się tego dokonać tak więc piszę.

W czym rzecz ? Chodzi o to że napisałem regulator PD w BASCOM-AVR rzecz jednak w tym ,że nie jest on chyba napisany prawidłowo. Problem polega na tym że bez względu jakie wartości współczynników dobieram robot nie potrafi pokonać kątów prostych. W zasadzie to wogóle mam wrażenie że mój człon D nieczego nie daj 😋

Kod wygląda tak:

$regfile = "m8def.dat"
'**********Konfiguacja ADC**********
Config Adc = Single , Prescaler = Auto , Reference = Avcc
'**********Konfiguacja PWM**********
Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Up , Compare B Pwm = Clear Up
Pwm1a = 0
Pwm1b = 0
'**********Konfiguacja Przełącznika**************
Config Pinc.5 = Input
Set Pinc.5
Sw Alias Pinc.5
'**********Konfiguacja diód sygnalowych**********
Config Portd.2 = Output
Config Portd.3 = Output
Led1 Alias Portd.2
Led2 Alias Portd.3
Led1 = 0
Led2 = 0
'**********Konfiguacja silników**********
Config Portd.5 = Output
Config Portd.6 = Output
Config Portd.7 = Output
Config Portb.0 = Output
'Silnik prawy
M1a Alias Portd.7
M1b Alias Portb.0
'Silnik lewy
M2a Alias Portd.5
M2b Alias Portd.6
'**********Konfiguacja kierunku silnikow**********
'Silnik prawy
M1a = 1
M1b = 0
'Silnik lewy
M2a = 1
M2b = 0
'**********Konfiguacja stalych i zmiennych**********
Const Granica = 900
Dim Czujnik1 As Single
Dim Czujnik2 As Single
Dim Czujnik3 As Single
Dim Czujnik4 As Single
Dim Czujnik5 As Single
Dim W As Word

Dim X As Integer                                            'Zmienna opisujaca ilosc czujnikow na linii
Dim A As Integer                                            'Wga czujnika
Dim Er As Integer                                           'Srednia waga czujnikow
Dim K As Integer                                            'Współczynnik członu P
Dim Tp As Integer                                           'Docelowa prędkość
Dim Zmiana As Integer
Dim Kd As Integer                                           'Współczynnik członu D
Dim Zmiana2 As Integer                                      'Zmienne pomocnicze
Dim Zmiana3 As Integer

Tp = 90
Kp = 4
Kd = 4



'**********Oczekiwanie na naciśnięcie przełącznika*******
While Sw = 1
Led1 = 1
Wend
Waitms 500
Toggle Led1
Waitms 500
Toggle Led1
Waitms 500
Toggle Led1
Waitms 500
Toggle Led1
Waitms 500
Toggle Led1
Waitms 500
Led1 = 0


Pwm1a = 0
Pwm1b = 0

Do


Led2 = 1
'**********Zerowanie wartosci zmiennych po kazdymsprawdzeniu czujnikow**********
X = 0
A = 0
'**********Sprawdzanie czujnikow**********
Gosub Wczytaj_stany_przetwornikow

'**********Liczymy blad oraz wartosci PWM**********
Er = A / X
D = Er - Erp
Erp = Er
Zmiana = Er * Kp
Zmiana2 = Erp * Kd
Zmiana3 = Zmiana + Zmiana2


Pwm1a = Tp - Zmiana3
Pwm1b = Tp + Zmiana3



  Loop
End


Wczytaj_stany_przetwornikow:
Start Adc
W = Getadc(4)
Stop Adc
Czujnik5 = W
If Czujnik5 > Granica Then
A = A - 8
X = X + 1
End If
Start Adc
W = Getadc(3)
Stop Adc
Czujnik4 = W
If Czujnik4 > Granica Then
A = A - 4
X = X + 1
End If
Start Adc
W = Getadc(2)
Stop Adc
Czujnik3 = W
If Czujnik3 > Granica Then
A = A - 0
X = X + 1
End If
Start Adc
W = Getadc(1)
Stop Adc
Czujnik2 = W
If Czujnik2 > Granica Then
A = A + 4
X = X + 1
End If
Start Adc
W = Getadc(0)
Stop Adc
Czujnik1 = W
If Czujnik1 > Granica Then
A = A + 8
X = X + 1
End If
Return

Bardzo proszę o pomoc i wyrozumiałość 😅

[MirekCz]

1. Dlaczego korzystasz z Erp a nie z D? Tak jak masz to Er = Erp, więc Kp i Kd się sumują i masz podwójny człon P

Czyli powinno być Zmiana2 = D * Kd

2. Sprawdzić człon D jest prosto:

- Zrób prostą linię z samym członem P (może być przesadzony, tak, żeby robot wpadał w sinusoidę)

- Dodaj człon D i jego wartość zwiększaj do czasu aż robot w krótkim czasie będzie prawidłowo wyrównywał się na linii, a nie oscylował.