General Servo Controller by PIC (CCS Compiler)

     General Servo Controller by PIC (CCS Compiler)

              คือปกติแล้วในการ Control Servo Motor นั้นเราต้องป้อนพัลส์ไปยังขา Signal ของ Servo Motor ครับ ซึ่งพัลส์นั้นจะมีความกว้างตั้งแต่ 0.5mS - 1.5mS. ครับโดยที่ 0.5mS นั้น Servo Motor จะหมุ่นมาที่ -90 องศาครับส่วน 1.5mS. นั้นจะหมุนไปที่ +90 องศา ถ้าพัลส์ปกติ (1.0mS)ก็จะหมุนมาที่ 0 องศา ครับ ดังนั้นการเขียนโปรแกรมให้ Servo ทำงานได้ต่อเนื่องนั้นต้องเขียนให้การทำงานในส่วนของ Servo เป็น Background Process ครับ คือโปรแกรมของผมจะเขียนโดยให้ Interrupt Timer เป็น Background Process แทนครับ มาดูวีดีโอที่ผมทดลอง Simulate กันครับว่ามอเตอร์นั้นทำงานอย่างไร

              ความสามารถของ Lib คือต่อ Servo เข้าขาไหนก็ได้ของ PIC แค่เป็นขาที่สามารถเป็น output ได้เป็นพอครับ

กำหนดค่าในโปรแกรมนิดหน่อยก็ใช้งานได้แล้วครับ เดี๋ยวมาดูโค้ดกันก่อนครับ

 

/*

Name : General Servo Controller.

Compiler Support : CCS C Compiler.

Designed by : Wittawat Sompong @ RMUTI

Software use timer_1 and CCP1 in MCU.

*/

#include <16f877.h>

#fuses HS,NOLVP,NOWDT,NOPROTECT

#use delay(clock=20M)

//-------------------------General config----------------------------------//

#define SERVO_MAX 5 //Max of Server connector

#define SERVO_MAX_VALUE 2500 //Max of pulse width micro sec(uS)

#define SERVO_MIN_VALUE 500 //Min of pulse width micro sec(uS)

typedef struct tServo{

volatile unsigned pin;

volatile unsigned Active;

volatile unsigned int16 value;

}Servo;

//------------------------Start Prototype-----------------------------------//

void ServoActive(unsigned chanel,unsigned vPin,unsigned int16 vValue);

void ServoValue(unsigned chanel,unsigned int16 value);//value is uS.

//------------------------End of Prototype---------------------------------//

//------------------------Servo gobal variable----------------------------//

signed ServoCnt=0;

Servo nServo[SERVO_MAX];

//------------------------Servo background task use CCP1 interrupt---------//

#INT_CCP1

void ServoTask()

{

//if enc of servo count ,reset timer1.

if(ServoCnt<0){

set_timer1(0);

}else{

//if server pin is active set output servo pin to low.

if(nServo[ServoCnt].Active)

output_low(nServo[ServoCnt].pin);

}

ServoCnt++;

if(ServoCnt<SERVO_MAX)

{

//set interrupt to next servo time.

CCP_1 = get_timer1()+nServo[ServoCnt].value;

//if servo pin is active set output servo pin to high.

if(nServo[ServoCnt].Active)

output_high(nServo[ServoCnt].pin);

}else{

//set fist chanel to start scan.

CCP_1 = get_timer1()+100;

ServoCnt = -1;

}

}

/*

ServoActive(c,p,v)

set pin 'p' to connect servo chanel 'c' and set pulse width 'v' uS.

Example : ServoActive(0,PIN_A0,1500);

set PIN_A0 to connect servo at chanel '0' and set pulse width 1500 uS.

*/

void ServoActive(unsigned chanel,unsigned vPin,unsigned int16 value)

{

nServo[chanel].pin = vPin;

nServo[chanel].value = ((value*5)/8);

nServo[chanel].Active = 1;

}

/*

ServoValue(c,v)

Set servo chanel 'c' to pulse width 'v' uS.

Example : ServoValue(0,2400);

set servo chanel '0' to pulse width 2400 uS. (2.4mS)

*/

void ServoValue(unsigned chanel,unsigned int16 value)

{

if(value>SERVO_MAX_VALUE)

value=SERVO_MAX_VALUE;

if(value<SERVO_MIN_VALUE)

value=SERVO_MIN_VALUE;

if (chanel>=SERVO_MAX)

chanel=SERVO_MAX-1;

nServo[chanel].value = ((Value*5)/8);

}

void main()

{

unsigned i;

//-----------------------------------------Setup timer1 to servo task-----------------------------//

setup_timer_1 (T1_INTERNAL|T1_DIV_BY_8);

setup_ccp1 (CCP_COMPARE_INT);

enable_interrupts(GLOBAL);

enable_interrupts(INT_CCP1);

//-----------------------------------------------------------------------------------------------//

//Add pin_d0 to connect servo chanel '0'

ServoActive(0,PIN_D0,1500);

ServoActive(1,PIN_D1,1500);

ServoActive(2,PIN_B0,1500);

ServoActive(3,PIN_B1,1500);

delay_ms(1000);

while(true)

{

for(i=0;i<4;i++)

{

ServoValue(i,1000);

//delay_ms(200);

}

delay_ms(1000);

for(i=0;i<4;i++)

{

ServoValue(i,1500);

//delay_ms(200);

}

delay_ms(1000);

for(i=0;i<4;i++)

{

ServoValue(i,2000);

//delay_ms(200);

}

delay_ms(1000);

for(i=0;i<4;i++)

{

ServoValue(i,1500);

//delay_ms(200);

}

delay_ms(1000);

}

}

                   หลักๆที่ท่านจะได้ใช้คือ ServoActive() ครับเป็นฟังก์ชั่นที่เอาไว้เพิ่มหรือกำหนดว่าเราจะต่อขาอะไรเข้ากับ Servo

                  ตัวอย่างเช่น ผมต้องการต่อขา PIN_D0 เข้า Servo ช่องที่ 0 และตั้งให้พัลส์ที่ขาออก 1.5mS 

ผมก็เขียนว่า ServoActive(0,PIN_D0,1500); ถ้าผมต้องการเปลี่ยนองศาการหมุนของมัน

ไปเป็น +90 องศา ก็จะเขียน ServoValue(0,1000);

                   การใช้งานเบื้องต้นมีเท่านี้ครับ ใช้ง่ายครับ แต่โค้ดต้นฉบับเองผมได้ดูมาจาก Arduino 

และดัดแปลงมาเขียนบน AVR Studio และก็แปลงมาเป็น CCS อีกที

เดี๋ยวตั้วหน้าจะเอาเวอร์ชั่น AVR มาให้ครับ พอดียังไม่ได้ลงโปรแกรม AVR Studio ครับเลยยังไ่ม่มีเวลาทดสอบ

                  ส่วนไกล้ๆงานย่าโมโรโบคอนปีนี้ เดี๋ยวจะเอาโค้ดโปรแกรมเซ็นเซอร์ตรวจจับเส้น

ที่เท่ไม่หยอกและบางคนอาจจะคิดไม่ถึงมาให้ดูกันครับ 

ส่วนโปรแกรมนี้ท่านไหนสงสัยส่วนไหน หรือมีอะไรติชม ก็ยินดีครับ

โค้ดครับ : https://docs.google.com/file/d/0B9HppoQOVif0M3FTTnZHV2pNVzg/edit?usp=sharing

ปล.ผมคอมเม้นในโค้ดเป็นภาษาอังกฤษนะ อาจะดูกระด้างไปหน่อย ครั้งหลังจะปรับปรุงให้ดีกว่านี้ครับ ;)

ขอบคุณครับ

โอ.... ;D