📄 sensorless.c
字号:
#include "C240.h"
#define TRUE 1
#define FALSE 0
#define VELOCITY_CONST 3125000
/* define variable */
static unsigned int PBvalue[4][6]=
{{0x0703,0x0704,0x0706,0x0701,0x0702,0x0705},
{0x0705,0x0702,0x0704,0x0703,0x0706,0x0701},
{0x0704,0x0703,0x0701,0x0706,0x0705,0x0702},
{0x0702,0x0705,0x0703,0x0704,0x0701,0x0706}};
unsigned int align_PBvalue;
unsigned int disalign_PBvalue;
unsigned int CAP_A;
unsigned int CAP_B;
unsigned int CAP_C;
unsigned int IA_max,IB_max,IC_max; /* MAX limit for current */
unsigned int IA_min,IB_min,IC_min; /* MIN limit for current */
/*unsigned int ADC_channel; /* Channel for A/D convert */
int display_times; /* accumulation for display */
int input_state;
int sign_state;
int start_mode;
int run_state;
int dot_state;
int CAP2_flag;
int CAP3_flag;
int inputdata_num;
unsigned long input_value;
unsigned int input_dotbit;
unsigned long position_value;
unsigned int position_dotbit;
unsigned long position;
unsigned long velocity_value;
unsigned int velocity_dotbit;
int display_order; /* 1 while display velocity ,2 while display current ,3 while display input data */
/* define Input/Output variable value */
ioport unsigned int port0,port1,port2,port3,port4,port5;
/* define structure type and variable value */
struct CAP_INT
{
unsigned int CAP_value; /* the capture value of CAP unit */
unsigned long delta_count; /* the delta value of 2 times ajacent capture value */
struct CAP_INT * next;
};
struct CAP_INT * CAP_p;
/*********************************************/
/* Creat circle linkage table for CAP */
struct CAP_INT * creat_link()
{
int i;
struct CAP_INT * h,* p; /* pointer h point to virtual value */
h=(struct CAP_INT *)malloc(sizeof(struct CAP_INT));
h->next=h;
for(i=1;i<=5;i++)
{
p=(struct CAP_INT *)malloc(sizeof(struct CAP_INT));
p->CAP_value=0;
p->delta_count=0xffff;
p->next=h->next;
h->next=p;
}
return (h); /* return head pointer */
}
/*********************************************/
void dsp_setup()
{
unsigned int temp;
/*********************************************/
/* Disable watchdog timer */
/*********************************************/
temp = WDCR;
temp = temp|0x68;
WDCR = temp;
/*********************************************/
/* initialize PLL module (20 MHz XTAL1) */
/*********************************************/
CKCR1 = 0x60; /* crystal=20MHz CPUCLK = 20MHz */
/* and 1x PLL mult ratio */
CKCR0 = 0xc3; /* low–power mode 0, */
/* ACLK enabled, */
/* PLL enabled, */
/* SYSCLK=CPUCLK/2 */
SYSCR = 0x40c0;
XINT1CR=0x0006;
}
/* Initiate the event management register */
void eventmgr_init()
{
GPTCON=0x0042;
/* No period interrupt start A/D */
/* Initialize PWM */
/* Period is 100us */
ACTR=0x0fff;
COMCON=0x4a57;
/*ACTR=0x0aaa;
CMPR1=0x01f4;
CMPR2=0x01f4;
CMPR3=0x01f4;
DBTCON=0x0000;
COMCON=0x4a57;
COMCON=0xca57;*/
T1PER=0x07d0;
T1CON=0x0000;
T1CON=0x910e;
/* enable PWM output */
T1CON=0x914e;
/* Initial timer 2 */
/* Initialize T2 */
/*T2CNT=0x00;
/*T2PER=0x0190;
/*T2CON=0x9000; /* Period is 20us */
/*T2CON=0x9040;
/* Setup shared pins */
OCRA = 0xf800; /* pins IOPB0–IOPB3 & IOPA0–IOPA2 to I/O pins */
OCRB = 0x00f0; /* pins are: ADSOC, XF, /BIO, CAP1–CAP4 */
PBDATDIR = 0x0700;
/* outputs IOPB0–IOPB2, set low */
PADATDIR = 0x0707; /* outputs IOPA0–IOPA2, set high */
/* Initialize T3 */
T3CNT=0x0;
T3PER=0xffff;
T3CON=0x9440; /* Prescaler=/16 */
/* Setup capture units */
CAPCON = 0x0; /* reset capture control register */
CAPFIFO= 0x0ff; /* Clear FIFO’s */
CAPCON = 0xb6fc; /* enable #1,#2,#3 use Timer3, both edges */
}
/*******************************************************************/
void disable_interrupts()
{
asm(" SETC INTM");
}
/************************************************************************/
void enable_interrupts()
{
IFR = 0xffff; /* Clear pending interrupts */
IFRA = 0xffff;
IFRB = 0xffff;
IFRC = 0xffff;
IMR = 0x0022; /* Enable CPU Interrupts:INT 2,3,4,6 */
IMRA = 0x0080; /* Enable timer 1 period interrupts */
IMRB = 0x0000; /* Enable timer 3 period */
IMRC = 0x0000; /* Enable CAP1~3 interrupts*/
/* start ADC */
ADCTRL1=0xc904;
ADCTRL1=0xca05;
ADCTRL2=0x0005;
asm(" CLRC INTM"); /* Global interrupt enable */
}
/* initiating parameter and register */
void init_SRM()
{
velocity_value=0;
velocity_dotbit=4;
position_value=position=0;
position_dotbit=4;
input_value=0;
input_dotbit=4;
display_times=0;
input_state=FALSE;
sign_state=FALSE;
dot_state=FALSE;
display_order=3;
/*inputdata_num=0;
start_mode=0; */
run_state=0; /* 0,idle;1 while accelerate;3 while disaccelerate */
CAP_A=CAP_B=CAP_C=0;
CAP2_flag=FALSE;
CAP3_flag=FALSE;
/*IA_max=IB_max=IC_max=0x8c00; /* 3.3A, Voltage from LEM is 2.738V */
/*IA_min=IB_min=IC_min=0x8800; /* 2.25A, Voltage from LEM is 2.659V */
/*IA_max=IB_max=IC_max=0x8700; /* 2A,Voltage from LEM is 2.642V */
/*IA_min=IB_min=IC_min=0x83C0; /* 1.024A, Voltage from LEM is 2.577V */
/*IA_min=IB_min=IC_min=0x82c0; /* 0.774A,Voltage from LEM is 2.558V */
/*IA_min=IB_min=IC_min=0x96c0; /* 7A, Voltage from LEM is 3.018V */
/*IA_min=IB_min=IC_min=0x96c0; /* 6A,Voltage from LEM is 2.948V */
/*IA_max=IB_max=IC_max=0x9e00; /* 8A,Voltage from LEM is 3.092V */
/*IA_max=IB_max=IC_max=0xa600; /* 10A, Voltage from LEM is 3.25V */
/*IA_min=IB_min=IC_min=0xa240; /* 9A,Voltage from LEM is 3.175V */
IA_max=IB_max=IC_max=0x92c0; /* 5A, Voltage from LEM is 3.25V */
IA_min=IB_min=IC_min=0x8f00; /* 4A,Voltage from LEM is 3.175V */
}
/********************************************************** */
/* READ CAPTURE FIFO REGISTERS
/* This routine is used to read the data from the capture FIFO
/* registers.
/*
/* inputs: capture = which FIFO to read?
/* range = 1–3
/* outputs fifo_data =
/* range = 0–65535
/********************************************************* */
unsigned int read_fifo(int capture)
{
unsigned int fifo_data;
int fifo_status;
if (capture == 1)
{
do {
fifo_data = FIFO1; /* read value */
fifo_status = CAPFIFO & 0x0300; /* read status register, mask bits */
}
while (fifo_status != 0);
}
else if (capture == 2)
{
do {
fifo_data = FIFO2; /* read value */
fifo_status = CAPFIFO & 0x0c00; /* read status register, mask bits */
}
while (fifo_status != 0);
}
else if (capture == 3)
{
do {
fifo_data = FIFO3; /* read value */
fifo_status = CAPFIFO & 0x3000; /* read status register, mask bits */
}
while (fifo_status !=0);
}
else fifo_data = 0xffff; /* error, not a valid capture */
return fifo_data;
}
/*********************************************/
/* ISR for GPT1 interrupt */
void c_int2()
{
IFR=0x0002; /* clear CPU interrupt flag */
IFRA=0x7ff;
display_times++;
}
/*************************************/
/* drive SRM */
void drive_SRM()
{
int i;
struct CAP_INT * p;
CAP_p=creat_link();
p=CAP_p;
for(i=1;i<=6;i++)
{
p->delta_count=0xfffff;
p->CAP_value=0;
p=p->next;
}
if (sign_state) /* drive SRM */
PBDATDIR=0x0702;
else
PBDATDIR=0x0704;
}
/*********************************************/
void keyboard_ISR()
{
int i;
unsigned int scan_in,scan_out,x2;
unsigned long align_position;
int row,column=0,key_NO; /* row=0,1,2; column=1,2,3,4 */
row=4;
for(i=0;i<5000;i++);
scan_in=port5;
x2=scan_in & 0x0f;
if (x2!=0x0f)
{
switch(x2)
{
case 0x0e: column=4;
break;
case 0x0d: column=3;
break;
case 0x0b: column=2;
break;
case 0x07: column=1;
break;
default : column=17;
}
scan_out=0x0f7;
port0=scan_out; /* scan keyboard */
for(i=0;i<1000;i++);
scan_in=port5;
while ((scan_in & 0x0f)==0x0f)
{
scan_out=scan_out>>1;
port0=scan_out;
for(i=0;i<1000;i++);
scan_in=port5;
row--;
if (row==0)
{
row=17;
break;
}
}
key_NO=4*(row-1)+column;
port0=0x00;
x2=port5;
while ((x2&0x0f)!=0x0f)
{
x2=port5;
port0=0x00;
}
for(i=0;i<7500;i++);
x2=port5;
while ((x2&0x0f)!=0x0f)
{
x2=port5;
port0=0x00;
}
switch(key_NO)
{
case 1 :
case 2 :
case 3 :
case 4 :
case 5 :
case 6 :
case 7 :
case 8 :
case 9 :
case 10 : if (input_state==FALSE) /* deal with data keyboard */
break;
if (dot_state)
input_dotbit--;
input_value=input_value*10+key_NO-1;
inputdata_num++;
break;
case 11 : if (input_state==FALSE) /* deal with radix point keyboard */
break;
if (dot_state)
break;
if (inputdata_num!=0)
{
input_dotbit=3;
dot_state=TRUE;
break;
}
case 12 : if (input_state==FALSE) /* deal with data keyboard */
break;
if (sign_state) /* deal with sign keyboard */
sign_state=FALSE;
else
{
sign_state=TRUE;
position=0;
CAP_A=CAP_B=CAP_C=0;
CAP2_flag=FALSE;
CAP3_flag=FALSE;
run_state=1;
PBDATDIR=0x0702;
}
break;
case 13 : init_SRM();
/*input_value=360;*/
PBDATDIR = 0x0701; /* outputs IOPA0–IOPA2, IOPA0 set high */
input_state=TRUE;
break;
case 14 : display_order++;
if (display_order==4)
display_order=1;
break;
case 15 :
run_state=1;
drive_SRM();
break;
case 16 :
run_state=0;
PBDATDIR = 0x0703; /* outputs IOPA0–IOPA2, IOPA0 set high */
break;
default : break;
}
}
}
/*************************************/
/* ISR for A/D and keyboard interrupt */
void c_int6()
{
unsigned int ADCfifo_data,adc1data,channel;
int ADCfifo_status;
/*PCDATDIR=0x0700; */
IFR=0x0020;
/*do {
/* ADCfifo_data= ADCFIFO1; /* read value */
/* ADCfifo_status = ADCTRL2 & 0x0c0; /* read status register, mask bits */
/* }
while (ADCfifo_status != 0); */
ADCfifo_data= ADCFIFO1;
adc1data= ADCFIFO1;
if (adc1data!=0)
ADCfifo_data=adc1data;
channel=ADCTRL1 & 0x0e;
switch(channel)
{
case 6: if (ADCfifo_data>=IB_max)
PADATDIR=PADATDIR & 0x070d;
else if (ADCfifo_data<=IB_min)
PADATDIR=PADATDIR | 0x02;
ADCTRL1=0xc908;
ADCTRL1=0xca09;
break;
case 8:
if (ADCfifo_data>=IC_max)
PADATDIR=PADATDIR & 0x070b;
else if (ADCfifo_data<=IC_min)
PADATDIR=PADATDIR | 0x04;
ADCTRL1=0xc904;
ADCTRL1=0xca05;
break;
default:
case 4: if (ADCfifo_data>=IA_max)
PADATDIR=PADATDIR & 0x070e;
else if (ADCfifo_data<=IA_min)
PADATDIR=PADATDIR | 0x01;
ADCTRL1=0xc906;
ADCTRL1=0xca07;
break;
}
/*ADC_channel++;
if (ADC_channel>4)
ADC_channel=2;
ADCTRL1=0xca00 + (ADC_channel<<1);
ADCTRL1=ADCTRL1 | 1; */
/*PCDATDIR=0x0701; */
}
/* ***************************************************************** */
/* The main program
/* ***************************************************************** */
main()
{
unsigned int INT1_status;
disable_interrupts();
dsp_setup();
eventmgr_init();
init_SRM();
enable_interrupts();
for(;;)
{
port0=0x00;
INT1_status=XINT1CR;
if (INT1_status & 0x8000)
{
keyboard_ISR();
XINT1CR=0x0006;
}
}
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -