// unsigned long z;
unsigned int s1[9],s2[9];
idata unsigned int s1o[9],s2o[9];
idata unsigned int va,vb,vc,vd,ve,vf,vg,vh,z2,z0;
// idata unsigned long a1,a2,a3,a4,a5,a6,a7,a8;
idata unsigned long xo,xp,xn,xm,xa,xb,xc,xd,xe,xf,xg,xh,xi,xj,xk,xl;
char cc,z3;
int i;
sbit P1_0 = P1^0;
sbit P1_1 = P1^1;
sbit P1_2 = P1^2;
sbit P1_3 = P1^3;
sbit P1_4 = P1^4;
sbit P1_5 = P1^5;
sbit P1_6 = P1^6;
sbit P1_7 = P1^7;
int cho;

void hi_p32(void) interrupt 0 //function interupt timer 1
{/*
z3 = P3;// รับค่าจากพอท P3
// z3 >>= 4;
z3 = z3 & 0xF1; // เอามาแอนกับ11110011

switch(z3){//ได้ค่ามา จัดค่าเข้าทามเมอร์
case 0x10 : s1[1] = P0; s2[1] = P2; break;
case 0x20 : s1[2] = P0; s2[2] = P2; break;
case 0x30 : s1[3] = P0; s2[3] = P2; break;
case 0x40 : s1[4] = P0; s2[4] = P2; break;
case 0x50 : s1[5] = P0; s2[5] = P2; break;
case 0x60 : s1[6] = P0; s2[6] = P2; break;
case 0x70 : s1[7] = P0; s2[7] = P2; break;
case 0x80 : s1[8] = P0; s2[8] = P2; break;
case 0x01 : va = P0; vb = P0; vc = P0; vd = P0;
ve = P0; vf = P0; vg = P0; vh = P0; break;
}
*/
}

void Timer0 (void) interrupt 1 //function interupt timer 1 loop
{ //ทามเมอร์ทำงานตามเวลาที่กำหนดไว้
switch(cho){
case 1 :
TR0 = 0;P1_0 = 0; TH0 = 255-s2o[2]; TL0 = 255-s1o[2]; TR0 = 1;P1_1 = 1; break;
case 2 :
TR0 = 0;P1_1 = 0; TH0 = 255-s2o[3]; TL0 = 255-s1o[3]; TR0 = 1;P1_2 = 1; break;
case 3 :
TR0 = 0;P1_2 = 0; TH0 = 255-s2o[4]; TL0 = 255-s1o[4]; TR0 = 1;P1_3 = 1; break;
case 4 :
TR0 = 0;P1_3 = 0; TH0 = 255-s2o[5]; TL0 = 255-s1o[5]; TR0 = 1;P1_4 = 1; break;
case 5 :
TR0 = 0;P1_4 = 0; TH0 = 255-s2o[6]; TL0 = 255-s1o[6]; TR0 = 1;P1_5 = 1; break;
case 6 :
TR0 = 0;P1_5 = 0; TH0 = 255-s2o[7]; TL0 = 255-s1o[7]; TR0 = 1;P1_6 = 1; break;
case 7 :
TR0 = 0;P1_6 = 0; TH0 = 255-s2o[8]; TL0 = 255-s1o[8]; TR0 = 1;P1_7 = 1; break;
case 8 :
TR0 = 0;P1_7 = 0; TH0 = 255-s2o[1]; TL0 = 255-s1o[1]; break;
case 9 :
TR0 = 0; break;
}

cho++;
if(cho >= 9){cho = 1;}
}

void hi_p33 (void) interrupt 2 //function interupt timer 2
{/*
z3 = P3;
// z3 >>= 4;
z3 = z3 & 0xF1; //1111xx11

switch(z3){
case 0x10 : s1[1] = P0; s2[1] = P2; break;
case 0x20 : s1[2] = P0; s2[2] = P2; break;
case 0x30 : s1[3] = P0; s2[3] = P2; break;
case 0x40 : s1[4] = P0; s2[4] = P2; break;
case 0x50 : s1[5] = P0; s2[5] = P2; break;
case 0x60 : s1[6] = P0; s2[6] = P2; break;
case 0x70 : s1[7] = P0; s2[7] = P2; break;
case 0x80 : s1[8] = P0; s2[8] = P2; break;
case 0x01 : va = P0; vb = P0; vc = P0; vd = P0;
ve = P0; vf = P0; vg = P0; vh = P0; break;
}
*/
}

void Timer1 (void) interrupt 3 //function interupt timer 2
{
//-----------------------------------------------------------------
z3 = P3;
z0 = P0;
z2 = P2;
// z3 >>= 4;
z3 = z3 & 0xF1; //1111xx11

switch(z3){
case 0x10 : s1[1] = z0; s2[1] = z2; break;
case 0x20 : s1[2] = z0; s2[2] = z2; break;
case 0x30 : s1[3] = z0; s2[3] = z2; break;
case 0x40 : s1[4] = z0; s2[4] = z2; break;
case 0x50 : s1[5] = z0; s2[5] = z2; break;
case 0x60 : s1[6] = z0; s2[6] = z2; break;
case 0x70 : s1[7] = z0; s2[7] = z2; break;
case 0x80 : s1[8] = z0; s2[8] = z2; break;
case 0x01 : va = z0; vb = z0; vc = z0; vd = z0;
ve = z0; vf = z0; vg = z0; vh = z0; break;
}
//-----------------------------------------------------------------
TR1 = 0;
TR0 = 0;
TH1 = 0xFF-0x9C; //set timer 1 20ms at 24mh
TL1 = 0xFF-0x40;
//---------------------
// if(s1o[1] < s1[1]){s1o[1] += a1*k1;}
xn = s2[1]*255+s1[1];
// xo = s2o[1]*255+s1o[1];
if(xp < xn){xp += va;if(xp > xn){xp = xn;}}
if(xp > xn){xp -= va;if(xp < xn){xp = xn;}}

// xo++;
s1o[1] = xp % 255 ;
s2o[1] = xp / 255 ;

// if(xo >= 4050){xo=4050;}
// if(xo < 500){xo=500;}

// s1o[1] = s1[1];
// s2o[1] = s2[1];

///--------------------------
xa = s2[2]*255+s1[2];
if(xb < xa){xb += vb;if(xb > xa){xb = xa;}}
if(xb > xa){xb -= vb;if(xb < xa){xb = xa;}}

s1o[2] = xb % 255 ;
s2o[2] = xb / 255 ;
// s1o[2] = s1[2];
// s2o[2] = s2[2];
///--------------------------
xm = s2[3]*255+s1[3];
if(xo < xm){xo += vc;if(xo > xm){xo = xm;}}
if(xo > xm){xo -= vc;if(xo < xm){xo = xm;}}

s1o[3] = xo % 255 ;
s2o[3] = xo / 255 ;
// s1o[3] = s1[3];
// s2o[3] = s2[3];
///--------------------------
xc = s2[4]*255+s1[4];
if(xd < xc){xd += vd;if(xd > xc){xd = xc;}}
if(xd > xc){xd -= vd;if(xd < xc){xd = xc;}}

s1o[4] = xd % 255 ;
s2o[4] = xd / 255 ;
// s1o[4] = s1[4];
// s2o[4] = s2[4];
///--------------------------
xe = s2[5]*255+s1[5];
if(xf < xe){xf += ve;if(xf > xe){xf = xe;}}
if(xf > xe){xf -= ve;if(xf < xe){xf = xe;}}

s1o[5] = xf % 255 ;
s2o[5] = xf / 255 ;
// s1o[5] = s1[5];
// s2o[5] = s2[5];
///--------------------------
xg = s2[6]*255+s1[6];
if(xh < xg){xh += vf;if(xh > xg){xh = xg;}}
if(xh > xg){xh -= vf;if(xh < xg){xh = xg;}}

s1o[6] = xh % 255 ;
s2o[6] = xh / 255 ;
// s1o[6] = s1[6];
// s2o[6] = s2[6];
///--------------------------
xi = s2[7]*255+s1[7];
if(xj < xi){xj += vg;if(xj > xi){xj = xi;}}
if(xj > xi){xj -= vg;if(xj < xi){xj = xi;}}

s1o[7] = xj % 255 ;
s2o[7] = xj / 255 ;
// s1o[7] = s1[7];
// s2o[7] = s2[7];
///--------------------------
xk = s2[8]*255+s1[8];
if(xl < xk){xl += vh;if(xl > xk){xl = xk;}}
if(xl > xk){xl -= vh;if(xl < xk){xl = xk;}}

s1o[8] = xl % 255 ;
s2o[8] = xl / 255 ;
// s1o[8] = s1[8];
// s2o[8] = s2[8];
///--------------------------
s1o[0] = s1[0];
s2o[0] = s2[0];
///--------------------------

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

TH0 = 255-s2o[1]; //ตั้งค่าทามเมอร์ 0
TL0 = 255-s1o[1];

P1_0 = 1;//สังให้ P1 ออน
TR0 = 1; //สั่งทามเมอร์0ทำงานตามปรกติ
TR1 = 1;//สั่งทามเมอร์1ทำงานตามปรกติ
}

void main (void) {

TMOD = 0x11; // เซตทามเมอร์ t0,t1เป็น16 บิด
IE = 0x8A; //เปิดอินเตอร์รับ10001010

TH1 = 0xFF-0x9C; //ตั่งเวลาทามเมอร์ 1ที่ 20ms
TL1 = 0xFF-0x40;// โดยความถึี 24 Mh

cho = 1;// กำหนดค่าตัวแปล cho= ตัวเลือก
P1_0 = 1; P1_1 = 0;// พอท P1_0 = 1
P1_2 = 0; P1_3 = 0;//นอกนั้นเป็น0
P1_4 = 0; P1_5 = 0;
P1_6 = 0; P1_7 = 0;

TR0 = 1; // เปิดการใช้ทามเมอร์ 0
TR1 = 1; //เปิดการใช้ทามเมอร์ 1

for(i=0;i<=9;i++){//วนหรุปใส่ค่ากลางฮาย
s1[i] = 0xFF-0x23;
s2[i] = 0xFF-0xFA;
}
for(i=0;i<=9;i++){//วนหรุปใส่ค่ากลางโล
s1o[i] = 0xFF-0x23;
s2o[i] = 0xFF-0xFA;
}

//ค่ากลาง
xo = 500; xp = 500; xn = 500; xm = 500;
xa = 500; xb = 500; xc = 500; xd = 500;
xe = 500; xf = 500; xg = 500; xh = 500;
xi = 500; xj = 500; xk = 500; xl = 500;

va = 20; vb = 20; //ความเร็ว
vc = 20; vd = 20;
ve = 20; vf = 20; r
vg = 20; vh = 60;
while(1); // ทำซ้ำไม่ให้จบโปรแกรม

โปรแกรมได้แยกออกเป็น3ส่วนดังนี้

หัวด้านบน
การประกาศตัวแปลโดยที่ 51ปกรติมีแรม 128
ซึ่งไม่พอต่อการใช้จึงต้องใช้ 52 ที่มีแรม 256
เวลาการเรียกใช้จึงใช้เป๋น idata รวมกับการประกาศ
ตัวแปลธรรมดา ระวังในการเรียกใช้ให้ดีๆ เพราะถ้า
ใช้ตัวแปลเป็น long จะเปลืองแรมมาก

1.ส่วนเมนล่างสุด (สีน้ำเงิน)

2.ส่วนที่ 2 ทามเมอร์ 0 เป็นสีแดง
ทามเมอร์ 0 จะทำงานตามเวลาที่กำหนดไว้ในแต่ละมุมของมอเตอร์
แต่ละตัว แล้วแต่ค่าที่ได้รับมา
ทามเมอร์0ทำงานจะทำให้ขาP1 ที่ต่างเปิด
เป็นเวลาต่างๆกันไล่ไป ไล่เปิดและปิดเละเปิดใหม่
เป็นอย่างนี้จนกระทั่งครบทั้งหมด 8 ตัวแล้วจะทำการ
หยุดรอจนกระทั่งครบ 20ms ทามเมอร์1จึงเรื่มทำงาน

3.ส่วนสีเขียว ส่วนนี้ค่อนข้างสำคัณมาก
เพราะว่าส่วนนี้จะประกอบด้วย 3 ส่วนย่อยด้วยกัน
และจะทำงานที่ทุกๆ 20ms

3.1 ส่วนรับข้อมูลเข้า

ส่วนนี้ทำหน้าที่รับข้อมูลเข้าโดยจะทำการอ่านข้อมูล
จาก P3 P0และ P1 ในเวลาไล่เลี่ยกัน(เพื่อเป็นการ
กันการอ่านข้าข้ามในกรณีที่คาบเวลาของ 51 ทั้ง
สองตัวเหลื่อมล้ำกัน เป็นการกันการกระโดดของข้อมูล
ทำให้การกระตุกของเซอร์โวน้อยลงไป )
เมื่ออ่านค่าจาก P3 จะทำการเปลี่ยนเทียบค่าที่ได้ว่า
เป็นของมอเตอร์ตัวใหน และจะทำการนำค่าที่ได้จาก
P0และP2ไปเก็บไว้ในอาเรของมอเตอร์นั้นๆ

การรับค่านั้นจะเห็นว่าเรารับค่าได้แค่ 6 ตัว เพราะ
อีก2บิดเป็นบิทไว้อินเตอร์รับ ดังนั้นเราจึงสั่งมอเตอร์
ได้เพียง 2^6 64 ตัวคือ 11110011

3.2 ส่วนคิดเรื่องความเร็ว

ส่วนนี้จะทำหน้าที่กำกนดความเร็วของมอเตอร์
ทำได้โดยการ

xa = s2[2]*255+s1[2];//รวมแปดบิดสองตัวให้เป็น
เลขฐาน10ก่อน ค่าที่เข้ามาใหม่ ถ้าค่าที่เข้ามาใหม่
ไม่มีการเปลี่ยนแปลง มันก็จะยังคงค่านั้นไว้เรื่อยๆ
if(xb < xa){xb += vb;if(xb > xa){xb = xa;}}
if(xb > xa){xb -= vb;if(xb < xa){xb = xa;}}
//ทำการเปลียบเทียบค่าว่าของเก่ามากว่าหรือน้อยกว่า
ถ้ามากกว่าก็ให้บวกค่าขึ้นเท่ากับตัวแปลที่ตั้งไว้

//ทำการเปลี่ยนเลขฐาน10ให้เป็นฐาน16
s1o[2] = xb % 255 ;
s2o[2] = xb / 255 ;

ถ้าเป็นความเร่ง ก็ให้เอาค่าความเร่ืงคุณเช้าไปกับค่า
ความเร็ว แล้วก็บอกกลับเข้าไปเหมือนเดิม

3.3เป็นส่วนที่ทำให้ทามเมอร์1ทำงานเหมือนเดิม

unsigned char s1[17],s2[17];
unsigned int va;
char cc;
sbit P3_7 = P3^7;
int setx,i,j;

void start (void) { // open seirl port
SCON = 0x52; // set speacial function register
TMOD = 0x21; // set timer 0,1 at 8 bit count
TH1 = 0xFB; // Baud Rate = 9600 at 18.432Mh
TR1 = 1; // timer 1 run
}
void hi_p32(void) interrupt 0 //function interupt timer 1
{ }

void Timer0 (void) interrupt 1 //function interupt timer 1
{
// P1 = 0x02; P1 = 0x03; // start to intreup another 51
TR0 = 0;
TH0 = 0xFF-0x7E; //set timer at 20 ms/robe at 18.432
TL0 = 0xFF-0x02;
//--------------------2 loop to delay -------------------------
switch(j){
case 1: P2 = s2[1]; P0 = s1[1]; P1 = 0x12; P1 = 0x13; break; //16 stage
case 2: P2 = s2[2]; P0 = s1[2]; P1 = 0x22; P1 = 0x23; break;
case 3: P2 = s2[3]; P0 = s1[3]; P1 = 0x32; P1 = 0x33; break;
case 4: P2 = s2[4]; P0 = s1[4]; P1 = 0x42; P1 = 0x43; break;
case 5: P2 = s2[5]; P0 = s1[5]; P1 = 0x52; P1 = 0x53; break;
case 6: P2 = s2[6]; P0 = s1[6]; P1 = 0x62; P1 = 0x63; break;
case 7: P2 = s2[7]; P0 = s1[7]; P1 = 0x72; P1 = 0x73; break;
case 8: P2 = s2[8]; P0 = s1[8]; P1 = 0x82; P1 = 0x83; break;

case 9: P2 = s2[9]; P0 = s1[9]; P1 = 0x92; P1 = 0x93; break; //16 stage
case 10: P2 = s2[10]; P0 = s1[01]; P1 = 0xA2; P1 = 0xA3; break;
case 11: P2 = s2[11]; P0 = s1[11]; P1 = 0xB2; P1 = 0xB3; break;
case 12: P2 = s2[12]; P0 = s1[12]; P1 = 0xC2; P1 = 0xC3; break;
case 13: P2 = s2[13]; P0 = s1[13]; P1 = 0xD2; P1 = 0xD3; break;
case 14: P2 = s2[14]; P0 = s1[14]; P1 = 0xE2; P1 = 0xE3; break;
case 15: P2 = s2[15]; P0 = s1[15]; P1 = 0xF2; P1 = 0xF3; break;
case 16: P2 = s2[16]; P0 = s1[16]; P1 = 0x02; P1 = 0x03; break;

}
j++;
if(j >= 17){j = 1;}

//---------------------------------------------
TR0 = 1;
}

void hi_p33 (void) interrupt 2
{ }

void main (void) {

IE = 0x82; //open interuptt 0,1,2,3
TH0 = 0xFF-0x7E; //set timer at 20 ms/robe at 18.432
TL0 = 0xFF-0x02;
TR0 = 1; //on timer 1
P1=0xFF;
j = 0;
start();
printf ("SERVO CONTROL R01 \n");
for(i=1;i<=16;i++){
s1[i] = 0x0f; s2[i] = 0x0f;
}
// while ((cc = _getkey ()) != 0x1B) {
while (cc = _getkey ()) {
switch(cc){
case '1': s2[1] = 0x05; s1[1] = 0xdc; break;
case '2': s2[1] = 0x0b; s1[1] = 0xb8; break;
case 0x01: s2[1] = _getkey (); s1[1] = _getkey (); break;
case 0x02: s2[2] = _getkey (); s1[2] = _getkey (); break;
case 0x03: s2[3] = _getkey (); s1[3] = _getkey (); break;
case 0x04: s2[4] = _getkey (); s1[4] = _getkey (); break;
case 0x05: s2[5] = _getkey (); s1[5] = _getkey (); break;
case 0x06: s2[6] = _getkey (); s1[6] = _getkey (); break;
case 0x07: s2[7] = _getkey (); s1[7] = _getkey (); break;
case 0x08: s2[8] = _getkey (); s1[8] = _getkey (); break;

case 0x09: s2[9] = _getkey (); s1[9] = _getkey (); break;
case 0x0A: s2[10] = _getkey (); s1[10] = _getkey (); break;
case 0x0B: s2[11] = _getkey (); s1[11] = _getkey (); break;
case 0x0C: s2[12] = _getkey (); s1[12] = _getkey (); break;
case 0x0D: s2[13] = _getkey (); s1[13] = _getkey (); break;
case 0x0E: s2[14] = _getkey (); s1[14] = _getkey (); break;
case 0x0F: s2[15] = _getkey (); s1[15] = _getkey (); break;
case 0x10: s2[16] = _getkey (); s1[16] = _getkey (); break;

//--***********-------**********----------**********
case 0x21:
P2 = _getkey (); P0 = _getkey (); P1 = 0x08; P1 = 0x0B; break;
//----------***********----------*********----------
default:
_getkey ();
_getkey ();

}
// printf ("SERVO\n");
}
}

ส่วนแรกเป็นส่วนที่ทำหน้าที่รับข้อมูลจากซีเรี่ยว
ในที่นี้ใช้ทามมเอร์ 0 เป็นตัวนับ รูปแบบคำสังอยู่ในส่วนของ start เราใช้ค่าบอกเรทที่
9600 ตั้งค่า TH1 0xfb ที่ 18.432

ในส่วนที่เป็นสีแดง
จะทำการ วนหรูปรับค่าโดยใช้คำสั่ง
_getkey () หมายถึงรับค่าเข้ามาทีละตัวแต่ไม่
พิพม์ค่าออกไป
ชุดคำสังขเงเราืที่เข้ามาจะประกอบด้วย 3 ตัว
ตัวแรกจะเป็นเบแร์มอเตอร์ ตัวต่อมาจะเป็น
เลขข้างฮาย อีกตัวเป็นเลขข้างโล

ดังนั้นหรูปเราจะทำการรับค่าแล้วก็เลือกว่าลงช่องใหน
แล้วก็จะทำการรับค่าอีก 2 ค่าก็เป็นการจบหรูป
ในกรณีที่ค่าไม่ตรงกับอะไรเลย หรูปจะทำ
การรับค่าทิ้งไปเฉยๆ เพื่อให้การทำงานเร็วและไม่ผิด

ส่วนที่ 2 เป็นส่วนของทามเมอร์ 1

จะทำงานทุก 20 ms ในที่นี้เราใช้ความถื่ที่ 18.432
เพราะว่าทำการติดต่อซีเรี่ยวแล้วผิดพลาดน้อย
เราตั่งค่า th0 TH0 = 0xFF-0x7E
tl0 TL0 = 0xFF-0x02
คือ ทุกๆ20 ms จะทำการส่งค่าออกไปทาง P0 P2
และ P1 ส่วน P3 เอาไว้ทำหน้าที่ติดต่อกับซีเรี่ยว

ทุกๆ20 ms จะส่งค่าออกนั่นหมายความว่า
เรามีมอเตอร์ 16 ตัว การส่งค่าหมดต้องใช้เวลา
16x20ms 320 ms ซึ่งนานพอสมควร ตอนนี้ก็ติดอยู่ที่ตรงนี้ ถ้าแก้ตรงนี้ได้จะดีมาก