KC伺服舵机带参四方向Demo程序

来源：互联网发布：python 支持中文编辑：程序博客网时间：2024/05/16 08:53

/***********************************************************
功能：KC伺服舵机带参四方向Demo程序
说明：P1_0接左电机，P1_1接右电机（Velocity的值在0～200之间）
时间：2012.12.10
作者：荆轩道
************************************************************/
#include<BoeBot.h>
#include<uart.h>
void Forward(int PulseCount,int Velocity)
/* Velocity should be between 0 and 200 */
{
    int i;
    for(i=1;i<= PulseCount;i++)
{
  P1_1=1;
  delay_nus(1500+ Velocity);
        P1_1=0;
  P1_0=1;
  delay_nus(1500- Velocity);
  P1_0=0;
  delay_nms(20);
}
}
void Left(int PulseCount,int Velocity)
/* Velocity should be between 0 and 200 */
{
    int i;
    for(i=1;i<= PulseCount;i++)
{
  P1_1=1;
  delay_nus(1500-Velocity);
        P1_1=0;
  P1_0=1;
  delay_nus(1500-Velocity);
  P1_0=0;
  delay_nms(20);
}
}
void Right(int PulseCount,int Velocity)
/* Velocity should be between 0 and 200 */
{
    int i;
    for(i=1;i<= PulseCount;i++)
{
  P1_1=1;
  delay_nus(1500+Velocity);
        P1_1=0;
  P1_0=1;
  delay_nus(1500+Velocity);
  P1_0=0;
  delay_nms(20);
}
}
void Backward(int PulseCount,int Velocity)
/* Velocity should be between 0 and 200 */
{
    int i;
    for(i=1;i<= PulseCount;i++)
{
  P1_1=1;
  delay_nus(1500-Velocity);
        P1_1=0;
  P1_0=1;
  delay_nus(1500+ Velocity);
  P1_0=0;
  delay_nms(20);
}
}
int main(void)
{
uart_Init();
printf("Program Running!\n");

Forward(65,200); //向前
Left(26,200); //左转90度
Right(26,200); //右转90度
Backward(65,200);
while(1);
}