Saturday, March 27, 2010

How to program the Pololu 3pi Robot using C language - "Motor Control"

Pololu 3pi Robot


如上圖 [Ref.1]， Pololu 3pi Robot是一台小型的Robot， 主要依靠一顆AVR微處理器(AVR Atmel ATmega328P)來控制LCD、 LED、Motors...等各項硬體設備。 ATmega328P提供了ISP(In-System Programming)的功能， 我們可以利用C語言來控制Pololu 3pi Robot，例如在Robot的LCD上show出文字， 或是讓Robot走直線、走迷宮。 下面兩張圖分別是Pololu 3pi Robot的正反面， 圖上並且標記一些硬體設備的名稱 [Ref.1]。 



Motor
關於Motor的部份，它負責提供能量給3pi，並使用TB6612FNG dual motor driver [Ref.2]。當然，Pololu AVR Library已經提供一些Library來控制motor，但這邊想探討的是比較low-level的motor control [Ref.3]。由上圖我們可以看到3pi有兩個motor，Motor 1連接接腳PD5跟PD6，Motor 2連接接腳PD3跟PB3。這些接腳會連接ATmega328p的4個8-bit hardware PWM outputs (PD5=OC0B, PD6=OC0A, PD3=OC2B, and PB3=OC2A)，我們可以透過這些硬體上的timers來控制3pi的速度，而不需經由軟體來控制，它的好處是可以讓CPU有多點心思可以花在其它任務上，至於怎麼去控制在稍後會討論。

I/O-Ports
要控制motor前，我們需要先知道它的硬體是怎麼規劃及怎麼接線的。不同的AVR Microcontroller型號，有其不同的I/O Port數目及I/O接腳。AVR的I/O Port皆有3個8-bit I/O暫存器(在datasheet裏稱之為I/O memory address locations)，分別為PORTx、DDRx及PINx，其中x代表I/O Port的編號，而其意義分別簡述如下:
  1. Data Register – PORTx



    • 當I/O Port被設定為output，則PORTx會用來輸出資料
    • 當I/O Port被設定為input，則PORTx另有它用(可參考datasheet [Ref.4])

  2. Data Direction Register – DDRx
    此暫存器用來設定    I/O Port為input或output，所有I/O Port的每個接腳皆可以獨立設定



    • 當接腳設為0，則I/O Port設定為input
    • 當接腳設為1，則I/O Port設定為output

  3. Port Input Pins – PINx
    此暫存器只能read    ，不能write。當DDRx設定為input，可以從此暫存器讀取外部接腳信號。

Variable Speed Control Using Hardware PWMs
ATmega328p有兩個8-bit timers: Timer0與Timer2，
Each of these timers has two PWM output pins. The Timer0 PWM output pins are OC0A on PD6 and OC0B on PD5; these are the control lines for motor 1. The Timer2 PWM output pins are OC2A on PB3 and OC2B on PD3; these are the control lines for motor 2. ~by [Reference 3]
那麼，到底怎麼利用PWM來控制motor的速度?太細部的電子電路相關理論在這恐怕沒辦法說得太細，而在ATmega328p是支援PWM的前提下，整體觀念應該可以這樣來說：
  • 首先，我們應該先瞭解何謂PWM，它是Pulse-Width Modulation的縮寫，它長得像下圖[Ref.5]



                _      __     ___    _____  _      _____  __     _     
           | |    |  |   |   |  |     || |    |     ||  |   | |    
PWM Signal | |    |  |   |   |  |     || |    |     ||  |   | |    
         __| |____|  |___|   |__|     || |____|     ||  |___| |____


    而PWM的其中一個應用，就是其對電流的控制。 
  • 接著我們必須瞭解Duty cycle，簡單來講它的意思就是，在PWM波的一個時間周期中，高電位所佔的比例，即稱為Duty cycle
  • Duty cycle越高，其速度是越快的，因此我們控制速度的主要依據就是兩個8-bit timers，它可以計時高低電位持續的時間
  • 至於motor的轉動方向，所依據的是H-bridge的原理，H-bridge是一個常被用在robot的電子電路，它有四個switch(S1、S2、S3、S4)可以控制motor的轉動方向，如下面兩圖示[Ref.6]，



    • 當S1及S4為連接的，而S2及S3為非連接的，則motor方向會向右(順時針轉動)
    • 當S2及S3為連接的，而S1及S4為非連接的，則motor方向會向左(逆時針轉動)



Demo
利用Variable Speed Control Using Hardware PWMs，加上H-bridge的原理，我們可以控制3pi的方向跟速度，這邊我測試了一個簡單的motor control，也就是以一個中心為原點，讓3pi以順時針的方向，來回的行走於各個角度，再回到原中心點，這個demo的目的只是要簡單show出3pi的前進、後退及轉彎，下圖中是我跟我妹借的一些Hello Kitty當Model:

然後，這是可愛的Hello Kitty們跟3pi的合照XD
 
Hello Kitty說穿了其實只是裝飾用的(真是辛苦她們了XD)，切入正題，我想做的是讓Hello Kitty將3pi圍成一圈，3pi將以順時針方向來回拜訪每個Hello Kitty，如下圖示
 
接著，來看看成果吧 :)



Source Code
下面是Source Code，首先是標頭檔

// Modified by Trek Lee to work with the Pololu 3pi Robot

#ifndef _DEVICE_
#define _DEVICE_

#ifndef F_CPU
#define F_CPU 20000000 // system clock is 20 MHz
#endif //!F_CPU

/* Data Register, Port B */
#define    PB7      7
#define    PB6      6
#define    PB5      5
#define    PB4      4
#define    PB3      3
#define    PB2      2
#define    PB1      1
#define    PB0      0

/* Data Register, Port D */
#define    PD7      7
#define    PD6      6
#define    PD5      5
#define    PD4      4
#define    PD3      3
#define    PD2      2
#define    PD1      1
#define    PD0      0
 
#endif //_DEVICE_


接著是主程式
/*
 * 3pi-circularly saunter - demo code for the Pololu 3pi Robot
 * 
 * This code will circularly saunter, using PWM-Based Motor Control Code.
 * It demonstrates circularly movement of 3pi motor control.
 * 
 * Source code is partially reference from Application Note: 
 * Using the Motor Driver on the 3pi Robot and Orangutan Robot Controllers, 
 * http://www.pololu.com/docs/pdf/0J15/motor_driver_application_note.pdf
 *
 * http://seeyababy.blogspot.com/
 *
 */
 

#include <avr/io.h>
#include "device.h" 
#include <util/delay.h> // F_CPU is defined in "device.h" above

// Motor Control Functions -- pwm is an 8-bit value
// (i.e. ranges from 0 to 255)
void M1_forward(unsigned char pwm)
{
 OCR0A = 0;
 OCR0B = pwm;
}
void M1_reverse(unsigned char pwm)
{
 OCR0B = 0;
 OCR0A = pwm;
}
void M2_forward(unsigned char pwm)
{
 OCR2A = 0;
 OCR2B = pwm;
}
void M2_reverse(unsigned char pwm)
{
 OCR2B = 0;
 OCR2A = pwm;
}

// Motor Initialization routine -- this function must be called
// before you use any of the above functions
void motors_init()
{
 // configure for inverted PWM output on motor control pins:
 // set OCxx on compare match, clear on timer overflow
 // Timer0 and Timer2 count up from 0 to 255
 TCCR0A = TCCR2A = 0xF3;
 // use the system clock/8 (=2.5 MHz) as the timer clock
 TCCR0B = TCCR2B = 0x02;
 // initialize all PWMs to 0% duty cycle (braking)
 OCR0A = OCR0B = OCR2A = OCR2B = 0;
 // set PWM pins as digital outputs (the PWM signals will not
 // appear on the lines if they are digital inputs)
 DDRD |= (1 << PD3) | (1 << PD5) | (1 << PD6);
 DDRB |= (1 << PB3);
}

// delay for time_ms milliseconds by looping
// time_ms is a two-byte value that can range from 0 - 65535
// a value of 65535 (0xFF) produces an infinite delay
void delay_ms(unsigned int time_ms)
{
 // _delay_ms() comes from <util/delay.h> and can only
 // delay for a max of around 13 ms when the system
 // clock is 20 MHz, so we define our own longer delay
 // routine based on _delay_ms()
 unsigned int i;
 for (i = 0; i < time_ms; i++)
  _delay_ms(1);
}

int main()
{
    
 motors_init();
 delay_ms(2000);


 while(1)
 {

  M1_forward(25);
  delay_ms(1000);

  //motionless
  M1_forward(0);
  delay_ms(1000);

  //forward
  M1_forward(25);
  M2_forward(25);
  delay_ms(2000);

  //motionless
  M1_forward(0);
  M2_forward(0);
  delay_ms(2000);

   //reverse
  M1_reverse(25);
  M2_reverse(25);
  delay_ms(2000);

  //motionless
  M1_forward(0);
  M2_forward(0);
  delay_ms(1000);

 } 
 
 return 0;
}
  

Conclusion
這篇文章首先簡單介紹了Pololu 3pi Robot，它採用AVR Atmel ATmega328P微處理器，並且提供In-System Programming。接著，說明了ATmega328P的I/O Port及各個Port所代表的意義，而motor連結了具PWM 輸出的Port，透過PWM的輸出，加上H-bridge的原理，讓我們可以控制motor的速度及方向。最後，透過Demo來呈現motor的控制效果。

Coming soon...
3pi在目前這個project是依照我們自己設定的角度(值)來轉彎，然而，3pi本身有個Line Following [7]的功能(sensor)，因此，在未來我們將會利用Line Following的功能來引導3pi，可以預期的是Line Following將節省我們手動去調那些轉彎角度的時間，3pi只需依照規定路線去跑即可完成任務。
 
[Reference]
  1. Pololu 3pi Robot User's Guide, http://www.pololu.com/docs/0J21
  2. Toshiba Bi-CD Integrated Circuit Silicon Monolithic
    T B 6 6 1 2 F N G, http://www.pololu.com/file/download/TB6612FNG.pdf?file_id=0J86
  3. Application Note: Using the Motor Driver on the 3pi Robot and Orangutan Robot
    Controllers, http://www.pololu.com/docs/pdf/0J15/motor_driver_application_note.pdf
  4. AVR Solutions - Datasheets, http://www.atmel.com/dyn/resources/prod_documents/8271.pdf
  5. Pulse-width modulation, http://en.wikipedia.org/wiki/Pulse-width_modulation
  6. H-bridge, http://en.wikipedia.org/wiki/H-bridge
  7. Line Following, http://www.pololu.com/docs/0J21/7.a

張貼者： Trek 於 9:28 PM 0 意見  

標籤： ,

Subscribe to: Posts (Atom)