飞思卡尔光电管 舵机，电机控制程序2

chensmake

//延时查询表 6*6
static UNIT DelayTable[]=  
{
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,0,
0,0,0,0,0,0
}

void PWMInit()    //32HHz
{
PWMCTL_CON01= 1;   //0, 1合为16bit
PWMCAE_CAE1= 1;    //Center mode
PWMCLK_PLCK1= 0;   //Clock A
PWMPRCLK_PCKA= 5;   //Clock A= 32MHz/32= 1MHz
PWMPOL_PPOL1= 0;   //开始为低电压
PWMPER0,1= 10000;   //50Hz PWM输出
PWMDTY0,1= MIDDLE;   //(10000- 750)* 100%= 92。5%占空比，1.5ms高电压时间
PWME_PWME1= 1;    //enable
}

static void delay(int ms)  
{
  int ii,jj;
   if (ms<1) ms=1;
   for(ii=0;ii<ms;ii++)
     for(jj=0;jj<2770;jj++);    //32MHz--1ms        
}

static void SetPWM(UINT m_nPWM)  //PWM设置
{
PWMDTY0,1= m_nPWM;
}

//number 9种情况，sub 7种情况，LeftRight 2中情况
//共128种情况，采用模糊控制暂时归纳为11种情况
//11种情况将填入表中提供查询，11中情况分别为：
//左右5， 15， 30， 45， 60度
//和正中间0度
static UINT GetPWM(UCHAR number, UINT sub, UCHAR LeftRight)
{
switch(sub)   //通过sub算出索引值
{
  case 0x8000- 0x0100:
  case 0x0100- 0x0001;
  case 0x0100- 0x0002;
   sub= 0;
   break;
  case 0x4000- 0x0100:
  case 0x0100- 0x0004:
   sub= 1;
   break;
  case 0x2000- 0x0100:
  case 0x0100- 0x0008:
   sub= 2;
   break;
  case 0x1000- 0x0100:
  case 0x0100- 0x0010:
   sub= 3;
   break;
  case 0x0800- 0x0100:
  case 0x0100- 0x0020:
   sub= 4;
   break;
  case 0x0400- 0x0100:
  case 0x0100- 0x0040:
   sub= 5;
   break;
  case 0x0200- 0x0100:
  case 0x0100- 0x0100:
  case 0x0100- 0x0080:
   sub= 6;
   break;

}
return PWMTable[number+ sub* 10+ LeftRight* 70];
}


//通过m_nPWM来改变速度，并返回改变的值
static UINT ChangeSpeed(UINT m_nPWM)
{
UINT m_nSpeed;
switch(m_nPWM)   //根据m_nPWM 调节速度
{
  case LEFT60:
   Speed(SPEED60);
   m_nSpeed= SPEED60;
   break;
  case LEFT45:
   Speed(SPEED45);
   m_nSpeed= SPEED45;
   break;
  case LEFT30:
   Speed(SPEED30);
   m_nSpeed= SPEED30;
   break;
  case LEFT15:
   Speed(SPEED15);
   m_nSpeed= SPEED15;
   break;
  case LEFT5:
   Speed(SPEED5);
   m_nSpeed= SPEED5;
   break;
  case MIDDLE:
   Speed(SPEED0);
   m_nSpeed= SPEED0;
   break;
  case RIGHT60:
   Speed(SPEED60);
   m_nSpeed= SPEED60;
   break;
  case RIGHT45:
   Speed(SPEED45);
   m_nSpeed= SPEED45;
   break;
  case RIGHT30:
   Speed(SPEED30);
   m_nSpeed= SPEED30;
   break;
  case RIGHT15:
   Speed(SPEED15);
   m_nSpeed= SPEED15;
   break;
  case RIGHT5:
   Speed(SPEED5);
   m_nSpeed= SPEED5;
   break;
}
return m_nSpeed;
}


//获得查表时的索引值
UINT GetIndex(UINT m_nSpeed)
{
if(m_nSpeed<= SPEED60)
{
  m_nSpeed= 0;
}
else if(m_nSpeed<= SPEED45)
{
  m_nSpeed= 1;
}
else if(m_nSpeed<= SPEED30)
{
  m_nSpeed= 2;
}
else if(m_nSpeed<= SPEED15)
{
  m_nSpeed= 3;
}
else if(m_nSpeed<= SPEED5)
{
  m_nSpeed= 4;
}
else
{
  m_nSpeed= 5;
}
return m_nSpeed;
}

//m_nSpeed2为欲设值
//m_nSpeed为当前速度
UINT GetDelay(UINT m_nSpeed, UINT m_nSpeed2)
{
m_nSpeed= GetIndex(m_nSpeed);
m_nSpeed2= GetIndex(m_nSpeed2);
return DelayTable[m_nSpeed* 6+ m_nSpeed2];
}

void Turn(UCHAR number, UINT sub, UCHAR LeftRight, UCHAR ret)//ret not be used now
{
UINT m_nPWM;
UINT m_nSpeed;
UINT m_nSpeed2;
UINT m_nDelay;       //延时参数
m_nPWM= GetPWM(number, sub, LeftRight);
m_nSpeed= GetSpeed()     //测速模块
m_nSpeed2= ChangeSpeed(m_nPWM);
if(m_nSpeed2> m_nSpeed)     
  m_nSpeed= m_nSpeed2- m_nSpeed;
else
  m_nSpeed= m_nSpeed- m_nSpeed2;
SetPWM(m_nPWM);       //转角
m_nDelay= GetDelay(m_nSpeed, m_nSpeed2);
delay(m_nDelay);      //根据速度和角度延时
SetPWM(MIDDLE);       //舵机摆正
}
转载于：http://zhidao.baidu.com/question/90016084.html?si=1

星雨

领教

安杰

暂时看不懂！呵呵

嗷嗷疯

楼主真好

龙游天下明

看不懂