feedback.c

电机PID调速程序

#include <ADUC842.H>
#include <math.h>
#include "feedback.h"
#include "lcd.h"

sbit  FB_DIR_L = P1^6;				//用于反馈电机转向,根据需要调整 MOTOR_FEEDBACK()鉴向函数
sbit  FB_DIR_R = P1^7;				//顺时针时 FB_DIR_L = 1


//反馈参数定义
int fb_V_L;					 	   	//反馈当前速度
int fb_V_R;

long xdata rea_all_num_l;
long xdata rea_all_num_r;

/*****************************************延时函数************************************************/
//输入值为延时模式mode和延时长度time,无返回值
//调用函数:无

void DELAY(unsigned char mode,unsigned char time)
{
	unsigned int m,n;
	unsigned char k;
	switch(mode)								//mode为延时模式，time为延时长度	
	{
		case 0:	        						//2.68毫秒,此时对应电机速度100--1.2m/s
		{
				for(k=0;k<time;k++)
				for(m=0;m<2;m++)
				for(n=0;n<2450;n++);
		}break;

		case 1:		   							//一秒
		{	
				for(k=0;k<time;k++)
				for(m=0;m<400;m++)
				for(n=0;n<4000;n++);

		}break;

		case 2:		  							//一分钟
		{
				for(k=0;k<time;k++)
				for(m=0;m<24000;m++)
				for(n=0;n<4000;n++);
		}break;

		default:								 //其他
		break;
	} 
}


/*******************************速度反馈*********************************/
//反馈捕获函数
//获得反馈速度和反馈电流

void GET_FEEDBACK()
{
    int speed_num_l=0;						//用于获得当前速度
	int speed_num_r=0;

	TL0 = 0;
	TH0 = 0;
	TL1 = 0;
	TH1 = 0;
	TR0 = 1;							    //计数器接通工作
	TR1 = 1;
    
	DELAY(0,1);							   //the sampling cycle根据所选电机实际测得,
	
	speed_num_l    = ((int)TH0<<8)+TL0;	   //可以加上个防止溢出的中断程序				   
	speed_num_r    = ((int)TH1<<8)+TL1;

	
    TR0 = 0;							    //关闭计数器
	TR1 = 0;

/***********************位置控制**************************/
 	 	TR0 = 1;
  
       if(FB_DIR_L)			//鉴向
	      {
	  	    rea_all_num_l -= (((int)TH0<<8)+TL0);
          }																	 
	   else
	      {
		    rea_all_num_l += (((int)TH0<<8)+TL0);			//可以加上个防止溢出的中断程序 	
          }
 
	   TR1 = 1;
       	           
       if(FB_DIR_R)
	     {
           rea_all_num_r += (((int)TH1<<8)+TL1); 				
	     }
	   else
	     {
		   rea_all_num_r -= (((int)TH1<<8)+TL1); 		
	     }       

    TL0 = 0;
	TH0 = 0;
	TL1 = 0;
	TH1 = 0;
	//---------------check the direction-------------------------------------------------
    if(FB_DIR_L)			//鉴向
	{
	  	speed_num_l = 0 - speed_num_l;
	}																	 
	else
	{
		speed_num_l = speed_num_l; 	
	}
	
	if(FB_DIR_R)
	{
        speed_num_r = speed_num_r; 				
	}
	else
	{
		speed_num_r = 0 - speed_num_r; 		
	}

	fb_V_L   =	speed_num_l;
	fb_V_R	 =  speed_num_r;
}