#include "LF2407.h"
#include "math.h"
#include "stdio.h"
int N_x,N_y,Done_Flagx,Done_Flagy,Sx,Sy;
float Position_x,Position_y,x,y,K,mycos;

int Residue_x,Residue_y;//插补时的余数
int F=4000;
int t=2;//插补周期取为2ms
//禁止总中断子程序      
void inline disable()
{
  asm(" setc INTM");
  asm(" setc SXM");
}
//使能总中断子程序
void  inline  enable()
{
   asm(" clrc INTM");
}

void initial()
{
  asm(" setc SXM"); //抑制符号位扩展
  asm(" clrc OVM"); //清楚累加器溢出位
  asm(" clrc CNF"); //将B0配置为数据空间
  *SCSR1=0X83FF;    //×2模式,主频30MHz
  *WDCR=0X0E8;      //禁止看门狗
 
  *IMR=0X0004;       //禁止所有中断
  *IFR=0XFFFF;       //清除所有中断标志
  WSGR=0x00;         //禁止所有等待状态
}

int Hw_Line(float x,float y,float F)    
{
  float f;
  int delta_x,delta_y;
  while(Done_Flagx!=1 || Done_Flagy!=1)
 {  
  f=F*t/60;
  delta_x=f*mycos;
  delta_y=delta_x*K;
  if(Done_Flagx==0)  //插补未完成
  {
      N_x=(delta_x+Residue_x)*0.2;   //加上上一次的余数,然后除以5，得x方向发送的脉冲数
      Position_x=Position_x+N_x*0.005;  //当前位置，单位mm
      Residue_x=(delta_x+Residue_x)%5;  //更新之后供下一次使用
      if((Position_x-x)*Sx>=0)           //如果已经超界了
        {
          delta_x=(x-Position_x+N_x*0.005)*1000;//相当于减去第i－1次的当前位置,um
          N_x=delta_x*0.2; 
          Position_x=x;
          Done_Flagx=1;
         
        }
      
   }
  if(Done_Flagy==0)
  {
      N_y=(delta_y+Residue_y)*0.1; //加上上一次的余数,然后除以5，得x方向发送的脉冲数
      Position_y=Position_y+N_y*0.01;//当前位置，单位mm
      Residue_y=(delta_y+Residue_y)%10; //更新之后供下一次使用
      if((Position_y-y)*Sy>=0)           //如果已经超界了
        {
          delta_y=(y-Position_y+N_y*0.01)*1000;//相当于减去第i－1次的当前位置,um
          N_y=delta_y*0.1;   //最后一次输出的脉冲数，可能跟前几次的不同
        
          Position_y=y; 
           Done_Flagy=1;
        
         
        }
      
     
   }
 }                                       
 *T2CNT=0X00;                      //计数值归零
 *EVAIFRB=*EVAIFRB & 0X0001;       //清除中断标志
 enable();
}
main()
{
  int flag=1;     //暂定为直线插补标志
  disable();
  initial();
  N_x=0;
  N_y=0;
  Position_x=0;
  Position_y=0;
  Residue_x=0;
  Residue_y=0;
  x=0.562;
  y=0.374;     //假定这是目的坐标
  *EVAIFRB=0XFFFF;    //清楚T2的所有中断标志
 *T2CON=0X9440;      //连续增计数模式，X/16 ,应该放在主程序中，根据插补的需要而将第6位置位或清零，使能/禁止定时器
 *T2CNT=0X0;
 *T2PER=0X249F;      //这个是一个常数，可以在程序中定义为const,5ms 
 *EVAIMRB=0X000f;   //使能T2的周期中断 ，

  enable();
  while(1)
  {
    if(flag==1)
    {
      if((x-Position_x)>=0)
        Sx=1;
      else
        Sx=-1;
      if((y-Position_y)>=0)
        Sy=1;
      else
        Sy=-1;   
     Done_Flagx=0;
     Done_Flagy=0;
     K=(y-Position_y)/(x-Position_x);
     mycos=1/sqrt(1+K*K);//以上为离线计算的变量
    
    } 
  }
}
void interrupt T2ISR()
{
   switch(*PIVR)
   {
    case 43 : Hw_Line(x,y,F);break; //若未到位置，则继续插补
 
   }
}

void  interrupt  nothing()
{
 return;
}