WatchMe Robot

     A mai bejegyzés szervó vezérlés elméletet valamint egy FPGA-ra megvalósított változatot fog tárgyalni. Szükséges eszközök :

• Futaba S3003 szervó

• Nexys 2 FPGA fejlesztőlap

• Külső tápegység 

• PMOD CON3

• Xilinx SDK
• Firefox vagy hasonló böngésző
• Photoshop
• egy darab papír, olló és ragasztószalag

     Legelső lépés, keressünk rá google-ben, hogyan kell vezérelni egy szervómotrot. A szervómotor egy belső visszacsatoló- és vezérlőkörrel rendelkező kis motor, ami azért jó, mert nem kell mi azzal foglalkozzunk, hogy ellenőrizzük, vajon eljutott már a megfelelő pozícióba, vagy sem. (Persze most sokan felhördülnek, hogy terhelés mellett lehet olyan, hogy a fogaskerekek elcsúsznak egymás mellett, de kit érdekel az ilyen? :P)

     Vezérlése egy PWM jellel valósítható meg. Modelltől és tipustól függően más és más értékekkel. Az általánosan elfogadott változat a következő : 20 ms-os periodussal, perióduson belül 1-2 ms közötti kitöltési tényezővel juttathatjuk el az egyik végállásból a másik végállásba. 1.5 ms-os kitöltési tényező a középállás.

      Találtam sok hasonló ábrát, de a lényegük mindnek ugyanez.


     Frekvencia, kitöltési tényező, ennyi. Következő lépés megkeresni a szervó lábkiosztását. Ez is legtöbb szervónál ugyanaz, hogy könnyebben lehessen cserélni őket.  Fehér kábelen megy a jel, vagyis a kitöltési tényező, piros a pozitív, fekete a föld.


     Fogjuk, s rányomjuk a PMODCon3-as csatlakozóra, majd a csatlakozót összekötjük a labortáppal. Labortápon beállítunk valami 4.6 és 6 volt közötti értéket majd nekikezdünk programozni.

     Először megírni a customIP-t, mivel az SDK alapból nincs felszerelve ilyen elemekkel.

entity my_pwm is
    Port ( clk               : in  STD_LOGIC;
           max_count         : in  STD_LOGIC_VECTOR (31 downto 0);
            on_time           : in  STD_LOGIC_VECTOR (31 downto 0);
           pwm_out           : out STD_LOGIC;
              reset             : in  STD_LOGIC
              );
end my_pwm;

architecture Behavioral of my_pwm is

signal counter               : std_logic_vector(31 downto 0):=(others=>'0');


begin

    PwmCounter: process(clk, max_count)
        begin
                if clk'event and clk='1' then
                    if counter >= max_count then
                        counter <= (others=>'0');
                    else
                        counter <= counter + '1';    
                    end if;        
                end if;
        end process;

    PWM: process(counter, on_time, reset )
        begin
                if (counter<on_time) and reset = '0' then 
                    pwm_out <= '1';
                else     
                    pwm_out <= '0';
                end if;    
        end process;
        
    
   
end Behavioral;

     A PwmCounter processz segitségével generáljuk a megfelelő frekvenciát (50 Hz, vagyis az egy másodpercnyi órajelet 50 részre osztjuk fel ahhoz, hogy megkapjuk a max_count értékét). Mivel az én órajelem 50.000.000 Hz-es, igy nálam a max_count értéke 1.000.000 lesz.

    A PWM processzus felelős a kitöltési tényező beállításáért. Amíg a számláló értéke kisebb, mint az on_time értéke, addig logikai "1" van a kimeneten, amikor meg nagyobb, akkor logikai "0".

    Az így elkészült VHDL állományt importáljuk a Xilinx SDK fejlesztőkörnyezetbe, mint egy CustomIP. Ha ügyesek voltunk, meg is jelenik a többi IP alatt.


     Használatához szükség van a következő sorokra (teljes programot nem másolom be...)

//(...)
#include "servo_controller.h"


//====================================================
//             Definitions
#define SERVO_ADDRESS XPAR_SERVO_CONTROLLER_0_BASEADDR
#define FPGA_CLK 50000000 //50 MHz

unsigned int max_count=0;
unsigned int on_time=0;

//             END OF USER DEFINITIONS
//====================================================

//====================================================
//                    FUNCTIONS
void Set_Servo_Position(unsigned int value)
{
    if (value<=0)
        max_count=FPGA_CLK/50;
      on_time = 20000+(value*400);
   SERVO_CONTROLLER_mWriteSlaveReg0(SERVO_ADDRESS, 0, max_count);
    SERVO_CONTROLLER_mWriteSlaveReg1(SERVO_ADDRESS, 0, on_time); 
}
//(...)
//            END OF USER FUNCTIONS
//====================================================

 
int main (void) 
{
//(...)

    while(1)
    {
        xil_printf("Servo Position Value = 2\n");
        Set_Servo_Position(0);
        sleep(80);
        
        xil_printf("Servo Position Value = 115\n");
        Set_Servo_Position(115);
        sleep(50);

        xil_printf("Servo Position Value = 230\n");
        Set_Servo_Position(230);
        sleep(50);
    }
   return 0;
}

      Itt a programba beírt értékek nem teljesen találnak azzal az 1-2 ms-os leírással... Viszont próbálkozni kell... Oszcilloszkóp híján a szervó motoron. Nagyon kooperatív volt a kicsike...

      Jujj, majdnem elfelejtettem... Olló, papír, ragasztó. Kivágunk a papírból egy nyíl formájú darabot (akinek nem megy magától, az nyomtathat wordban nagy nyilat, s azt körbevágja...), aminek a hátára mindkét oldalán tapadós ragasztószalagot teszünk és felragasztjuk a szervó fejére. Ha ezzel megvagyunk és a program is működik, akkor a következő élményben lehet részünk :




     Akinek elsőre nem megy, az még ne adja fel. Kicsit nehéz eltalálni a szervónak a lelkivilágát, viszont nem lehetetlen...