📄 3sifu_pid.c
字号:
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=330; */
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 PID interrupt */
void pid_isr()
{ int kp,ki,kd,delt_i,delt_p1,delt_p2,delt_p;
int e,kk;
kp=1;ki=1;kd=1;
delt_p=p_given-position;
e=15;
if (delt_p<e)
{
delt_i=kp*delt_p+kd*(delt_p2-delt_p1);
}
else
delt_i=kp*delt_p+ki*delt_p+kd*(delt_p2-delt_p1);
IA_max=IA_max+delt_i;
IA_min=IA_min+delt_i;
delt_p1=delt_p2;
delt_p2=delt_p;
}
/*************************************/
/* 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; */
}
/* display data on LCD */
void display(display_value,dot_bit)
unsigned long display_value; /* the value want to be displayed */
int dot_bit; /* dot bit */
{
unsigned long bit_value[4];
unsigned int x,port1_value;
port1_value=0;
if (display_value>=2000)
port1_value=port1_value|0x40;
bit_value[3]=display_value%10; /* the lowest bit data */
bit_value[2]=(display_value/10)%10;
bit_value[1]=(display_value/100)%10;
bit_value[0]=display_value/1000; /* the highest bit data */
if (bit_value[0]==1)
port1_value=port1_value|0x02;
if (sign_state)
port1_value=port1_value|0x01;
x=dot_bit;
if ((x>=1)&&(x<=3))
port1_value=port1_value|(1<<(x+1)); /* show radix point */
port1=port1_value;
if ((dot_bit!=1)&&(dot_bit!=2)&&(bit_value[0]==0)&&(bit_value[1]==0))
port2=0x0f;
else
port2=bit_value[1];
if (((dot_bit==3)&&(bit_value[1]==0)&&(bit_value[0]==0))||((dot_bit==4)&&(bit_value[0]==0)&&(bit_value[1]==0)&&(bit_value[2]==0)))
port3=0x0f;
else
port3=bit_value[2];
port4=bit_value[3];
}
/* refresh LCD */
void LCD_refresh()
{
unsigned long velocity;
int i;
struct CAP_INT * p1;
unsigned long delta_count;
if (display_times>=1000)
{
display_times=0;
switch (display_order)
{
case 1: /* read velocity */
delta_count=0;
p1=CAP_p;
for(i=1;i<=6;i++)
{
delta_count=delta_count+p1->delta_count;
p1=p1->next;
}
if (delta_count==0)
velocity=0;
else
velocity=VELOCITY_CONST*60/delta_count;
velocity_value=(unsigned int)velocity;
velocity_dotbit=4;
display(velocity_value,velocity_dotbit);
break;
case 2: /* read position */
position=800;
position_value=position;
if (position_value%10)
{
position_value=position;
position_dotbit=3;
if (position_value>2000)
{
position_value=position/10;
position_dotbit=4;
}
}
else
{
position_value=position;
position_dotbit=4;
}
display(position_value,position_dotbit); break;
case 3: display(input_value,input_dotbit); break;
default: break;
}
}
}
/* ***************************************************************** */
/* The main program
/* ***************************************************************** */
main()
{
unsigned int INT1_status;
unsigned int mid;
disable_interrupts();
dsp_setup();
eventmgr_init();
init_SRM();
enable_interrupts();
for(;;)
{
port0=0x00;
INT1_status=XINT1CR;
if (INT1_status & 0x8000)
{
keyboard_ISR();
XINT1CR=0x0006;
}
LCD_refresh();
mid=WDCR;
if (mid & 0x080)
{WDKEY=0x55;
WDKEY=0xAA;
mid = mid|0x68;
WDCR = mid;
}
if (run_state)
{
if (input_value)
{
if (input_value<=15)
{
run_state=0;
PBDATDIR=0x0701;
/*PBDATDIR=align_PBvalue; */
}
else
{
/*if (sign_state)
{if ((input_value*8)<=position)
run_state=3;
}
else if ((input_value*8)<=position)
run_state=3; */
/*if ((input_value*9)<=position)
run_state=3; */
if ((input_value*10-75)<=position)
{
run_state=0;
PBDATDIR=0x0704;
/*PBDATDIR=align_PBvalue;*/
}
}
}
}
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -