rebuilt_for_l297_main.c

control step motor (use for CNC machine), use chip dspic30f4011, include interpolation

 else if(tuyetdoi2>y)
 {
  y=tuyetdoi2-y;
  chieu2=nghich;//quay nguoc
  direction2=chieu2;
 }
 else y=0; 
 //==============================
   nstt_tuongdoi(chieu1, x, chieu2, y );// ham noi suy tuyen tinh toa do tuong doi 
}
//***************************************************************************//
void nstt_tuongdoi(unsigned char chieux, unsigned int x,unsigned char chieuy, unsigned int y ) // x, y: tinh theo toa do tuong doi
                                                                     /*
                                                                     cai nay can kiem tra lai
                                                                     */
{
    float temp; //gia tri so sanh voi tananfa
  chieu1=chieux;
  direction1=chieu1;
  chieu2=chieuy;
  direction2=chieu2;
  
 deltax=x; //chuyen sang so thuc.
 deltay=y;
 tananfa=deltay/deltax;
 temp=tananfa;
//==========================
  while((deltax)&&(deltay))
  {
      temp=deltay/deltax;
   //if(deltax!=0) //tranh' truong hop khong xac dinh ????????
   {
    if(temp<tananfa)
    {
     quaytuongdoi1(1); // do phan giai? la 1 buoc (step)
 //    while(_T1IE) ;
     deltax=deltax-1;
    }
   else
   {
     quaytuongdoi2(1); // do phan giai? la 1 buoc (step)
  //   while(_T2IE) ;
     deltay=deltay-1;
   }
  }
while((_T1IE)&&(_T2IE));  //doi 2 dong co chay
//while(runflag==1); //chua co dong co nao chay xong
 }
  //========================
 if(deltax==0)  while(deltay!=0)
 {
     quaytuongdoi2(1);
     while(_T2IE);
     deltay=deltay-1;
 }
  if(deltay==0)  while(deltax!=0)
 {
     quaytuongdoi1(1);
     while(_T1IE);
     deltax=deltax-1;
 }
   // while((_T1IE)||(_T2IE)); 
}
//***************************************************************************//
void tuyetdoi(unsigned int x, unsigned y) //dung de di chuyen nhanh den toa do moi ( chay khong tai?)
{
quaytuyetdoi1(x);
quaytuyetdoi2(y);
while((_T1IE)||(_T2IE));
}
//***************************************************************************//
void tuongdoi(unsigned char i,unsigned int x,unsigned char j,unsigned int y)  //dung de di chuyen nhanh den toa do moi ( chay khong tai?)
{
 chieu1=i;//chieu quay truc x
 direction1=chieu1;
 chieu2=j;
 direction2=chieu2;
 quaytuongdoi1(x);
 quaytuongdoi2(y);
while((_T1IE)||(_T2IE));
}
//***************************************************************************// 
void quaytuyetdoi1(unsigned int i) //unused
{
 if(tuyetdoi1<i)
 { 
  i=i-tuyetdoi1; 
  chieu1=thuan; //quay thuan 
//  direction1=chieu1;
 }
 else if(tuyetdoi1>i)
 {
  i=tuyetdoi1-i;
  chieu1=nghich;//quay nguoc
 // direction1=chieu1;
 }
 quaytuongdoi1(i);
}
//***************************************************************************//
void quaytuyetdoi2(unsigned int i) //unused
{
 if(tuyetdoi2<i)
 { 
  i=i-tuyetdoi2;   
  chieu2=thuan; //quay thuan
//  direction2=chieu2;
 }
 else if(tuyetdoi2>i) 
 {
  i=tuyetdoi2-i;
  chieu2=nghich;//quay nguoc
 // direction2=chieu2;
 }
 quaytuongdoi2(i);
}
//***************************************************************************//
void quaytuongdoi1(unsigned int i)
{
 if(i!=0)
{    
 tuongdoi1=i;
//enable1=TRUE; //cho phep dong co hoat dong
runflag=1;
 _T1IE=1;//cho phep' ngat'
}
}
//***************************************************************************//
void quaytuongdoi2(unsigned int i)
{
if(i!=0)
{ 
 tuongdoi2=i;
// enable2=TRUE; //cho phep dong co hoat dong
runflag=2;
 _T2IE=1;//cho phep' ngat'
}
}
//***************************************************************************//
void quay1()
{
 clock1=!clock1;
}
//***************************************************************************//
void quay2()
{
clock2=!clock2;
}
//***************************************************************************//
void SystemInit(void) // 
{
	//Init I/O
	TRISB = 0x0000;		//PORTB Ouput
	TRISD=0x00;
	PORTB = 0;
	LATB = 0x0000;		//LATB = 0
	LATD=0x00;
}
//***************************************************************************//
void Init_Timer1(unsigned int i) // C鄆 dat toc do dong co  (Hz)
{
	T1CON = 0b1000000000100000; //prescale 1:64
	PR1 = 46875/(i*2);		//1s

//T1CON = 0b1000000000000000; //prescale 1:64
	//PR1 = 1;		//1s
	_T1IF=0;
}
//***************************************************************************//
void Init_Timer2(unsigned int i)
{
	T2CON = 0b1000000000100000; //prescale 1:64
	PR2 = 46875/(i*2);		//1s
//	T2CON = 0b1000000000000000; //prescale 1:64
//	PR2 = 1;		//1s
	_T2IF=0;
}
//***************************************************************************//
void InitTimer12(unsigned int timer1,unsigned int timer2)
{
    Init_Timer1(timer1);
    Init_Timer2(timer2);
}
//***************************************************************************//
void __attribute__((interrupt, auto_psv)) _T1Interrupt(void) //h鄊 ng?t Timer quay d?ng co 1
{
      quay1(); 
      if(clock1==0) 
      {
          tuongdoi1--;
          if(chieu1==thuan) tuyetdoi1++; //quay thuan
          else if(chieu1==nghich) tuyetdoi1--; //quay nguoc
          if(tuongdoi1==0)
          {
              _T1IE=0;//ket' thuc' viec. di chuyen
              runflag=0;
          }
	  }
	  _T1IF=0; // tiep tuc ngat
}
//***************************************************************************//
void __attribute__((interrupt, auto_psv)) _T2Interrupt(void)
{
	quay2();
	if(clock2==0) 
	{
    	tuongdoi2--;
	if(chieu2==thuan) tuyetdoi2++; //quay thuan
	else if(chieu2==nghich) tuyetdoi2--; //quay nguoc
	if(tuongdoi2==0) 
	{
    	_T2IE=0;//ket' thuc' viec. di chuyen
    	runflag=0;
    }
	}
	_T2IF=0;
}

//***************************************************************************//
//										Main
//***************************************************************************//
int main(void)
{

	SystemInit();
	InitUART(); 
    enable1=1;//cho phep chay
    enable2=1;
	
    direction1=thuan; //truc X
    direction2=thuan; //truc Y
/*  
     InitTimer12(10,10);
     nstt_tuyetdoi(20,20);
     //tuyetdoi(20,20);
     InitTimer12(50,50);
     nstt_tuyetdoi(100,100);
     //tuyetdoi(100,100);
     InitTimer12(200,200);
     nstt_tuyetdoi(400,400);
     //tuyetdoi(400,400);
     InitTimer12(400,400);
     nstt_tuyetdoi(700,700);
     //tuyetdoi(700,700);
     InitTimer12(200,200);
     nstt_tuyetdoi(900,900);
     //tuyetdoi(900,900);
     InitTimer12(100,100);
     nstt_tuyetdoi(1000,1000);
    //tuyetdoi(1000,1000);
     InitTimer12(30,30);
     nstt_tuyetdoi(1050,1050);
     //tuyetdoi(1050,1050);
     InitTimer12(5,5);
     nstt_tuyetdoi(1055,1055);
     //tuyetdoi(1055,1055);
  */   

  /*
     InitTimer12(10,10);
     nstt_tuyetdoi(20,20);
     //tuyetdoi(20,20);
     InitTimer12(50,50);
     nstt_tuyetdoi(100,100);
     //tuyetdoi(100,100);
     InitTimer12(200,200);
     nstt_tuyetdoi(400,400);
     //tuyetdoi(400,400);
     InitTimer12(400,400);
     nstt_tuyetdoi(700,700);
     //tuyetdoi(700,700);
     InitTimer12(200,200);
     nstt_tuyetdoi(900,900);
     //tuyetdoi(900,900);
     InitTimer12(100,100);
     nstt_tuyetdoi(1000,1000);
    //tuyetdoi(1000,1000);
     InitTimer12(30,30);
     nstt_tuyetdoi(1050,1050);
     //tuyetdoi(1050,1050);
     InitTimer12(5,5);
     nstt_tuyetdoi(1055,1055);
     //tuyetdoi(1055,1055);
     */
   // nstt_tuyetdoi(4,6);
  
  
      InitTimer12(200,200);
      nstt_tuyetdoi(300,300);
      nsct_tuyetdoi(1100,300,700,700,thuan);
  
     while(1) ;
}
//********************************* End of Main *********************************//