📄 youyakokngzhi.c
字号:
{
delayuS(2); //延时
clk_hight; //产生一个上升沿
delayuS(5); //延时，等待CPLD将数据可靠地送到并口
cpld_dat[kk] = read_Byte(); //从CPLD并口读取一个字节数据
clk_low; //拉低CLK，准备读取下一个字节
}
cs_hight; //CS禁止，关闭CPLD并口
//--------------------------------------------------------------------------------
ddtt=0;
ddtt2=0;
ddtt=(ddtt|cpld_dat[0])<<8;
ddtt=(ddtt|cpld_dat[1])<<8;
ddtt=(ddtt|cpld_dat[2]);
ddtt2=(ddtt2|cpld_dat[3])<<8;
ddtt2=(ddtt2|cpld_dat[4])<<8;
ddtt2=(ddtt2|cpld_dat[5]);
//U0THR=0x20;
//U0THR=cpld_dat[0];
//U0THR=cpld_dat[1];
//U0THR=cpld_dat[2];
yabianli=ddtt;
shiyanli=ddtt2;
//-------------------------------------------------------------------------------
pwm=0xff; //控制周期到标志，通知主程序启动力值计算与PID控制，主程序复位
EXTINT=0x01; /*清除EINT0中断标志*/
VICVectAddr=0x00;
}
/******************************初始力值计算开始******************************************/
void lizhijisuan(void)
{
lzad_max=(lzvout_max/ADVIN_FULL)*MLCMS; /*力值理论满量程输出对应的A/D输出，（18位有效分辨率）*/
if(yabianli<0x800000) /*力值采样为正（相对拉向）*/
{
yabianli=(yabianli>>YYWS); /*取18位有效分辨率*/
yabianli=yabianli&QCFH;
ybl_sjz=((fp64)yabianli/(fp64)lzad_max)*ybl_bzlc; /*计算初始力值*/
ybl_sjz=-ybl_sjz; /*当前力值取负*/
ybl_sjz=ybl_sjz-ybl_clr;
}
if(yabianli>0x7fffff) /*力值采样为负（相对拉向）*/
{
yabianli=yabianli>>YYWS; /*取18位有效分辨率*/
yabianli=(~yabianli)+1; /*取补码*/
yabianli=yabianli&QCFH;
ybl_sjz=((fp64)yabianli/(fp64)lzad_max)*ybl_bzlc; /*计算当前力值*/
// ybl_sjz=-ybl_sjz; /*当前力值取负*/
ybl_sjz=ybl_sjz-ybl_clr;
}
if(shiyanli<0x800000) /*力值采样为正（相对拉向）*/
{
shiyanli=(shiyanli>>YYWS); /*取18位有效分辨率*/
shiyanli=shiyanli&QCFH;
syl_sjz=((fp64)shiyanli/(fp64)lzad_max)*syl_bzlc; /*计算初始力值*/
syl_sjz=-syl_sjz; /*当前力值取负*/
syl_sjz=syl_sjz-syl_clr;
}
if(shiyanli>0x7fffff) /*力值采样为负（相对拉向）*/
{
shiyanli=shiyanli>>YYWS; /*取18位有效分辨率*/
shiyanli=(~shiyanli)+1; /*取补码*/
shiyanli=shiyanli&QCFH;
syl_sjz=((fp64)shiyanli/(fp64)lzad_max)*syl_bzlc; /*计算当前力值*/
// syl_sjz=-syl_sjz; /*当前力值取负*/
syl_sjz=syl_sjz-syl_clr;
}
}
/*****************************************************************************************/
/**********************************************************************************/
void bilikongzhi(void)
{
fp64 ybl_chazhi,syl_chazhi;
if(chuzhi_bz==0x00)
{
ybl_bc=ybl_sjz;
syl_bc=syl_sjz;
chuzhi_bz=0xff;
}
syl_chazhi=syl_sjz-syl_bc;
newybl=ybl_bc+syl_chazhi*2;
ybl_chazhi=newybl-ybl_sjz;
if(ybl_chazhi>=0)
{
fxkz=0x00;
rOut=ybl_chazhi;
}
else
{
fxkz=0xff;
rOut=-ybl_chazhi;
}
}
fp64 PIDCalc(PID *pp, fp64 NextPoint)
{
fp64 dError,Error;
Error = pp->SetPoint - NextPoint;
pp->SumError += Error;
dError = pp->LastError - pp->PrevError;
pp->PrevError = pp->LastError;
pp->LastError = Error;
return (pp->Proportion * Error
+ pp->Integral * pp->SumError
+ pp->Derivative * dError);
}
void PIDyunsuan (void)
{
PID sPID; // PID Control Structure
fp64 rIn; // PID Feedback (Input)
sPID.Proportion = 0.5;
sPID.Integral = 0.5;
sPID.Derivative = 0.0;
sPID.SetPoint = ybl_sjz; // Set PID Setpoint
rIn = syl_sjz; // Read Input
rOut = PIDCalc ( &sPID,rIn ); // Perform PID Interation
}
/*******************************************************************************************************
** 名称：PWM_Out()
** 功能：初始化PWM功能模块，并由PWM输出波形
** 说明：PWM6输出，暂空比50%，使用11.0592 MHz（Fosc）外接晶振，Fcclk＝（Fosc * 4）＝44.2368 MHz
** Fpclk＝(Fcclk / 4) * 1＝11.0592 MHz，设置PR＝0，即定时器计数频率＝11.0592 MHz
** 入口参数：Servo_sign,Servo_puls
** 1.Servo_sign:输入脉冲符号，0x00（正向），0xff（反向）
** 2.Beam_speed:横梁移动速度
** 出口参数：无
** 全局变量：无
** 调用函数：无
********************************************************************************************************/
void PWM_Out(uint8 Servo_sign,uint32 Beam_speed)
{
uint8 i; /*延时计数*/
uint32 pwm_cyc; /*PWM周期*/
if(Servo_sign==0x00) IO0CLR=SN; /*伺服方向控制：正向*/
if(Servo_sign==0xff) IO0SET=SN; /*伺服方向控制：反向*/
for(i=0;i<=120;i++) /*符号超前脉冲>=2.5uS，此处120×0.025uS（单周期）约＝3uS*/
{
}
pwm_k=10000;
PWMPR=0x00; /*不分频，计数频率为Fpclk*/
//PWMMCR=0x02; /*设置PWMMR0匹配时复位PWMTC*/
pwm_cyc=(uint32)(pwm_k/Beam_speed); /*计算PWM周期*/
PWMMR0=pwm_cyc; /*设置PWM周期*/
PWMMR5=(uint32)(pwm_cyc/2); /*设置PWM5占空比*/
PWMLER=0x21; /*PWMMR0、PWMMR5锁存*/
PWMPCR=0x2000; /*允许PWM5输出，单边PWM*/
PWMTCR=0x09; /*启动定时器，PWM使能*/
}
/*******************************************************************************************************
** 名称：PWM_Stop()
** 功能：停止PWM波形输出
** 入口参数：无
** 出口参数：无
** 全局变量：无
** 调用函数：无
********************************************************************************************************/
void PWM_Stop(void)
{
uint8 i;
PWMPCR=0x00; /*禁止PWM5输出*/
PWMTCR=0X02; /*频繁切换频率时，关闭前必需复位计数器*/
PWMTCR=0X00; /*停止PWM定时器、计数器，PWM禁能*/
// VICIntEnClr=0x00004000; /*关闭EINT0中断*/
for(i=0;i<=120;i++) /*脉冲停止后延时>=2.5uS再改变符号，此处120×0.025uS（单周期）约＝3uS*/
{
}
}
/********************************************************************************************************/
//void task_read(void)
//{
// if(cpld_int==0xff)
// {
// cpld_data=read_Byte();
// }
//}
/********************************************************************************************************/
void task_send(void)
{
uint8 kk;
uint8 temp[6];
uint16 tx_tp1;
uint32 ddtt;
if(sq.front!=sq.rear) //发送数据
{
sq.front=(sq.front+1)%QUEUEMAX; //调整队首指针
ddtt=sq.L1[sq.front]; /*压边力值数据*/
// tx_tp1=ddtt/0x10000;
temp[0]=ddtt/0x10000; /*位移高字节*/
// temp[0]=tx_tp1%0x100; /*位移次高字节*/
tx_tp1=ddtt%0x10000;
temp[1]=tx_tp1/0x100; /*位移中字节*/
temp[2]=tx_tp1%0x100; /*位移低字节*/
temp[3]=0x20;
U0THR=0x20; //数据包指令
for(kk=0;kk<3;kk++) //3字节数据+1字节指令
{
if(temp[kk]==0xfe) //数据中出现0xfe结束符,进行转义处理
{
U0THR=0x7d;
temp[3]+=0x7d;
U0THR=0xee;
temp[3]+=0xee;
}
else if(temp[kk]==0x7d) //数据中出现0x7d转义符,进行转义处理
{
U0THR=0x7d;
temp[3]+=0x7d;
U0THR=0x6d;
temp[3]+=0x6d;
}
else
{
U0THR=temp[kk];
temp[3]=temp[3]+temp[kk];
}
}
U0THR=temp[3]; //发送校验和
U0THR=0xfe; //发送结束符
while( (U0LSR&0x20)==0 ); /*等待数据发送完成*/
}
}
/********************************************************************************************************/
void cmd_rcv(void)
{
uint32 uarttmp1,uarttmp2;
if(online==0x00)
{
if(uartrcv_new==0xff) //接收PC机联机指令
{
if(uartrcv[0]==0x10)
{
sq.front=sq.rear=QUEUEMAX-1; //初始化空队
online=0xff; //联机标志置位
U0THR=0x10;
U0THR=0x10;
U0THR=0xfe;
}
uartrcv_new=0x00;
uartrcv_cnt=0x00;
}
else if(uartrcv_new==0x88)
{
if(uartrcv[0]==0x10)
{
U0THR=0x22;
U0THR=0x10;
U0THR=0x32;
U0THR=0xfe;
}
uartrcv_new=0x00;
uartrcv_cnt=0x00;
}
}
if(online==0xff)
{
if(uartrcv_new==0xff)
{
switch(uartrcv[0])
{
case 0x11:online=0x00; //脱机指令
yunxing=0x00;
U0THR=0x11;
U0THR=0x11;
U0THR=0xfe;
break;
case 0x14: //夹紧
U0THR=0x14;
U0THR=0x14;
U0THR=0xfe;
while( (U0LSR&0x20)==0 ); /*等待数据发送完成*/
uarttmp1=uartrcv[2]*0x10000+uartrcv[3]*0x100+uartrcv[4];
uarttmp2=uartrcv[5]*0x10000+uartrcv[6]*0x100+uartrcv[7];
jiajinli=uarttmp1*1.0+uarttmp2/10000000.0;
jiajinchuli=0xff;
break;
case 0x15: //松弛
U0THR=0x15;
U0THR=0x15;
U0THR=0xfe;
while( (U1LSR&0x20)==0 ); /*等待数据发送完成*/
uarttmp1=uartrcv[2]*0x10000+uartrcv[3]*0x100+uartrcv[4];
uarttmp2=uartrcv[5]*0x10000+uartrcv[6]*0x100+uartrcv[7];
songchili=uarttmp1*1.0+uarttmp2/10000000.0;
songchichuli=0xff;
break;
case 0x16:
// PWM_Out(0x00,4);
yunxing=0xff; //运行
U0THR=0x16;
U0THR=0x16;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -