|
|
磁悬浮代码在此，代码写得不是很好，很简单，仅供参考。
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include "OLED12864.h"
#define LL PWMDTY23
#define LH PWMDTY01
#define RL PWMDTY45
#define RH PWMDTY67
#define N 12
uint valuex_buf[N]; //浮子位置采集数组
uint valuey_buf[N];
char i=0,i_y=0;
uint ADx_value,ADy_value;
int NET=0;
float A=0,B=0;
float AD_X,AD_Y;
float E_AD_X,E1_AD_X,E_AD_Y,E1_AD_Y;
float P_x =0.8;
float D_x =15.5;
float P_y =-0.8;
float D_y =-15;
float pwm_x,pwm_y;
//float Pwm_x = -300,Pwm_y = -300;
float SET_AD_X = 1400;
float SET_AD_Y = 1370;
//**************************************************
//****** 设置主频 *******
//**************************************************
void Busclock(void) //05,01 48MHz 06,01 56MHz 07,01 64MHz 08,01 72MHz
{
CLKSEL = 0x00;//初始化PLL前先使用外部晶振
PLLCTL_PLLON = 1;//开启PLL
SYNR = 0xC0 | 0x08;
REFDV = 0xC0 | 0x01;
POSTDIV = 0x00;
_asm(nop);
_asm(nop);
while(!(CRGFLG_LOCK==1));//等待PLL稳定
CLKSEL_PLLSEL = 1; //fbus采用根据PLL频率设定
}
//****************定时器初始化********************************
void initPIT()//定时中断初始化函数1MS 定时中断设置
{
PITCFLMT_PITE=0; //定时中断通道0 关
PITCE_PCE0=1;//定时器通道0 使能
PITMTLD0=64-1;//8 位定时器初值设定。240 分频，在24MHzBusClock 下，为0.1MHz 1us
PITLD0= 1000;//16 位定时器初值设定。PITTIME*0.01MS 1000*0.001=1ms
PITINTE_PINTE0=1;//定时器中断通道0 中断使能
PITCFLMT_PITE=1;//定时器通道0 使能
}
//************************************************************
//******************12位AD初始化******************************
//************************************************************
void int_ad() //AD初始化，12位精度
{
ATD0DIEN = 0;
ATD0CTL1 = 0x50;
ATD0CTL2 = 0x40;
ATD0CTL3 = 0xb0;
ATD0CTL4 = 0x29;
ATD0CTL5 = 0x30;
}
//***************读出AD值**********************
void get_ad() //读取AD值，数值放在数组中，用于中值滤波
{
while(ATD0STAT0&0X80!=1);
ADx_value=ATD0DR2;
ADy_value=ATD0DR3;
}
//---------------------------PWM初始化---------------------------------------
void PWM_speed_Init(void)
{
PWME_PWME1=0; //禁止
PWME_PWME3=0;
PWMPRCLK=0x22; //64/4=16MHz //72/4=18MHZ
PWMCLK_PCLK1=0; //设A为其时钟源
PWMCLK_PCLK3=0; //B为其时钟源
PWMPOL_PPOL1=1; //上升沿翻转
PWMCAE_CAE1=0; //左对齐输出
PWMPOL_PPOL3=1; //上升沿翻转
PWMCAE_CAE3=0; //左对齐输出
PWMCTL_CON01=1; //0，1级连方式
PWMCTL_CON23=1; //2，3级连方式
PWMDTY01=0; //初始化占空比为0的波形
PWMDTY23=0;
PWMPER01=1600; //输出为10KHz的波
PWMPER23=1600;
PWMCNT01=0x00; //2通道计数器清0
PWMCNT23=0x00; //3通道计数器清零
PWME_PWME1=1; //2通道使能，2通道为输出通道
PWME_PWME3=1; // 3使能
}
void PWM_speed_Init_1(void)
{
PWME_PWME5=0; //禁止
PWME_PWME7=0;
PWMPRCLK=0x22; //64/4=16MHz /72/4=18MHZ
PWMCLK_PCLK5=0; //设A为其时钟源
PWMCLK_PCLK7=0; //B为其时钟源
PWMPOL_PPOL5=1; //上升沿翻转
PWMCAE_CAE5=0; //左对齐输出
PWMPOL_PPOL7=1; //上升沿翻转
PWMCAE_CAE7=0; //左对齐输出
PWMCTL_CON45=1; //0，1级连方式
PWMCTL_CON67=1; //2，3级连方式
PWMDTY45=0; //初始化占空比为0的波形
PWMDTY67=0;
PWMPER45=1600; //输出为10KHz的波
PWMPER67=1600;
PWMCNT45=0x00; //2通道计数器清0
PWMCNT67=0x00; //3通道计数器清零
PWME_PWME5=1; //2通道使能，2通道为输出通道
PWME_PWME7=1; // 3使能
}
//--------------------PWM输出函数------------------------------
void speed(float Left,float Right )
{
if(Left>0)
{
LL = 0; LH = Left+B;
}
if(Left<0)
{
LL = -Left+B;LH = 0;
}
if(Left==0)
{
LL = 0; LH = 0;
}
if(Right==0)
{
RL = 0;RH = 0;
}
if(Right>0)
{
RL = 0;RH = Right+A;
}
if(Right<0)
{
RL = -Right+A;RH = 0;
}
}
/***************************************************
******* 延时函数 ****
****************************************************/
void delay(uchar t)
{
uchar a;
for(a=0;a<t;a++);
}
void delaychang(uchar t)
{
uchar a,b;
for(a=0;a<t;a++)
for(b=0;b<1000;b++);
}
//延时函数
void delay_OLED(char mxs)
{
char yy,zz,bb;
for(yy=mxs;yy>0;yy--)
for(zz=253;zz>0;zz--)
for(bb=252;bb>0;bb--);
}
//----------------------------
//---------N阶滑动滤波程序-----
//-----------------------------
float filter_x()
{
float sum=0;
char count;
valuex_buf[i++]=ADx_value;
if(i==N) i=0;
for(count=0;count<N;count++)
sum=sum+valuex_buf[count];
return sum/N;
}
float filter_y()
{
float sum=0;
char count;
valuey_buf[i_y++]=ADy_value;
if(i_y==N) i_y=0;
for(count=0;count<N;count++)
sum=sum+valuey_buf[count];
return sum/N;
}
/****************************************************
******* I/O 初始化 ****
****************************************************/
void Init_IO(void)
{
DDRB=0xff;//PB6、PB5输出,,,0b01111110 = 0x7E，外部计数器清零、LED
DDRA=0x00;//PA口采集外部计数器CD4520的值 外16位计数器低八位
DDRM=0x00;//PM口采集高八位
DDRH=0x00;
}
//-------------------带参数显示函数----------------
void xianshi_LCD(int a,int b,int c)
{
switch(c)
{
case 0 CD_P8x16Str(a,b,"0");break;
case 1:LCD_P8x16Str(a,b,"1");break;
case 2:LCD_P8x16Str(a,b,"2");break;
case 3:LCD_P8x16Str(a,b,"3");break;
case 4:LCD_P8x16Str(a,b,"4");break;
case 5:LCD_P8x16Str(a,b,"5");break;
case 6:LCD_P8x16Str(a,b,"6");break;
case 7:LCD_P8x16Str(a,b,"7");break;
case 8:LCD_P8x16Str(a,b,"8");break;
case 9:LCD_P8x16Str(a,b,"9");break;
}
}
void dispiay()
{
int bai=0;
int shi=0;
int ge1=0;
int bai1=0;
int shi1=0;
int ge=0;
if(ADy_value>=1000)
{
xianshi_LCD(30,0,((int)ADy_value)/1000);
bai=((int)ADy_value)%1000;
xianshi_LCD(40,0,bai/100);
shi=bai%100;
xianshi_LCD(50,0,shi/10);
ge=shi%10;
xianshi_LCD(60,0,ge);
}
if(ADy_value>=100 && ADy_value<1000)
{
xianshi_LCD(30,0,0);
xianshi_LCD(40,0,ADy_value/100);
shi=ADy_value%100;
xianshi_LCD(50,0,shi/10);
ge=shi%10;
xianshi_LCD(60,0,ge);
}
if(ADy_value<100)
{
xianshi_LCD(30,0,0);
xianshi_LCD(40,0,0);
xianshi_LCD(50,0,ADy_value/10);
ge=ADy_value%10;
xianshi_LCD(60,0,ge);
}
if(ADx_value>=1000)
{
xianshi_LCD(30,2,((int)ADx_value)/1000);
bai1=((int)ADx_value)%1000;
xianshi_LCD(40,2,bai1/100);
shi1=bai1%100;
xianshi_LCD(50,2,shi1/10);
ge1=shi1%10;
xianshi_LCD(60,2,ge1);
}
if(ADx_value>=100 && ADx_value<1000)
{
xianshi_LCD(30,2,0);
xianshi_LCD(40,2,ADx_value/100);
shi1=ADx_value%100;
xianshi_LCD(50,2,shi1/10);
ge1=shi1%10;
xianshi_LCD(60,2,ge1);
}
if(ADx_value<100)
{
xianshi_LCD(30,2,0);
xianshi_LCD(40,2,0);
xianshi_LCD(50,2,ADx_value/10);
ge1=ADx_value%10;
xianshi_LCD(60,2,ge1);
}
}
//********闭环控制PID*********************************************
void AD_X_pid() //X方向PID调节
{
E_AD_X=AD_X-SET_AD_X;
pwm_x = P_x * E_AD_X + D_x * (E_AD_X - E1_AD_X);
if(pwm_x>=1550) pwm_x = 1550;
if(pwm_x<=-1550) pwm_x =-1550;
E1_AD_X = E_AD_X;
}
void AD_Y_pid() //Y方向PID调节
{
E_AD_Y=AD_Y-SET_AD_Y;
pwm_y = P_y * E_AD_Y + D_y * (E_AD_Y - E1_AD_Y);
if(pwm_y>=1550) pwm_y = 1550;
if(pwm_y<=-1550) pwm_y =-1550;
E1_AD_Y = E_AD_Y;
}
void main(void)
{
Busclock();
Init_IO();
int_ad();
initPIT();
get_ad();
PWM_speed_Init();
PWM_speed_Init_1();
LCD_Init();
EnableInterrupts;
for(;;)
{
delay_OLED(250);
dispiay();
LCD_P8x16Str(10,0,"X:");
LCD_P8x16Str(10,2,"Y:");
delay_OLED(250);
_FEED_COP(); /* feeds the dog */
} /* loop forever */
/*please make sure that you never leave main */
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)
{
NET++;
if(NET==1)
{
get_ad();
AD_Y=filter_y();
AD_X=filter_x();
AD_X_pid();
AD_Y_pid();
speed(pwm_y,pwm_x);
//eed(Pwm_x,Pwm_y);
NET=0;
}
PITTF_PTF0=1;//中断清零标志
}
#pragma CODE_SEG __NEAR_SEG DEFAULT
|
|
[复制链接]
楼主
发表于 2016-5-14 14:20:41
