?? motor_driver.c
字號:
#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;
}*/
}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -