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 LEDMotors...等各項硬體設備 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跟PD6Motor 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),分別為PORTxDDRxPINx,其中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。接著,說明了ATmega328PI/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

0 意見: