MG996R舵机控制

#include
sbit LED1 = P0^0;
sbit DJ = P0^0;
sbit key1 = P0^1;
sbit key2 = P0^2;
sbit key3 = P0^3;
sbit key4 = P0^4;
sbit key5 = P0^5;
void Delay05ms() //@24.000MHz
{
unsigned char i, j;

i = 2;
j = 239;
do
{
while (--j);
} while (--i);
}
void Delay175ms() //@24.000MHz
{
unsigned char i, j;

i = 69;
j = 20;
do
{
while (--j);
} while (--i);
}

void Delay18ms() //@24.000MHz
{
unsigned char i, j;

i = 71;
j = 6;
do
{
while (--j);
} while (--i);
}

void Delay185ms() //@24.000MHz
{
unsigned char i, j;

i = 72;
j = 249;
do
{
while (--j);
} while (--i);
}

void Delay19ms() //@24.000MHz
{
unsigned char i, j;

i = 74;
j = 235;
do
{
while (--j);
} while (--i);
}


void right45 ()
{
while (1)
{
DJ=1;
Delay05ms();
Delay05ms();
DJ=0;
Delay19ms();
if (key4!=0) {Delay05ms();if (key4!=0)break;}
}
}
void right90()
{
while (1)
{
DJ=1;
Delay05ms();
DJ=0;
Delay19ms();
Delay05ms();
if (key4!=0) {Delay05ms();if (key4!=0)break;}
}
}
void left45()
{
while (1)
{
DJ=1;
Delay05ms();
Delay05ms();
Delay05ms();
Delay05ms();
DJ=0;
Delay18ms();
if (key2!=0){Delay05ms();if (key2!=0)break;}
}

}

void left90()
{
while (1)
{
DJ=1;
Delay05ms();
Delay05ms();
Delay05ms();
Delay05ms();
Delay05ms();
DJ=0;
Delay175ms();
if (key5!=0){Delay05ms();if (key5!=0)break;}
}

}
void middle()
{
while (1)
{
DJ=1;
Delay05ms();
Delay05ms();
Delay05ms();
DJ=0;
Delay185ms();
if (key3!=0) {Delay05ms();if (key3!=0)break;}
}

}

void main ()
{
while (1)
{
if(key1 == 0) left90();
if(key2 == 0) left45();
if(key3 == 0) middle();
if(key4 == 0) right45();
if(key5 == 0) right90();
}
}