Przyjazne Bascom Forum

Forum
https://bart-projects.pl/forum/

Enkoder 32bit poraz enty i servomotor PID

https://bart-projects.pl/forum/viewtopic.php?f=18&t=417

Strona 1 z 2

Enkoder 32bit poraz enty i servomotor PID

: 04 lis 2021, 18:18
autor: tomaszdko
Kiedyś napisałem sterownik PID pod servomotor (bo w necie nic nie znalazłem prócz na stronie bascom ale był zbyt rozbudowany) . Z samym sterownikiem nie mam problemu bo chodzi poprawnie ale , no właśnie ale . Program był tworzony pod silniki z enkoderami 40 impulsów na obrót i wystarczył jeden timer ustawiony jako enkoder sprzętowy . Teraz mam silnik z tarczą 100 impulsów i przekładnią więc taki silnik trochę pokręci a timer musi zliczyć sporo i tu zaczyna się problem bo w projekcie mam śrubę o długości 1m . Timer nie da rady zliczyć takie ilości na całą długość . Wstawiłem dwa timery jako 32bit , wszystko piękanie do momentu kiedy trzeba zliczać w tył . Xmega nie ma rejestru 32bit . Jeśli przeliczy timer1 do 65535 impulsów to następuje inkrementacja timera2 o jeden a timer1 jest zerowany no i jest ok ale jeśli po inkrementacji timer2 cofniemy się enkoderem w tył to nie zmienia to timer2 a w timer1 będziemy mieć np 65530 ...i znowu damy w przód to automatycznie nastąpi inkrementacja timer2 co jest błędem bo w rzeczywistości timer1 nie zliczył od 0 do 65535 ...xmega zlicza poprawnie ale logika jest błędna :/ jak to rozwiązać !? ...Robiłem też programowe zliczanie na przerwaniach i chodzi ale ...znowu ale :/ ...wygląda jak by gubiło kroki bo enkoder na silniku i enkoder do pozycjonowania jak i procedura PID są na przerwaniach (możliwe że to miesza) bo na sprzętowym chodzi jak trzeba aaaa na sprzętowym zlicza mi inaczej niż na programowym . Zliczanie sprzętowe - silnik ma enkoder 100inp a zlicza mi 400 , enkoder sterujący ma 400inp a pokazuje po pełnym obrocie 1600 . Zliczanie programowe - zlicza 100 , enkoder sterujący zlicza 400 (nie wiem dlaczego tak jest)

Re: Enkoder 32bit poraz enty i servomotor PID

: 08 lis 2021, 15:24
autor: yaro
Miałem podobny problem i rozwiązałem go troszkę inaczej - nie łączyłem liczników w tryb 32 bitowy tylko skorzystałem z bitu DIR w rejestrze CTRLFCLR. Licznik pozostał 16 bitowy a ja finalna pozycję przeliczałem z aktualnej wartości licznika i zliczonej ilości przepełnień (tutaj kierunek zliczania "DIR" odpowiadał za zwiększenie lub zmniejszenie licznika).

Re: Enkoder 32bit poraz enty i servomotor PID

: 08 lis 2021, 15:28
autor: yaro
Miałem podobny problem i rozwiązałem go troszkę inaczej - nie łączyłem liczników w tryb 32 bitowy tylko skorzystałem z bitu DIR w rejestrze CTRLFCLR. Licznik pozostał 16 bitowy a ja finalna pozycję przeliczałem z aktualnej wartości licznika i zliczonej ilości przepełnień (tutaj kierunek zliczania "DIR" odpowiadał za zwiększenie lub zmniejszenie licznika).

Re: Enkoder 32bit poraz enty i servomotor PID

: 08 lis 2021, 16:37
autor: niveasoft
Przepraszam za to opóźnienie. Ze względów spamu i tym podobnych weryfikacji podlega pierwszy post nowego forumowicza na forum. Powinien tyczyć się Bascoma. Oczywiście nie śpię z komputerem ani nie jestem do niego przywiązany :D Dbam tylko o spokój bo zdarzały się reklamy lub boty i na razie się udaje.
Przy okazji witam.

Re: Enkoder 32bit poraz enty i servomotor PID

: 08 lis 2021, 21:09
autor: Jacek
Witam - Tomek jutro wysyłka xmegi do Ciebie na maila wysłałem foto z info.
pozdrawiam Jacek.

Re: Enkoder 32bit poraz enty i servomotor PID

: 18 lis 2021, 14:35
autor: tomaszdko
Mam nocki i system 12h więc czasem nie mogę odpisać . Jacek jeszcze nie doszła paczka (pewnie w tym tygodniu będzie, jak coś to napisze maila).
Wracając do PID to nagrzebałem trochę i chodzi a nawet nie dla Xmega a dla arduino nano czyli bez użycia sprzętowego timera (tym lepiej bo taniej i wygodniej ale dla Xmega jeszcze przerobię i rozbuduję).
Dziwiło mnie czemu raz jest dobrze a raz delikatnie przestawia położenie . To tak jest jak się robi na stykówkach i drugi błąd to nie podciągnięty enkoder silnika rezystorami 4,7k pod "+".
Jeszcze dwa przypały , które wyszły to użycie nieodpowiedniego mostka H (mx1508 za słaby) i WAŻNE to sondy oscyloskopu miałem ustawione na x1 (zakłócało przebieg enkoderów).
Testowałem dobrą godzinę i teraz wygląda że jest igła tak jak trzeba.

Kod działa dla sterowania z enkodera zewnętrznego jak i rs232 (trzeba wybrać , który chce się użyć a niepotrzebny odhaczyć) . Muszę dorobić jeszcze sterowanie STEP/DIR.
Code: [Zaznacz cały] [Rozwiń/Zwiń]
  1. $REGFILE = "M328PDEF.DAT"
  2. $CRYSTAL = 16000000
  3. $BAUD = 9600
  4. $HWSTACK = 100
  5. $SWSTACK = 100
  6. $FRAMESIZE = 100
  7. ENABLE INTERRUPTS
  8. '------------------------------------------------------------------------------------------------------------------
  9. '--------------------------------------------- UART ---------------------------------------------------------------
  10. '------------------------------------------------------------------------------------------------------------------
  11.    CONFIG COM1 = 9600  , SYNCHRONE = 0 , PARITY=NONE , STOPBITS=1 , DATABITS=8 , CLOCKPOL=0
  12.    OPEN "COM1:" FOR BINARY AS #1
  13.    CONFIG SERIALIN0 = BUFFERED , SIZE = 30 , BYTEMATCH = 13
  14.    DIM COM1 AS STRING*30
  15.    DIM RS232 AS  LONG
  16.    ENABLE URXC
  17. '------------------------------------------------------------------------------------------------------------------
  18. '--------------------------------------------- INT1-SILNIK --------------------------------------------------------
  19. '------------------------------------------------------------------------------------------------------------------
  20.    CONFIG INT1 = CHANGE
  21.    ON INT1 SILNIK
  22.    ENABLE INT1
  23. '-------------------------------------------------------------
  24.    CONFIG PORTD.3 = INPUT            'ARDUINO NANO - D2
  25.    CONFIG PORTC.3 = INPUT            'ARDUINO NANO - A2
  26.    DIM POZYCJA_SILNIK AS LONG
  27. '------------------------------------------------------------------------------------------------------------------
  28. '--------------------------------------------- INT0-ENKODER -------------------------------------------------------
  29. '------------------------------------------------------------------------------------------------------------------
  30.    CONFIG INT0 = CHANGE
  31.    ON INT0 ENKODER
  32.    ENABLE INT0
  33. '-------------------------------------------------------------
  34.    CONFIG PORTD.2 = INPUT            'ARDUINO NANO - D3
  35.    CONFIG PORTC.2 = INPUT            'ARDUINO NANO - A3
  36.    DIM POZYCJA_ENKODER AS LONG
  37. '------------------------------------------------------------------------------------------------------------------
  38. '------------------------------------------- TIMER1 / DT ----------------------------------------------------------
  39. '------------------------------------------------------------------------------------------------------------------
  40.    CONFIG TIMER1 = TIMER , PRESCALE = 1
  41.    ENABLE TIMER1
  42.    STOP TIMER1
  43. '------------------------------------------------------------------------------------------------------------------
  44. '------------------------------------------- TIMER0 / PWM ---------------------------------------------------------
  45. '------------------------------------------------------------------------------------------------------------------
  46. '    PD3 - OC2B;    PB3 - OC2A;
  47. '    PD5 - OC0B;    PB2 - OC1B;
  48. '    PD6 - OC0A;    PB1 - OC1A;
  49.  
  50.    CONFIG TIMER0 = PWM ,PWM=ON , COMPARE_A_PWM = CLEAR_UP , COMPARE_B_PWM = CLEAR_UP   ,PRESCALE = 1
  51.    ENABLE TIMER0
  52.    START TIMER0
  53.    CONFIG PORTD.5=OUTPUT           'ARDUINO NANO - D5
  54.    CONFIG PORTD.6=OUTPUT           'ARDUINO NANO - D6
  55. '------------------------------------------------------------------------------------------------------------------
  56. '--------------------------------------------ZMIENNE---------------------------------------------------------------
  57. '------------------------------------------------------------------------------------------------------------------
  58.    DIM CEL AS LONG                                  'CEL GDZIE MA DOJSC
  59.    DIM AKTUALNE AS LONG                             'POLOZENIE AKTUALNE
  60.    DIM ERROR AS SINGLE                              'ROZNICA MIEDZY POLOZENIEM AKTUALNYM A CELEM
  61.    DIM DT AS SINGLE                                 'OKRES POMIARU
  62.    DIM ERROR_STARY AS SINGLE                        'WARTOSC POPRZEDNIEGO ERROR , CYKL WCZESNIEJ
  63.    DIM ZMIANA AS SINGLE                             'ROZNICA MIEDZY ERROR_OLD A ERROR
  64.    DIM ZBOCZE AS SINGLE
  65.    DIM STREFA AS SINGLE
  66.    DIM STREFA_1 AS SINGLE
  67.    DIM STREFA_STARA AS SINGLE
  68.    DIM P AS SINGLE , I AS SINGLE,,   D AS SINGLE
  69.    DIM K1 AS SINGLE , K2 AS SINGLE , K3 AS SINGLE
  70.    DIM PID AS SINGLE
  71.    DIM PID_1 AS SINGLE
  72.    DIM PID_STARY AS SINGLE
  73.    DIM PID_WYJ AS LONG
  74.    DIM PID_WYJ_A AS LONG
  75.    DIM PID_WYJ_B AS LONG
  76.  
  77.    K1= 22          '11    22                         'USTAWIENIA DLA PID DOBRAC POD SILNIK
  78.    K2= 0.95        '0.8    0.8
  79.    K3=0.01         '0.01   0.001
  80.    DT=2000                                           'DOMYSLNE DT PRZY PRZEJSCIU PIERWSZEJ PETLI
  81.  
  82.    CONFIG PORTC.5=OUTPUT                            'ARDUINO NANO - A5 , LED
  83.  
  84. DO
  85. '-----------------------------------------------      '  WYBRAC I ODHACZYC
  86.    CEL=RS232                                          '  ZMIENNA NA UART , ILOSC IMPULSOW DO PRZEBYCIA -/+
  87.    'CEL=POZYCJA_ENKODER                               '  ENKODER STERUJACY , ILOSC IMPULSOW DO PRZEBYCIA -/+
  88. '-----------------------------------------------
  89.    ERROR_STARY=ERROR
  90.    ERROR=CEL-AKTUALNE
  91.    ZMIANA=ERROR-ERROR_STARY
  92.    ZBOCZE=ZMIANA * DT
  93.    STREFA_1=ERROR / DT
  94.    STREFA=STREFA_STARA+STREFA_1
  95.    STREFA_STARA=STREFA
  96.  
  97.    P=K1*ERROR                                         ' PID
  98.    I=K2*ZBOCZE
  99.    D=K3*STREFA
  100.  
  101.    PID_1=P+I
  102.    PID_1=PID_1+D
  103.    PID=PID_1
  104.    PID_STARY=PID
  105.    AKTUALNE=POZYCJA_SILNIK
  106.    PID_WYJ=PID
  107. '-----------------------------------------------
  108.    IF PID_WYJ > -1 THEN
  109.    PID_WYJ_A=PID_WYJ
  110.    IF PID_WYJ_A>255 THEN PID_WYJ_A=255
  111.    COMPARE0B = PID_WYJ_A
  112.    COMPARE0A = 0                                       'OCR0B
  113.    END IF                                             'OCR0A
  114.  
  115.    IF PID_WYJ < 1 THEN
  116.    PID_WYJ_B = PID_WYJ*-1
  117.    IF PID_WYJ_B>255 THEN PID_WYJ_B=255
  118.    COMPARE0A = PID_WYJ_B                               'OCR0A
  119.    COMPARE0B = 0                                       'OCR0B
  120.    END IF
  121.  
  122.    IF POZYCJA_ENKODER=0 THEN                         'KONTROLA LED POZYCJI "0" TYLKO DLA ENKODERA STERUJACEGO
  123.    PORTC.5=0                                          'ARDUINO NANO - A5
  124.    ELSE
  125.    PORTC.5=1
  126.    END IF
  127.  
  128. STOP TIMER1                                           'ODLICZANIE DT W PETLI
  129. DT=TIMER1
  130. TIMER1=0
  131. START TIMER1
  132.  
  133. LOOP
  134. END
  135. '-----------------------------------------------
  136. SERIAL0CHARMATCH:
  137.    PUSHALL
  138.    INPUT #1,COM1 NOECHO
  139.    RS232=VAL(COM1)
  140.    POPALL
  141. RETURN
  142. '-----------------------------------------------
  143. SILNIK:
  144. If PIND.3 = 0 AND PINC.3 = 1 THEN DECR POZYCJA_SILNIK
  145. If PIND.3 = 0 AND PINC.3 = 0 THEN INCR POZYCJA_SILNIK
  146. If PIND.3 = 1 AND PINC.3 = 0 THEN DECR POZYCJA_SILNIK
  147. If PIND.3 = 1 AND PINC.3 = 1 THEN INCR POZYCJA_SILNIK
  148. RETURN
  149.  
  150. ENKODER:
  151. If PIND.2 = 0 AND PINC.2 = 1 THEN DECR POZYCJA_ENKODER
  152. If PIND.2 = 0 AND PINC.2 = 0 THEN INCR POZYCJA_ENKODER
  153. If PIND.2 = 1 AND PINC.2 = 0 THEN DECR POZYCJA_ENKODER
  154. If PIND.2 = 1 AND PINC.2 = 1 THEN INCR POZYCJA_ENKODER
  155. RETURN
Bartek możesz sprawdzić czy działa NoSaveTool by zoptymalizowac przerwania ? Mi nie pokazuje żadnych przerwań ale na innych programach wykrywa (nie zawsze).

Re: Enkoder 32bit poraz enty i servomotor PID

: 18 lis 2021, 14:44
autor: tomaszdko
YARO - Pokombinuję z tymi rejestrami.

Re: Enkoder 32bit poraz enty i servomotor PID

: 18 lis 2021, 15:11
autor: niveasoft
Życie byłoby łatwiejsze gdyby ludzie używali Code Explorera bo mój tu trochę narzekał. Ten kompilator bascomowy dużo ludziom wybacza.
Masz tam typo czyli błąd bo dwa przecinki i potem Single D jest niby niezdefiniowana. Kompilator się za Ciebie domyślił...
Te określanie kanałów dla Input w helpie jest tylko dla SPI a dla SERIALa chyba #1 już nie ma. Chyba o tym zapomnieli.

NoSave działa, ale nie spodziewa się słowa "ON" tylko "On". To może mój błąd i go poprawię w następnej wersji. Może cały plik OBJ do Uppercase albo Lowercase a może jakiś Regex nowy napiszę sobie (nie pamiętam jak to rozwiązane). Z poprawą Regex pójdzie szybciej. Pobawię się bo zainstalowałem już najnowsze Studio 2022 :D
Teraz zamień tylko duze O na małe o i działa.

A co do tego kodu to słyszałeś o Case Is? Wtedy zmienną sprawdza tylko raz...

Re: Enkoder 32bit poraz enty i servomotor PID

: 18 lis 2021, 16:00
autor: tomaszdko
Znowu ekspresowo odpowiadasz heheh .
-"Case Is" nie brałem pod uwagę ale dobrze wiedzieć.
-"ON" tylko "On" , działa heheh od jakiegoś czasu używam dużych liter , a te stare programy to dlatego widziało dobrze bo pisownia była mała .
-podwójny przecinek ,, to nie problem :) ale poprawiłem...i to jest wyższość bascoma nad C ;)
-uart działa mi zawsze przy takim definiowaniu (dla xmega i mega)i nie ma z tym problemu fakt że są luki w bascom bo kiedyś miałem problemy z kanałami pwm dla mega128 (nie były zdefiniowane w .dat)
pewnie już poprawili i to samo tyczy się samego kompilatora. Chodzi więc nie wnikam :)

Re: Enkoder 32bit poraz enty i servomotor PID

: 18 lis 2021, 17:30
autor: Jacek
OK - do Niemiec wysyłałem dzień wcześniej i doszło dopiero wczoraj.
Spoko nie ma ciśnienia.
pozdrawiam wszystkich Jacek.
Strefa czasowa UTC+01:00
Strona 1 z 2
Powered by phpBB® Forum Software © phpBB Limited
https://www.phpbb.com/