Elektroda.pl
Elektroda.pl
X

Search our partners

Find the latest content on electronic components. Datasheets.com
Elektroda.pl
Please add exception to AdBlock for elektroda.pl.
If you watch the ads, you support portal and users.

[Atmega8][C] DC Serwomotor driver dla Mach3

zippy 30 Mar 2009 13:36 13449 14
Tespol
  • #1
    zippy
    Level 12  
    Witam Wszystkich
    Chciałbym zbudować frezarkę CNC w oparciu o programowanie typu MACH 3. Na jego "wyjściu" podawany jest sygnał dir i step. Niestety ze względu na masę konstrukcji nie mogę zastosować silników krokowych.

    Powstaje problem jak obsługiwać silnik DC z enkoderem za pomocą sygnału przeznaczonego dla siników krokowych.
    Na elektrodzie znajduje się projekt takiego serownika:
    https://www.elektroda.pl/rtvforum/topic1253786.html
    Lecz jest on dość stary, ma trochę błędów a do tego nie jest udostępniony kod programu w C tylko binarki.
    Zalazłem wręcz idealny projekt, na którym można oprzeć całą część hardwarowa, lecz kod jest w assemblerze którego zupełnie nie znam:
    http://elm-chan.org/works/smc/report_e.html

    Jest kilka dużych problemów do rozwiązania takich jak:
    -Zamiana impulsów na rozkaz dla silnika - czy zliczać je w odstępach czasu czy pojedynczo wrzucać do "modułu sterownika"
    -Obsługa enkodera
    -Rozwiązanie sterowania silnikiem za pomocą PWM, zastosowanie regulatora najprawdopodobniej PIDi regulacje jego parametrów

    Wykorzystanie programu w asm nie jest rozwiązaniem ze względu na to że nie będę w stanie wykorzystać tej wiedzy(kodu programu) do następnych projeków - sterowanie serwem jako napędem do posówu tokarki, proporcjonalnie do kąta obrotu wrzeciona.

    Najprawdopodobniej któryś użytkownik tego zacnego forum juz napisał tego typu program i mógłby się podzielić nim.
    Dziękuje za pomoc
  • Tespol
  • #2
    __Maciek__
    Level 20  
    Właściwie oda linki które podałeś dotyczą tego samego projektu.

    Jeśli chodzi o sterowanie ( schemat, blok mocy etc ... ) to można znaleźć w sieci schematy od UHU.

    Z programem jest gorzej.
    Jeśli chodzi o pojedyncze elementy jak obsługa enkoderów kwadraturowych, PID, sterowanie silnikiem DC nie jest problemem.

    Problem pojawia się jeśli chcesz to wszystko połączyć. A to ze względu na szybkość procesorów ( dlatego właśnie sterownik serwa jaki znalazłeś jest napisany w asemblerze, aby uzyskać maksymalną wydajność. ).
    Jeśli chodzi o realizację w języku C to wg. mnie najlepszą platformą będzie jakiś ARM.

    Ad. poszczególnych bloków programowych to poszukaj. Gwarantuję że wszystko jest.
    W razie czego mogę poszukać i wrzucić kawałki kodu.
  • #3
    janbernat
    Level 38  
    Zanim zaczniesz kup albo wymontuj z jakiejś starej drukarki
    trzy silniki krokowe, zrób jakieś proste sterowniki i spróbuj
    "napędzić" to za pomocą MACH2 lub 3 w wersji demo.
    Ja spróbowałem-totalna porażka.
    Wystarczy że windows zacznie coś robić w "tle"-włączy dysk itp. sterowanie przez LPT dostaje "kota"-silniki wariują.
    W instrukcji piszą żeby wyłączyć tryb oszczędzania energii,
    wygaszanie ekranu i wszystko co można-nic nie pomaga.
    Jakiś czas chodzi wszystko dobrze i raptem "kicha".
    To może być do jakichś mało ważnych zadań ale jak silniki krokowe są dla ciebie za słabe to pewnie jakaś poważniejsza praca.

    Dodano po 8 [minuty]:

    PS.
    A problemem też jestem zainteresowany.
    W mojej firmie uruchamiam frezarkę 1500x2000mm
    napędzaną silnikami krokowymi.
    Być może za jakiś czas będę chciał zmienić na serwo, bo te krokowe są jednak wolne.
    Na razie nie mam pomysłu na tanie rozwiązanie(można kupić gotowy system).
  • #4
    User removed account
    User removed account  
  • #5
    zippy
    Level 12  
    Co do mach uważam ze działa ok, mam core 2 duo wiec możne dla tego nie odczuwam problemów, zrobiłem 2 sterowniki na atmaga 8 silników krokowych i działy ok.
    Kompletna elektronika jest na stronie ELM (tej zagranicznej), gdy by sie pojawił problem z wydajnością zawsze można lepszego atmela znaleźć one chyba chodzą do 32mHz?
    W ostateczności można zrobić budowę modułową, jeden procek od enkodera 2 od zliczania impulsów dir i stp a ostatni od PWM na silnik.

    Nawet sam sterownik z PMW i PID odbierający zygnały np po UART o ile kroków ma się przesunąć byłby niezwykle przydatnym modułem do budowy większych systemów.
    Co do wykorzystania tego - jestem studentem automatyki i zastanawiam się właśnie nad budową takiej obrabiarki na dyplom, warstwa fizyczna tzn mechanika nie jest problemem ponieważ koszty mieszczą sie w 1000 zł przy małej maszynie ( i dużej ilości kombinowania i własnej pracy), to już profesjonalna elektronika jest niewyobrażalnym wydatkiem :(
  • Tespol
  • #6
    janbernat
    Level 38  
    Mardook:
    "Możesz użyć śrub napędowych, co już dość sporo powinno przełożyć siłę"- i koszty.
    Za śrubę(z obróbką końcówek), nakrętkę i obudowy łożysk
    zapłaciłem 3200zł.(do osi X)
    Za oś Z z silnikiem krokowym 1000 zł.
    Za dwie śruby w Y po 3400zł.
    Koła zębate, paski, mocowania itp. ok.2000zł.
    Prowadnice, wałki, podpory, sterowniki silników, zasilacze
    ok.5000zł
    A konstrukcja i projekt całej maszyny zrobiona w firmie-czego nie liczę.
    Ale i tak w projekcie zippy postaram się udzielać bo jest interesujący.
    Jak się coś z tego urodzi to zapraszam do sprawdzenia na mojej maszynie.
  • #7
    zippy
    Level 12  
    Co do wydajnosci dodam jeszcze ze kolejna wersja tego serwo kontrolera wyszła na attiny2313 taktowane na 16Mhz. Pierwsza wersja za at90s2313 w czasie pracy zużycie procesora było 40% Wiec problemem teraz jest tylko algorytm - tzn kod programu.
    Może ktoś ze znajomością assemblera mógł by naświetlić jak dokładnie działa tamta aplikacja ?
    http://elm-chan.org/works/smc/smc3.zip
  • #8
    __Maciek__
    Level 20  
    Tak 16Mhz .. ale ckdiv8=1 więc 2Mhz ... na dodatek algorytmy PID zorganizowane są na zmiennych stałoprzecinkowych (chyba) wynika to z opisu konfiguracji.

    Jeśli chcesz to wrzucę Ci PID-a ale ze zmiennym przecinkiem .. trzeba by sprawdzić ile czasu zajmuje algorytm. ( coś mi się kołacze że na pusto @ 18Mhz Atmega168 robił coś ok 100Khz ale nie jestem pewien. )

    Enkoder to prosta sprawa albo układ rozpoznający kierunek na logice zewnętrznej i 2 przerwania + i - ( coś takiego widziałem gdzieś w sieci bodajże jakiś rosyjski klon UHU ) albo programowo.

    Zauważ że autor na stronie ELM pominął układ kontroli prądu silnika. Warto się zastanowić zatem czy nie wypadało by projektu wzbogacić o tą funkcjonalność. ( Kopiować raczej niema sensu może więc lepiej zrobić coś o krok dalej. )

    Odnośnie wydajności to sugerowałem się właśnie sterownikiem UHU ( tam jest też 2313 ale pracujący z kwarcem 24Mhz. )

    Myślę że algorytm nie jest bardzo skomplikowany. Popatrz sobie na górze strony źródłowej masz schemat algorytmu. Jest to PID wzbogacony o kontrolę prędkości maksymalnej i kontrolę prądu ( w tym przypadku bez sprzężenia zwrotnego. )
  • #10
    VolKhen
    Level 13  
    Podłączę się do tematu.
    Konstruuję sterownik silnika prądu stałego - serwonapęd.
    W tym momencie mam gotowe podprogramy:
    - odczyt pozycji z enkodera
    - regulator PID (nie działa tak jakbym chciał, albo potrzebny jest tuning parametrów albo mam gdzieś błąd w kodzie)
    W tym momencie silnik lubi oscylować wokół pozycji zadanej.
    W tym momencie stanąłem na funkcjach generowania ruchu trapezoidalnego. Prawdę mówiąc nie wiem od czego zacząć. Domyślam się, że potrzebna w programie będzie prędkość aby określić max V i a.
    Nigdy wcześniej nie programowałem, więc chyba i tak daleko z tym zaszedłem... :)
    Uprzejmie proszę o jakieś wskazówki jak zrobić pomiar prędkości. (coś próbowałem z sprawdzaniem różnicy stanu rejestru pozycji w parzystych i nieparzystych cyklach przerwania regulatora PID, ale nie działało to za dobrze).
    Timer0 działa jako przerwanie 1kHz z pętlą sprawdzania pozycji i liczenia regulatora PID.
    Timer1 działa jako PWM z f=15kHz.

    Poszukuję też kogoś z Bydgoszczy chętnego udzielić korepetycji w tym temacie. Oczywiście odpłatnie. Czekam na PM od chętnych.
    [Atmega8][C] DC Serwomotor driver dla Mach3

    Kod źródłowy:
    Code:

    #define F_CPU 16000000L
    #include <avr/io.h>
    #include <stdint.h>
    #include <stddef.h>
    #include <util/delay.h>
    #include <inttypes.h>
    #include <avr/interrupt.h>
    #include "rprintf.h"
    #include "pid.h"
    #include <math.h>


    #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
    #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))

    #define ENC_PORT PIND
    #define SET_IN3 PORTC    |= _BV(0)
    #define SET_IN4 PORTC    |= _BV(1)
    #define CLR_IN3 PORTC    &= ~_BV(0)
    #define CLR_IN4 PORTC    &= ~_BV(1)

    /*************** PID variables *******************/

    #define K_P     160
    #define K_I     0.01
    #define K_D     0

    struct PID_DATA pidData;

    volatile int16_t motorReference=0x0010;
    volatile int16_t measurementValue, inputValue;

    /**************** Encoder variables ***************/
    int16_t en_pos, en_pos_prev, change, speed;
    int i;
    int temp;
    unsigned char en_dir;
    unsigned char en_now=0;
    unsigned char en_prev;


    /**************** PWM variables *******************/
    int16_t pwm_value;

    void init_IO(void)
    {
    DDRD  = 0x20;               // Ustawia PORTD jako wyjscie/wejście
    PORTD = 0xFF;               // Podciąga PORTD do 1
    DDRA  = 0xF0;               // Ustawia PORTA jako wejscie/wyjscie
    PORTA = 0xFF;               // Podciaga PORTA do 1
    DDRB  = 0xFF;
    PORTB = 0xFF;
    DDRC  = 0xFF;
    PORTC = 0xFF;

    /**************************************************/
    TIMSK = 0x02;

    /***************** Config T0 **********************/
    //ustawienie timer do przerwania z f 1kHz

    OCR0  = 0x75;
    TCCR0 = 0x0B;

    /**************** PWM Config T1 *******************/
    TCCR1A = 0x83;               // Konfiguracja z CodeVision
    TCCR1B = 0x09;
    TCNT1H = 0x00;
    TCNT1L = 0x00;
    ICR1H  = 0x00;
    ICR1L  = 0x00;
    OCR1AH = 0x00;
    OCR1AL = 0x00;
    OCR1BH = 0x00;
    OCR1BL = 0x00;

    /***************** Config T2 **********************
    ASSR=0x00;
    TCCR2=0x07;
    TCNT2=0x00;
    OCR2=0xFF;

    /**************** LCD Init ************************/
    LCDinit();
    LCDcursorOFF();

    /**************** PID Init ************************/

    pid_Init(K_P * SCALING_FACTOR, K_I * SCALING_FACTOR , K_D * SCALING_FACTOR , &pidData);

    }

    void encoder_init(void)
    {
      en_pos = 0;
      sbi(MCUCR,ISC00);                // Ustawienie INT0
      cbi(MCUCR,ISC01);                // na zbocze rosnace i opadające
      sbi(GICR,INT0);                // Wlaczenie przerwania INT0
      sei();
    }

    void encoder_off(void)
    {
      cbi(GICR,INT0);
    }

    void encoder_on(void)
    {
      sbi(GICR,INT0);
    }

    void set_encoder_position(int position)
    {
      en_pos = position;
    }

    void go_right(void)
    {
      SET_IN3;
      CLR_IN4;
      PORTA    |= _BV(5);
      PORTA    &= ~_BV(4);
    }

    void go_left(void)
    {
      SET_IN4;
      CLR_IN3; 
      PORTA    |= _BV(4);
      PORTA    &= ~_BV(5);
    }

    void stop_motor(void)
    {
      CLR_IN3;
      CLR_IN4;
      PORTA    |= _BV(4);
      PORTA    |= _BV(5);
    }

    void print_byte_to_lcd(uint8_t dana)
    {
    LCDsendChar(dana);                // funkcja wypisujaca
    }

    void Set_Input(int16_t inputValue)
    {
      pwm_value = inputValue;   // Skalowanie wyniku -
      if(pwm_value >= 0x0240)      // Ograniczenie mocy silnika
      {
        pwm_value = 0x0240;             // Jeśli przekroczono wartość maks, to ustaw maks
      }
      if(pwm_value < 0x00)      //Wykluczenie pwm < od skutecznej wartości, ktora powoduje ruch silnika
      {
        pwm_value = 0x00;                 // Jeśli przekroczono wartość maks, to ustaw 0
      }
      OCR1A = pwm_value;               // Wpisanie wart. PWM do rejestru porównującego
    }

    SIGNAL (INT0_vect)                //obsługa przerwania
    {
      en_now = (ENC_PORT & (3 << 2)) >> 2;          // odczyt portu i przesuniecie bitow na prawo
      en_dir = (en_prev & 1) ^ ((en_now & 2) >> 1); // sprawdzenie kierunku obrotu
     
      if(en_dir == 0)
      {
        en_pos++;             // Kierunek obrotu w prawo: dodaj
      }
      else   
      {
        en_pos--;              // Kierunek obrotu w lewo: odejmij
      }
        en_prev = en_now;       // Zapamiętaj ostatnią wartosc do porownania nast. razem
    }

    ISR (TIMER0_COMP_vect)
    {
      /*i++;
      if(i % 2)
      {
      en_pos_prev = en_pos;
      }
      else
      {
      change = (en_pos - en_pos_prev) * 10;
      }*/

      measurementValue = en_pos;
      // zmiana kierunkow pracy
      if(motorReference < measurementValue)
      {
      go_left();
      }
      else
      {
      go_right();
      }
      if(abs(motorReference - measurementValue) < 1) //wylacz silnik w pozycji zadanej
      {
      stop_motor();
      }
     
      inputValue = pid_Controller(motorReference, measurementValue, &pidData);
             
      Set_Input(inputValue);
      PORTA ^= _BV(6);    // Mruganie diodą w celu sprawdzenia predkosci programu przerwania
    }   

    /*
    ISR (TIMER2_COMP_vect)
    {
      i++;
      if(i % 2)
      {
      en_pos_prev = en_pos;
      }
      else
      {
      change = en_pos - en_pos_prev;
      }
      PORTA ^= _BV(6);    // Mruganie diodą w celu sprawdzenia predkosci programu
    }
    */

    int main (void)
    {

      init_IO();
      encoder_init();
      rprintfInit(print_byte_to_lcd);
      LCDclr();
      while(1)
      {
       LCDhome();
       

        if((PIND & 1) == 0)
       {
       motorReference += 1;   //Sterowanie silnikiem w prawo
       }
        if((PIND & 2) == 0)
       {
       motorReference -= 1;   //w lewo
       }
       if((PINA & 1) == 0)
       {
       stop_motor();   //stop
       }
       if((PINA & 2) == 0)
       {
       go_right();   //w prawo
       }

       rprintf("M:%x", measurementValue);
       LCDGotoXY(0,1);
       rprintf("I:%x", inputValue);
       LCDGotoXY(7,0);
       rprintf("E:%x", motorReference);
       LCDGotoXY(7,1);
       rprintf("P:%x", change);
       
      }
    }


    Regulator PID:
    Standardowy regulator PID z noty katalogowej Atmela AVR221.
  • #11
    VolKhen
    Level 13  
    Program został przeze mnie mocno przebudowany. Zastosowałem inny regulator PID, który jest bardziej przejrzysty.

    Naprawde nikt nie ma nic do dodania?
  • #13
    utak3r
    Level 25  
    Tak odnośnie regulatora... przy silnikach poleciłbym Ci zastosować algorytm PI (bez D), czyli proporcjonalno-całkujący, bez różniczkowania. Daje to lepsze efekty, z powodu problemów z regulacją pełnego algorytmu.
  • #14
    _Robak_
    Level 33  
    Tutaj bym się nie zgodził, do regulacji serwonapędów stosuję się regulator PD, z tej przyczyny że przy PID masz dwa całkowania (silnik plus część całkująca regulatora) które to właśnie mogą powodować oscylacje i generalne problemy w regulacji.
  • #15
    VolKhen
    Level 13  
    Dzięki za pomoc choć nie było ona zbyt duża. W każdym razie dzięki. Udało się wszystko zrobić samemu choć momentami było ciężko.
    Praca zrobiona i obroniona na piątkę :) Serwonapęd działa świetnie. Kto chce może zobaczyć na youtube. Szukajcie po nicku.