求助!MEGA16按输入频率计算速度问题

huangxiaolpbany

程序目的：
采集X/Y轴的速度，计算出合成速度，并输出对应频率。
采取重INT0/INT1输入X/Y轴的编码器信号，中断中编码器进行计数。
T0设置为2ms，每2ms采集INT0/INT1的累积的数值，按照比例系数计算为速度，并按照合成速度=sqrt（x速度*x速度+y速度*y速度）计算出合成速度，按照合成速度输出对应的脉冲。

现在问题是xy速度较低时可以使用，速度稍微一高，就无法计算了。
之前使用采集模拟量的方法，不使用INT0/INT1中断做的时候没有问题的。
估计是中断太多造成的。
有没有更合适的方法，谢谢！


/********************************************************************
Program Name:Pulse Generator For Matthews 8400E Inkjet Controller
X_Y Speed Signal From Server Card                     
X_speed Pulse:INT0
Y_speed Pulse:INT1               
TC0 CTC mode,interrupt cycle 2ms                     
TC1 CTC mode,Pin_PD4 Pulse Output                    
PD6 Low_speed_Flag,1-Low_Speed
PD7 500Hz Frenquncy Out
LCD PIN CONNECT:
        PA0 D0-D7:Data Input
        PC1 RS:RS=0 Command/RS=1 Dispaly
        PC6 RW:RW=0 Read/RW=1 Write
        PC7 E:Pulse                        
********************************************************************/

#include <iom16v.h>
#include <macros.h>
#include <STDLIB.h>
#include <MATH.h>

#define uchar unsigned char
#define uint unsigned int
#define ulong unsigned long

//x.y.machine_speed_scale
#define x_scale 0.95357
#define y_scale 1.13331
#define m_scale 1

//x.y_speed_filter_Enable_or_Disable
//1-Enable,0-Disable
#define spd_flt 1

//low_speed_limit
#define spd_low_lmt 40

//m_speed_offset_Enable_or_Disable
//1-Enable,0-Disable
//speed_up/down_offset
//speed_up,r_spd=spd_cali+spd_up_offset
//speed_dn,r_spd=spd_cali-spd_dn_offset
//Uint-Hz
#define m_spd_offset 0
#define spd_up_offset 10
#define spd_dn_offset 15

//LCD1602_control_bit
//Chipest Slect,Serial Data,CLK
#define Set_Lcd1602_RS() DDRC|=BIT(1);PORTC|=BIT(1)
#define Set_Lcd1602_RW() DDRC|=BIT(6);PORTC|=BIT(6)
#define Set_Lcd1602_E() DDRC|=BIT(7);PORTC|=BIT(7)

#define Clr_Lcd1602_RS() DDRC|=BIT(1);PORTC&=~BIT(1)
#define Clr_Lcd1602_RW() DDRC|=BIT(6);PORTC&=~BIT(6)
#define Clr_Lcd1602_E() DDRC|=BIT(7);PORTC&=~BIT(7)

uint x_pulse,y_pulse;
uint x_spd,y_spd,m_spd,freq;
uint x_spd_arr[5],y_spd_arr[5];
uint m_spd_last,m_spd_now;

//ONLY_for_test
//uint x_spd_rand,y_spd_rand;

#pragma interrupt_handler cycle_2ms:20
#pragma interrupt_handler Int0:2
#pragma interrupt_handler Int1:3

/********************************************************************
Function Name:    delay_5us
Input Parameter:  us
Output Parameters:None
Function:         delay;
                                  delay time:5us*us(8MHz)
********************************************************************/
void delay_5us(uchar us)
{
         uchar i,j;
         for(i=0;i<us;i++)
                 for(j=0;j<8;j++);
}

/********************************************************************
Function Name:    delay_1ms
Input Parameter:  ms
Output Parameters:None
Function:         delay;
                                  delay time:1ms*ms(8MHz)
********************************************************************/
void delay_1ms(uint ms)
{       
        uint i,j;
    for(i=0;i<ms;i++)
            for(j=0;j<1051;j++);
}

/********************************************************************
Function Name:    Int0
Input Parameter:  None
Output Parameters:None
Function:         x_pulse++
********************************************************************/
void Int0()
{   
        GICR&=~BIT(6);
        x_pulse++;
        GICR|=BIT(6);
}

/********************************************************************
Function Name:    Int1
Input Parameter:  None
Output Parameters:None
Function:         y_pulse++
********************************************************************/
void Int1()
{   
        GICR&=~BIT(7);
        y_pulse++;
        GICR|=BIT(7);
}

/********************************************************************
Function Name:    isqrt32
Input Parameter:  x(long)
Output Parameters:y(long)
Function:         quick sqrt
********************************************************************/
unsigned int isqrt32(unsigned long x)
{
    unsigned long m,y,b;
    m=0x40000000;
    y=0;
    while(m!=0)
   {
     b=y|m;
     y=y>>1;
     if(x>=b)
     {
        x=x-b;
        y=y|m;
     }
     m>>=2;
    }
    return y;
}

/********************************************************************
Function Name:    cycle_2ms
Input Parameter:  None
Output Parameters:None
Function:         Calculate Pulse Out Frequncy
********************************************************************/
void cycle_2ms()
{   
   uint x_spd_ad,y_spd_ad,addata,x_spd_avg,y_spd_avg;
   //x.y_speed_from_AD_converter,AD_result,output frequency

   ulong x_spd_squ,y_spd_squ,freq_ocr;
   //x.y_real_speed square,OCR1A

   //For Test
   //x_spd_rand=15000;
   //y_spd_rand=3500;         

         x_spd_ad=x_pulse;
         y_spd_ad=y_pulse;
         
         //Clear x_pulse,y_pulse
         x_pulse=0;
         y_pulse=0;

         //x.y_speed_filter
         if(spd_flt)
         {
          x_spd_arr[0]=x_spd_arr[1];
          x_spd_arr[1]=x_spd_arr[2];
          x_spd_arr[2]=x_spd_arr[3];
          x_spd_arr[3]=x_spd_arr[4];
          x_spd_arr[4]=x_spd_ad;
          
          x_spd_avg=((x_spd_arr[0]+x_spd_arr[1]+x_spd_arr[2]+x_spd_arr[3]+x_spd_arr[4])/5);
          
          y_spd_arr[0]=y_spd_arr[1];
          y_spd_arr[1]=y_spd_arr[2];
          y_spd_arr[2]=y_spd_arr[3];
          y_spd_arr[3]=y_spd_arr[4];
          y_spd_arr[4]=y_spd_ad;
          
          y_spd_avg=((y_spd_arr[0]+y_spd_arr[1]+y_spd_arr[2]+y_spd_arr[3]+y_spd_arr[4])/5);          
         }
         
         else
         {
          x_spd_avg=x_spd_ad;
          y_spd_avg=y_spd_ad;
         }
         
         x_spd=x_scale*x_spd_avg;
         y_spd=y_scale*y_spd_avg;

         x_spd_squ=x_spd;
         y_spd_squ=y_spd;
         
         //x.y_real square
         x_spd_squ=x_spd_squ*x_spd_squ;
         y_spd_squ=y_spd_squ*y_spd_squ;
         //machine speed_cali
         m_spd=isqrt32(x_spd_squ+y_spd_squ);
         //machine real speed
         m_spd=m_scale*m_spd;
         
         //Low_Speed_LMT_Flag
         if(m_spd<spd_low_lmt)
         {
          PORTD|=BIT(6);
          freq=500;
         }
         else
         {
          PORTD&=~BIT(6);
          freq=m_spd*25;
         }
         
         //speed_up_or_down_offset
         if(m_spd_offset&(m_spd>=40))
         {          
                   m_spd_now=m_spd;
                  
              if(m_spd_now>m_spd_last)
              {
                      freq+=spd_up_offset;
              }
                  else if(m_spd_now<m_spd_last)
                  {
                   freq-=spd_dn_offset;
                  }
                  else
                  {
                   freq=freq;
                  }
                  
                  m_spd_last=m_spd;          
         }

         freq_ocr=(4000000/freq)-1;
         OCR1A=freq_ocr;
         
         PORTD^=BIT(7);
}

/********************************************************************
Function Name:    Write_Lcd1602
Input Parameter:  RS/W_Data
Output Parameters:None
Function:         Write a Byte to Lcd1602
                                  RS=0,Command/RS=1,Dispaly Data
                                  W_data,Command Or Display Data
********************************************************************/
void Write_Lcd1602(uchar RS,uchar W_data)
{   
   //Display Data
   if(RS==1)
   {
             Clr_Lcd1602_E();
          Clr_Lcd1602_RW();
          Set_Lcd1602_RS();
          delay_5us(2);
          PORTA=W_data;
          delay_5us(2);
          Set_Lcd1602_E();
          delay_5us(2);          
   }
   //Command Data
   else
   {
             Clr_Lcd1602_E();
          Clr_Lcd1602_RW();
          Clr_Lcd1602_RS();
          delay_5us(2);
          PORTA=W_data;
          delay_5us(2);
          Set_Lcd1602_E();
          delay_5us(2);          
   }
}

/********************************************************************
Function Name:    Lcd1602_Init
Input Parameter:  None
Output Parameters:None
Function:         Lcd1602 Initial
********************************************************************/
void Lcd1602_Init()
{
   uchar cmd;
      
   cmd=0x38;
   Write_Lcd1602(0,cmd);
   delay_1ms(2);
   
   cmd=0x01;
   Write_Lcd1602(0,cmd);
   delay_1ms(2);
   
   cmd=0x0C;
   Write_Lcd1602(0,cmd);
   delay_1ms(2);
   
   cmd=0x06;
   Write_Lcd1602(0,cmd);
   delay_1ms(2);
}





void main()
{
   uchar i;
   uint cycle;
   uint x_spd_show,y_spd_show,m_spd_show,freq_show;
   uint j;

   //Page 1
   uchar show_content1[]=" FARLEYLASERLAB ";
   uchar show_content2[]="   2014.04 v2   ";
   
   //page 2
   uchar show_content3[]=" PulseGenerator ";
   uchar show_content4[]="For Inkjet 8400E ";       
   
   //Page 3
   uchar show_content5[]="XSpeed:     mm/m";
   uchar show_content6[]="YSpeed:     mm/m";
   
   //Page 4
   uchar show_content7[]="MSpeed:     mm/m";
   uchar show_content8[]="PulseOut:     Hz";       
   
   //Port Initial
   //Lcd Inital
   DDRA=0XFF;
   PORTA=0X00;
   
   DDRC|=BIT(1)|BIT(6)|BIT(7);
   PORTC|=BIT(1)|BIT(6)|BIT(7);
   
   DDRD|=BIT(4)|BIT(6)|BIT(7);
   PORTD|=BIT(4)|BIT(6)|BIT(7);
   
   //int0,int1
   DDRD&=~BIT(2);
   DDRD&=~BIT(3);
   
   //TC0_Control_bit
   TCCR0=0X0B;
   OCR0=0X7C;
       
   //TC1_control_bit,CTC_mode
   DDRD|=BIT(4)|BIT(5);
   TCCR1A=0X50;
   TCCR1B=0X09;
   OCR1A=1999;
   
   //Initial Lcd 1602
   Lcd1602_Init();
   
   //Show Page #1
   Write_Lcd1602(0,0x80);   //显示的地址0x80
   for(i=0;show_content1[i]!='\0';i++)
   {
      Write_Lcd1602(1,show_content1[i]);
   }
   
   delay_1ms(10);
          
   Write_Lcd1602(0,0xC0);     //显示的地址0xC0
   for(i=0;show_content2[i]!='\0';i++)
   {
      Write_Lcd1602(1,show_content2[i]);
   }
   delay_1ms(2000);
   
   //Show Page #2  
   Write_Lcd1602(0,0x80);   //显示的地址0x80
   for(i=0;show_content3[i]!='\0';i++)
   {
      Write_Lcd1602(1,show_content3[i]);
   }
   
   delay_1ms(10);
          
   Write_Lcd1602(0,0xC0);     //显示的地址0xC0
   for(i=0;show_content4[i]!='\0';i++)
   {
      Write_Lcd1602(1,show_content4[i]);
   }
   delay_1ms(2000);

   //x.y_speed_array_initial
   for(i=0;i<5;i++)
   {
      x_spd_arr[i]=0;
          y_spd_arr[i]=0;
   }
   
   //TC0_enable
   TIMSK|=BIT(1);  
       
   //Int0_Int1_enable
   GICR|=BIT(7)|BIT(6);
   
   //Inq Enable
   SREG|=BIT(7);
   
   cycle=0;
   x_pulse=0;
   y_pulse=0;
   
   while(1)
   {
   
             //show Xspeed/Yspeed/MachineSpeed/Frequncy
          x_spd_show=x_spd;
          y_spd_show=y_spd;
          m_spd_show=m_spd;
          freq_show=freq;
          
          /*
          x_spd_show=12345;
          y_spd_show=23456;
          m_spd_show=34567;
          freq_show=45678;
          */
          
          x_spd_show*=60;
          y_spd_show*=60;
          m_spd_show*=60;
          
          
          for(i=11;i>6;i--)
          {
                 show_content5[i]=x_spd_show%10+0X30;
                 x_spd_show/=10;  
          }

          
          
          for(i=11;i>6;i--)
          {
             show_content6[i]=y_spd_show%10+0X30;
                 y_spd_show/=10;  
          }
          
          for(i=11;i>6;i--)
          {
             show_content7[i]=m_spd_show%10+0X30;
                 m_spd_show/=10;  
          }
          
          for(i=13;i>8;i--)
          {
             show_content8[i]=freq_show%10+0X30;
                 freq_show/=10;  
          }
          
          
          
      cycle++;
          if(cycle==480)
          {
             cycle=0;
          }
          
          if((cycle/240)==0)
          {
                 //Show Page #3
         delay_5us(1);
  
         Write_Lcd1602(0,0x80);   //显示的地址0x80
         for(i=0;show_content5[i]!='\0';i++)
         {
            Write_Lcd1602(1,show_content5[i]);
         }
          
         Write_Lcd1602(0,0xC0);     //显示的地址0xC0
         for(i=0;show_content6[i]!='\0';i++)
         {
            Write_Lcd1602(1,show_content6[i]);
         }

      }

          //Show page #4
          if((cycle/240)==1)
          {
         delay_5us(1);
          
               Write_Lcd1602(0,0x80);   //显示的地址0x80
         for(i=0;show_content7[i]!='\0';i++)
         {
            Write_Lcd1602(1,show_content7[i]);
         }
          
         Write_Lcd1602(0,0xC0);     //显示的地址0xC0
         for(i=0;show_content8[i]!='\0';i++)
         {
            Write_Lcd1602(1,show_content8[i]);
         }
      }
   }       
}

huangxiaolpbany

算了一下，每秒电机旋转64转，每转脉冲8192，脉冲频率为524K，这么大的脉冲频率要求中断捕捉，还要计算。
我是疯了，应该是之前计算出错了。