伺服舵机带参方向控制

/*******************************
功能：小车四个方向（带参数）打包
说明：P1_0接左电机，P1_1接右电机,速度值在0～200之间
时间：2012.12.10
作者：荆轩道
附录：脉冲数量：3/0.023=130个（3S）
************************************************************/
#include<BoeBot.h>
#include<uart.h>

void Forward(int PulseNo,int Velocity)  //前进：传入脉冲数量和速度
{
  int i;
  for(i=1;i<=PulseNo;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 Lift(int PulseNo,int Velocity)  //左转：传入脉冲数量和速度
{
  int i;
  for(i=1;i<=PulseNo;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 PulseNo,int Velocity)  //右转：传入脉冲数量和速度
{
  int i;
  for(i=1;i<=PulseNo;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 Back(int PulseNo,int Velocity)  //后退：传入脉冲数量和速度
{
  int i;
  for(i=1;i<=PulseNo;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 main()
{
  uart_Init();
  printf("Program is Running!!");

  while(1)
  {
    int i;
  //200代表全速，130代表运行3秒，65代表1.5秒，26为旋转90度 
   Forward(65,200);    
   Lift(26,200);  
   Back(65,200); 
   Right(26,200); 
  }
}