motor_driver.c

freescale mc9s12hz256 step motor control

#include <MC9S12HZ256.h>     /* derivative information */
#include "Motor_Driver.h"
#include "can.h"
#include "timer.h"
#include <hidef.h> 
unsigned int data_receive[32]; 
unsigned int final_step[4],current_step[4];
unsigned int step_1_times;
unsigned int motor_step2;
unsigned char Duty;

int dir_p[4];
unsigned char state_0;

volatile const char SinTbl4[48]={0,6,11,17,22,29,35,42,48,54,62,70,\
 						77,84,91,97,102,107,112,116,119,123,126,127,\
 						128,128,126,124,121,117,112,107,102,97,91,84,\
 						77,70,62,54,48,42,35,29,22,17,11,6};
volatile const char CosTbl4[48]={128,128,128,127,126,125,123,121,119,116,112,107,\
					    102,97,91,84,77,70,62,54,46,35,24,13,\
					    2,9,20,	31,42,52,62,70,77,84,91,97,\
					    102,107,112,116,119,121,123,125,126,127,128,128};
					    
					    
volatile const char SinTbl4_1[24]=
            {0,2,9,19,32,47,\
 						64,80,96,109,119,126,\
 						128,126,119,109,96,80,\
 						64,47,32,19,9,2};
volatile const char CosTbl4_1[24]=
             {128,126,119,109,96,80,\
					    64,47,32,19,9,2,\
					    0,2,9,19,32,47,\
					    64,80,96,109,119,126};
					    
volatile const char SinTbl4_2[8]=
            {0,19,\
 						64,109,\
 						128,109,\
 						64,19};
volatile const char CosTbl4_2[8]=
             {128,109,\
					    64,19,\
					    0,19,\
					    64,109};					    
volatile const char SinTbl4_3[12]=
            {0,9,32,\
 						64,96,119,\
 						128,119,96,\
 						64,32,9};
volatile const char CosTbl4_3[12]=
             {128,119,96,\
					    64,32,9,\
					    0,9,32,\
					    64,96,119};	
volatile const char SinTbl4_4[12]=
            {0,21,43,\
 						64,83,107,\
 						128,107,83,\
 						64,43,21};
volatile const char CosTbl4_4[12]=
             {128,107,83,\
					    64,43,21,\
					    0,21,43,\
					    64,83,107};						    				    
					    					    
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void init_stepper1()
{	
	MCCTL0=0x00;			  //;fbus(Eclock)=0, FAST=0 (slow mode)
	MCCTL1=0x80;   		  //;RECIRC =1 (low side), disable interrupt for PWM motor
	MCPERW=0x0080;		  //;0x80=128 resoultion frequency = fbus(Eclock)/resoulation 
 					            //                          = 62.5K(8M/128)		
	MCCC0=0x90;			    //;Dual Full H-bridge mode,left aligned
	MCCC1=0x90;			    //;Dual Full H-bridge mode,left aligned
	MCDC0W=0x0000;		  //;Duty cycle channel 0 for micro-stepping initialization
	MCDC1W=0x0000;		  //;Duty cycle channel 1 for micro-stepping initialization
  final_step[0]=00;
  current_step[0]=0000;
  dir_p[0]=0;
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void init_stepper2()
{
  MCCC2=0X90;
  MCCC3=0X90;
  MCDC2W=0X0000;
  MCDC3W=0X0000;
  final_step[1]=0000;
  current_step[1]=0000;
  dir_p[1]=0;
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void init_stepper3()
{
  MCCC4=0X90;
  MCCC5=0X90;
  MCDC4W=0X0000;
  MCDC5W=0X0000;
  final_step[2]=0000;
  current_step[2]=0000;
  dir_p[2]=0;
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void init_stepper4()
{
  MCCC6=0X90;
  MCCC7=0X90;
  MCDC6W=0X0000;
  MCDC7W=0X0000;
  final_step[3]=0000;
  current_step[3]=0000;
  dir_p[3]=0;
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void Motor_init(void) 
{
  init_stepper1(); 
  init_stepper2(); 
  init_stepper3(); 
  init_stepper4(); 
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
unsigned char motor_micro_update_12_dup(unsigned char channel) 
{
  unsigned char	ctemp,dir,i;
  volatile unsigned char *address;
  unsigned long data_temp;
  int		temp,itemp;
  itemp=final_step[channel-1]-current_step[channel-1];
  
	if((itemp>24)||(itemp<-24))//if (itemp!=0)//
	{
    if ((itemp<0)&&(dir_p[channel-1]<1))
    {
      dir_p[channel-1]=-1;
      current_step[channel-1]--;
      address=&MCDC0H+(channel-1)*4;
	    temp =current_step[channel-1];
	    ctemp=temp%24;
	    if((ctemp<6))
	    {
	      *address|=S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if((ctemp>=6)&&(ctemp<12))
	    {
	      *address|=S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if((ctemp>=12)&&(ctemp<18))
	    {  
	      *address&=~S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if((ctemp>=18)&&(ctemp<24))
	    {
	      *address&=~S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    if(ctemp>=12)
	     ctemp-=12;	  	
	    *(address+1)=SinTbl4_4[ctemp];
	    *(address+3)=CosTbl4_4[ctemp];							
    }
    else if((itemp<0)&&(dir_p[channel-1]==1))
    {
       dir_p[channel-1]=-1;
    }
	  else if ((itemp>0)&&(dir_p[channel-1]>-1))
	  {
	     dir_p[channel-1]=1;
	     current_step[channel-1]++;	   
	     address=&MCDC0H+(channel-1)*4;
	     temp =current_step[channel-1];
	     ctemp=temp%24;	   
	     if((ctemp<6))
	     {
	      *address&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	     }
	     else if((ctemp>=6)&&(ctemp<12))
	     {
	      *address&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	     }
	     else if((ctemp>=12)&&(ctemp<18))
	     {  
	      *address|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	     }
	     else if((ctemp>=18)&&(ctemp<24))
	     {
	      *address|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	     }
	     if(ctemp>=12)
	       ctemp-=12;	  	
	     *(address+1)=SinTbl4_4[ctemp];
	     *(address+3)=CosTbl4_4[ctemp];		 
	  }
	  else if((itemp>0)&&(dir_p[channel-1]==-1))
	  {
	     dir_p[channel-1]=1;
	  }
  }
  else 
  {
    dir_p[channel-1]=0;
    task_can_en=1;
  }//
  time_delay(20);
  return 1; 
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void motor_micro_update_12(unsigned char channel) 
{
  unsigned char	ctemp,dir,i;
  volatile unsigned char *address;
  int		temp,itemp;
  itemp=final_step[channel-1]-current_step[channel-1];
	if (itemp!=0)
	{
    if (itemp<0)
    {
     dir=0;
     current_step[channel-1]--;
    }
	  else if (itemp>0)
	  {
	   dir=1;
	   current_step[channel-1]++; 
	  }
	  address=&MCDC0H+(channel-1)*4;
	  temp =current_step[channel-1];
	  ctemp=temp%24;		
    if(dir == 1)
	  {
	    if((ctemp<6))
	    {
	      *address&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if((ctemp>=6)&&(ctemp<12))
	    {
	      *address&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if((ctemp>=12)&&(ctemp<18))
	    {  
	      *address|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if((ctemp>=18)&&(ctemp<24))
	    {
	      *address|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }	
					  
	  }
    else
	  { 
	    if((ctemp<6))
	    {
	      *address|=S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if((ctemp>=6)&&(ctemp<12))
	    {
	      *address|=S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if((ctemp>=12)&&(ctemp<18))
	    {  
	      *address&=~S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if((ctemp>=18)&&(ctemp<24))
	    {
	      *address&=~S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(address+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }												
	  } 
	
	  if(ctemp>=12)
	   ctemp-=12;	  	
	  *(address+1)=SinTbl4_3[ctemp];
	  *(address+3)=CosTbl4_3[ctemp];
  }
  else 
  {
   task_can_en=1;
  }
}
/**************************************************************************************************
*
*
*
***************************************************************************************************/
void motor_update(unsigned char channel)
{
 	volatile unsigned char	ctemp,*motor_channel;
 	unsigned char dir;
	int		temp,itemp;
	
	itemp=final_step[channel]-current_step[channel];
	
	if (itemp!=0)
	{
	  if (itemp<0)
	  {
	    current_step[channel]--;
	    dir=0;
	  }
	  else if (itemp>0)
	  {
	    current_step[channel]++;
	    dir=1;
	  }
	  motor_channel=&MCDC0H+channel*4;
	
	  temp = current_step[channel];
    asm	{
		ldd		temp;
		ldx		#4
		idiv
		stab	ctemp
	  }		
	  if(dir == 1)
	  {
	    if(ctemp==0)
	    {
	      *motor_channel&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(motor_channel+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if(ctemp==1)
	    {
	      *motor_channel&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(motor_channel+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if(ctemp==2)
	    {  
	      *motor_channel|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(motor_channel+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if(ctemp==3)	
	    {
	      *motor_channel|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(motor_channel+2)|=S1_DTC;		//;Duty cycle channel 1 (B PWM /B low)
	    }
	  }
	  else
	  {
	    if(ctemp==0)
	    {
	      *motor_channel&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(motor_channel+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }
	    else if(ctemp==1)
	    {
	      *motor_channel&=~S0_DTC;		//;Duty cycle channel 0 (A low /A PWM)
	      *(motor_channel+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if(ctemp==2)
	    {  
	      *motor_channel|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(motor_channel+2)|=S1_DTC;	//;Duty cycle channel 1 (B PWM /B low)
	    }
	    else if(ctemp==3)
	    {
	      *motor_channel|=S1_DTC;		//;Duty cycle channel 0 (A PWM /A low)
	      *(motor_channel+2)&=~S0_DTC;	//;Duty cycle channel 1 (B low /B PWM)
	    }						  
	  }	   			  			
  }
  /*else		 
  {
	  MotorStatus[channel]=OFF;		
  }*/ 
}