Elektroda.pl
Elektroda.pl
X

Search our partners

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

pojazd mobilny/minisumo kod C

mankut211 28 Nov 2013 11:57 1242 4
  • #1
    mankut211
    Level 9  
    Witam,
    od jakiegoś czasu staram się uporać z pojazdem typu minisumo (2 kołowym)

    w zasadzie jestem już na końcu swoich zmagań jednak zacząłem robić regulator obrotów silnika. Chodzi o to że prawe koło ma gonić lewe koło w taki sposób zeby ich liczba obrotów się finalnie zgadzała. użyłem do tego prostych czujników bramkowych i ,,tachografu".
    Do tego zostały zamontowane czujniki zbliżeniowe sharpa (10cm ) do wykrywania i omijania przeszkód.

    na początku gdy nie miałem regulatora robot działał poprawnie jednak po zaimplementowaniu regulatora coś jest nie tak.

    Problem polega na tym że w momencie kiedy startuje swojego robocika regulator liczy poprawnie, i robot jedzie prost, jednak kiedy wykryje przeszkodę ominie ją ale nie chce wrócić do funkcji jechania prosto i wciąż stara się coś omijać tzn. nie chce jechać prosto tylko silniki nie chcą wystartować na wartosć zadaną.

    Mam nadzieje że ktoś da radę pomóc mi w tym zapewne błahym problemie.
    Z góry dziękuje.

    Poniżej kod programu

    #define F_CPU 1000000L
    #include <avr/io.h>
    #include <util/delay.h>
    #include <stdio.h>
    #include <stdlib.h>
    #include <avr/interrupt.h>
    #define stalaI 0.004 //Stała czasowa całkowania
    #define stalaD 0.001 //Stała czasowa różniczkowania
    #define wzmocnienieP 10

    /* PROGRAM */

    int main(void)
    {

    double czas=0.001,calka;

    double sterowanie = 0,uchyb,uchyb_pop;
    unsigned int right=0,left=0,predkosc=200;
    int predkoscprawa=200,predkosclewa=200;
    // Konfiguracja portów we/wy
    DDRD = (1<<4)|(1<<5)|(1<<7);
    DDRB = (1<<3);

    DDRA = 0x00; //czujniki
    PORTC = 0xff;
    DDRC = 0x00; //czujniki
    PORTC = 0xff;// OC1A i OC1B wyjścia PWM

    /* Ustawienie sprzętowego PWM */
    /* ATmeta8 1MHz, PWM 2.5kHz, do OCR1A, 0CR1B wstawiać wartości 0-399 */


    // Fast PWM mode
    TCCR1A=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11);
    TCCR1B=(1<WGM12)|(1<<CS10)|(1<<CS11);
    TCCR0=(1<<COM01)|(1<<WGM01)|(1<<WGM00)|(1<<CS00)|(1<<CS01);
    TCCR2=(1<<COM21)|(1<<WGM21)|(1<<WGM20)|(1<<CS22);




    //jazda w tył
    //(jazda w przód)



    //koło lewe patrzac z przodu (jazda w przód)
    //port PD5 (jazda w tył)

    while(1)
    {


    if(!(PINC & 8))
    {

    right++;

    }

    if(!(PINC & 10))
    {
    left++;

    }




    if(!(PINC & 0x02)) //prawy czujnik patrząc od przodu prawe koło
    {
    OCR0 = 0; //jazda prosto
    OCR1A = 0;
    _delay_ms(100);
    OCR1B = 200; //skręt w lewo
    OCR2 = 50;
    _delay_ms(100); //
    }










    if(!(PINC & 0x01)) //lewy czujnik patrząc od przodu lewe koło
    {
    OCR0 = 0; //jazda prosto
    OCR1A = 0;
    _delay_ms(100);
    OCR1B = 100; //skręt w lewo lewe kolo
    OCR2 = 100;
    _delay_ms(100);
    //
    }










    if(!(PINC & 0x80)) //lewy czujnik od przodu
    {
    OCR0 = 0; //jazda prosto
    OCR1A = 0;
    _delay_ms(100);
    OCR1B = 100; //skręt w lewo
    OCR2 = 100;
    _delay_ms(100);
    // //

    }








    if(!(PINC & 0x40)) //prawy czujnik od przodu
    {
    OCR0 = 0; //jazda prosto
    OCR1A = 0;
    _delay_ms(100);
    OCR1B = 200; //skręt w lewo
    OCR2 = 50;
    _delay_ms(100); //

    }










    if(!(PINA & 0x04)) //TY L
    {

    OCR0 = 100; //jazda prosto
    OCR1A = 100;
    //
    }

    else
    {


    calka = (calka + uchyb) * czas;

    uchyb=(left - right); //obliczenie uchybu aktualnego

    sterowanie = (wzmocnienieP * uchyb) + (stalaD * ((uchyb - uchyb_pop)/czas)) + stalaI * calka; //obliczenie sterowania


    predkosclewa=(left - sterowanie); //ustawienie lewego PWM
    predkoscprawa=(left + sterowanie); //ustawienie prawego PWM

    uchyb_pop=uchyb;





    OCR0 = predkosclewa; //jazda prosto
    OCR1A = predkoscprawa;


    }
    }
    }
  • #2
    mankut211
    Level 9  
    Czy jest ktoś w stanie rozwiązać ten problem ;>
  • #3
    domind
    Level 14  
    Witam,
    troche slabo opisales swoj problem, pozatym jesli chodzi o kodowanie to chyba lepiej szukac pomocy w dziale mikrokontrolerow.

    "ATmeta8" - domyslam sie ze chodzi o ATmega8.
    Kod ma slabe komentarze, trzeba sie mocno domyslac co gdzie masz podpiete.

    Ja sie bawilem z Atmega8a, ale tam nie ma OCR0.

    Code: c
    Log in, to see the code

    10 jest chyba dziesietne, a wydaje mi sie ze chciales miec 0x10.

    OCR2 - rzeczywiscie to chcesz ustawiac? W/g komentarzy wydawalo mi sie ze silniki masz pod PB1 i PB2.
    Zobacz tez sobie na forbot - tam jest ladne rozwiazanie skrecania, bez takich mnozen i liczb rzeczywistych.
  • #4
    mankut211
    Level 9  
    faktycznie nie napisałem jaka atmega - atmega32

    Tak masz rację tam chciałem 0x10 ;P ale to nie zmienia pracy całego układu o dziwo.

    a co do OCR0 i OCR1A - tak tam chce wpisywać bo chodzi o PWM nic nie pisałem o PB1 PB2

    faktycznie napisane w komentarzu jest Atmega8 (częśc kodu brałem z internetu jak uczyłem się konfigurować te porty )
  • #5
    domind
    Level 14  
    Jak dlugi jest impuls od "tachografu", moze program gubi odczyt bo jest zajety czyms innym? np. liczeniem "sterowanie = .."

    right, left - nigdzie poza poczatkiem nie zerujesz/wyrownujesz wartosci. Kiedy i jakie wartosci to dostaje?

    "predkosclewa=(left - sterowanie); " nie wywala Ci tu bledu/warning rzutowania double do int ? Nawet jak nie, to wydaje mie sie ze moze byc problem z zaokragleniami. Przynajmniej do testow - zrob to zwyklym dodawaniem na int. np. sterowanie1=uchyb_nowy*mnoznik; uchyb_nowy=uchyb_nowy/2;
    Poczytaj czy z double nie ma problemow pod avr, sam nie uzywalem, ale widzialem ze ktos polecal raczej float. Jak mozesz to sprobuj to zrobic i zostawic na int.


    Zrob dokladniejszy opis, bo na razie to sie domyslam, ze OCR0, OCR1A jest do jazdy do przodu, a OCR1B, OCR2 do tylu, ale pewien nie jestem.