PIC Vietnam - đồ án chế tạo robot rắn.(worm robot)

em vừa bảo vệ xong đồ án tốt nghiệp về robot này, đã hoàn thành, di chuyển như rắn luôn robot của em muốn phát triển thêm lắp nhiều cảm biến để có thể leo cầu thang, nhưng kinh phí tốn quá nên đành chịu, anh em nào muốn phát triển thêm robot này không. Nếu phát triển thêm em chắc chắn robot sẽ có ứng dụng nhiều lắm.

đây là video robot rắn.
http://www.youtube.com/watch?v=0uRJrdJxAjE

robot được làm từ những vật liệu đơn giản là nhựa, robot ghép lại từ nhiều khúc với nhau, các khớp đều là khớp quay, và điều khiển bằng Pic 18f4431, điều khiển 8 động cơ servo RC. để phối hợp chuyển động đồng bộ, e dùng 2 timer1, 5(16 bit ) tạo xung điều khiển luôn cho cả 8 servo...

Và đây là code lập trình.
#include<18f4431.h>
#fuses XT,NOLVP,NOWDT,PUT
#use delay (clock=4000000) // defind crystal = 4MHz
#use Fast_IO(C)
//#use rs232(baud=19200,xmit=PIN_C6, rcv=PIN_C7)
// Uses RS232 Baud-rate 19200 , TX PIN = RC6, RX PIN = RC7
static int mark1, mark2, mark3, mark4, mark5, mark6, mark7, mark8;
static int16 count1, count5;
static int16 loop1, loop5;

int16 const q1[]={ 64365,64335,64305,64265,64225,64195,64165,64125,64 095,64075,64075,64085,64105,64135,64165,64205,6424 5,64275,64305,64345,64375,64395,64395,64385,}; //63
int16 const q2[]={ 64365,64405,64415,64425,64415,64395,64355,64325,64 285,64245,64195,64135,64105,64065,64055,64045,6405 5,64075,64115,64145,64185,64225,64275,64335,};//63
int16 const q3[]={ 64105,64145,64205,64245,64295,64335,64385,64405,64 415,64415,64405,64385,64365,64325,64265,64225,6417 5,64135,64085,64065,64055,64055,64065,64085,}; //63
int16 const q4[]={ 64105,64075,64045,64055,64065,64085,64105,64145,64 205,64245,64295,64345,64365,64395,64425,64415,6440 5,64385,64365,64325,64265,64225,64175,64125,};//63
int16 const q5[]={ 64355,64305,64265,64205,64155,64125,64095,64075,64 045,64045,64055,64085,64115,64165,64205,64265,6431 5,64345,64375,64395,64425,64425,64415,64385,}; //63
int16 const q6[]={ 64375,64405,64415,64425,64415,64375,64345,64305,64 265,64225,64175,64125,64095,64065,64055,64045,6405 5,64095,64125,64165,64205,64245,64295,64345,};//63
int16 const q7[]={ 64125,64165,64225,64265,64315,64365,64385,64405,64 425,64415,64405,64375,64345,64305,64245,64205,6415 5,64095,64125,64065,64045,64055,64065,64095,}; //63
int16 const q8[]={ 64095,64095,64075,64095,64105,64135,64185,64225,64 245,64295,64325,64375,64375,64375,64395,64375,6436 5,64335,64285,64245,64225,64175,64145,64095,};

int16 const Max1 =23;
int16 const Max5 =23;
static int V1, V5;

#INT_TIMER1
void q_timer1() {

disable_interrupts(GLOBAL);

if (count1>=Max1){
count1=1;
loop1=0;
}
//loop++;
if (count1==0)
V1=50;
else
V1=1;

if(loop1>V1)
{
loop1=1;
if(count1<=count5)

count1++;

}

if ((mark1==1)&(mark2==1)&(mark3==1)&(mark4==1))
{
set_timer1(50536);
output_low(PIN_C0);
output_low(PIN_C1);
output_low(PIN_C2);
output_low(PIN_C3);
mark1=2;
mark2=2;
mark3=2;
mark4=2;
loop1++;
}else if(mark1==2){
set_timer1(q8[count1]);
output_high(PIN_C0);
mark1=1;
}else if(mark2==2){
output_low(PIN_C0);
set_timer1(q7[count1]);
output_high(PIN_C1);
mark2=1;
}
else if(mark3==2){
output_low(PIN_C1);
set_timer1(q6[count1]);
output_high(PIN_C2);
mark3=1;
}
else if(mark4==2){
output_low(PIN_C2);
set_timer1(q5[count1]);
output_high(PIN_C3);
mark4=1;
}

enable_interrupts(GLOBAL);

}

#INT_TIMER5
void q_timer5() {

disable_interrupts(GLOBAL);

if (count5>=Max5){
count5=1;
loop5=0;
}
//loop1++;
if (count5==0)
V5=50;
else
V5=1;

if(loop5>V5)
{
loop5=1;
if(count5<=count1)

count5++;

}

if ((mark5==1)&(mark6==1)&(mark7==1)&(mark8==1))
{
set_timer5(50536);
output_low(PIN_C4);
output_low(PIN_C5);
output_low(PIN_C6);
output_low(PIN_C7);
mark5=2;
mark6=2;
mark7=2;
mark8=2;
loop5++;
}else if(mark5==2){
set_timer5(q4[count5]);
output_high(PIN_C4);
mark5=1;
}else if(mark6==2){
output_low(PIN_C4);
set_timer5(q3[count5]);
output_high(PIN_C5);
mark6=1;
}
else if(mark7==2){
output_low(PIN_C5);
set_timer5(q2[count5]);
output_high(PIN_C6);
mark7=1;
}
else if(mark8==2){
output_low(PIN_C6);
set_timer5(q1[count5]);
output_high(PIN_C7);
mark8=1;
}

enable_interrupts(GLOBAL);

}

//================================================== ============================
main()
{
set_tris_C(0);
output_C(0x0);
mark1=1;
mark2=1;
mark3=1;
mark4=1;
mark5=1;
mark6=1;
mark7=1;
mark8=1;
loop1=1;
count1=0;
loop5=1;
count5=0;
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
setup_timer_5(T5_INTERNAL|T5_DIV_BY_1);
enable_interrupts(GLOBAL);
enable_interrupts(INT_TIMER1);
enable_interrupts(INT_TIMER5);

while(TRUE);

}

//delay (in ms) = (# ticks) * 4 * prescaler * 1000 / (clock frequency)
//delay (in ms) = (# ticks) * 4 * prescaler * 1000000 / (clock frequency)
//sticks=65535-number
//20=count*4*1000/4000000 count=45535
//1.5=count*4*1000/4000000 count=64035
//2=count*4*1000/4000000 count=63535
//1.3 64235
//1.8 63735
//0.6 count=64935
//2.4 count=63135

//20 = n * 4 *8 * 1000 / 4000000
//q3=256-n
//2=n * 4 *8 * 1000 / 4000000

vài bức ảnh về chú robot rắn của t :

http://i264.photobucket.com/albums/i...6/RECO0013.jpg
http://i264.photobucket.com/albums/i...6/RECO0018.jpg
http://i264.photobucket.com/albums/i...b86/IMG115.jpg