要求平衡杆能在正负30°的范围内平衡。现在我只能让其在水平位置以下平衡

ID:111420 · 发表于 2016-3-31 17:23

要求平衡杆能在正负30°的范围内平衡。现在我只能让其在水平位置以下平衡，到了水平位置以上就不能平衡，会飞到上面去了。求大神指导，不胜感激！
附上程序：

#include <reg52.h>
#include <intrins.h>
#include <math.h>
#define _Nop() _nop_()
#define nop() _nop_()
#include<stdio.h>
#define uchar unsigned char
#define uint unsigned int
#define delayNOP(); {_nop_();_nop_();_nop_();_nop_();};
#define LCD_data P0
sbit LCD_RS=P2^3;
sbit LCD_RW=P2^4;
sbit LCD_EN=P2^5;
sbit LCD_PSB=P3^3;
unsigned int c,d,e,f,m,n,p;
uchar code dis1[]={"单轴螺旋桨悬浮系"};
uchar code dis2[]={"角度："};
uchar code dis3[]={"调整时间："};
uchar code dis4[]={"°"};
uchar code dis5[]={"秒"};
sbit lowspeed=P3^6; //K3
sbit autospeed=P3^4; //K1 ！！！之前的K4与LCD12864端口冲突了！！！
sbit pmw=P3^5;
sbit ADC0832_CLK = P1^4;
sbit ADC0832_DIO = P1^5;
sbit ADC0832_CS = P2^0;
unsigned int chuzhi0=0xb9b0,chuzhi1=0xf830,time,time1;
unsigned char ADC_flag,balance=0x9f;
unsigned char kaiguan=1;
unsigned char jiaodu[20];
unsigned char jiaodu1,jiaodu2,jiaodu3;
unsigned int angle; /*实测角度值*/
float setjiaodu=0;
float Pro=0.01,
I=0.0001,
De=0,
lasterror=0,
pre_error=0,
sumerror=0;
unsigned char ADC0832_Read(unsigned char ch){
unsigned char i,ADC_buff=0,temp=0;
ADC0832_CS = 1;
ADC0832_DIO = 1;
ADC0832_CLK = 0;
ADC0832_CS = 0;
nop();
ADC0832_CLK = 1;
nop();
ADC0832_CLK = 0;
ADC0832_DIO = 1;
nop();
ADC0832_CLK = 1;
nop();
ADC0832_CLK = 0;
if(ch==0)
ADC0832_DIO = 0;
else
ADC0832_DIO = 1;
ADC0832_CLK = 1;
nop();
ADC0832_CLK = 0;
nop();
ADC0832_DIO = 1;
nop();
ADC0832_CLK = 1;
nop();
for(i=0;i<8;i++){
nop();
ADC0832_CLK = 0;
nop();
nop();
ADC_buff=ADC_buff<<1;
if(ADC0832_DIO==1)
ADC_buff=ADC_buff+1;
ADC0832_CLK = 1;
}
for(i=0;i<8;i++){
temp = temp>>1;
if(ADC0832_DIO==1)
temp = temp | 0x80;
ADC0832_CLK = 1;
nop();
ADC0832_CLK = 0;
nop();
}
ADC0832_CS = 1;
ADC0832_CLK = 1;
if(temp == ADC_buff)
ADC_flag = 1;
else
ADC_flag = 0;
return ADC_buff;
}
//1MS为单位的延时程序
void xianshi2();
unsigned int set_to_dat(float a)
{
float b;
if(a>0)
{b=256/5*(2*sqrt(1-((3.1415*a/180)*(3.1415*a/180)))+2.5);}
else
{b=256/5*(2.5-2*sqrt(1-((3.1415*a/180)*(3.1415*a/180))));}
return b;
}
unsigned int ADC0832da_to_Angle(unsigned char da)
{
/*将输入的数据转换成角度值*/
float Volage;
float Angle;
int zhuan;
Volage =da*5.0/256;
if(0.5*Volage-1.25>=0){
Angle = (float)(asin(0.5*Volage-1.25))*180/3.1415;
zhuan=(float)((asin(0.5*Volage-0.25*5)*180/3.1415)*100);
}
else{
Angle = (float)(asin(1.25-0.5*Volage))*180/3.1415;
zhuan=(float)((asin(1.25-0.5*Volage)*180/3.1415)*100);
}
return zhuan;
}
void delay_1ms(uchar x)
{
uchar j;
while(x--){
for(j=0;j<125;j++)
{;}
}
}
void delay(unsigned int a);
///////////////////以下是LCD12864驱动程序//////////////////////////////
bit lcd_busy() //检查LCD忙状态
{
bit result;
LCD_RS = 0;
LCD_RW = 1;
LCD_EN = 1;
delayNOP();
result = (bit)(P0&0x80);
LCD_EN = 0;
return(result);
}
void lcd_wcmd(uchar cmd)//写指令数据到LCD
{
while(lcd_busy());
LCD_RS = 0;
LCD_RW = 0;
LCD_EN = 0;
_nop_();
_nop_();
P0 = cmd;
delayNOP();
LCD_EN = 1;
delayNOP();
LCD_EN = 0;
}
void lcd_wdat(uchar dat) //写显示数据到LCD
{
while(lcd_busy());
LCD_RS = 1;
LCD_RW = 0;
LCD_EN = 0;
P0 = dat;
delayNOP();
LCD_EN = 1;
delayNOP();
LCD_EN = 0;
}
void lcd_init() //LCD初始化设定
{
delay(15);
LCD_PSB = 1; //并口方式
delay(5);
lcd_wcmd(0x34); //扩充指令操作
delay(5);
lcd_wcmd(0x30); //基本指令操作
delay(5);
lcd_wcmd(0x0C); //显示开，关光标
delay(5);
lcd_wcmd(0x01); //清除LCD的显示内容
delay(5);
}
////////////////以上是LCD12864驱动程序////////////////////////////
void xianshi()
{
if(jiaodu2<128){
lcd_wcmd(0X93);
delay_1ms(15);
lcd_wcmd(0X93);
delay_1ms(15);
lcd_wcmd(0X93);
delay_1ms(15);
lcd_wdat(0x2B);
delay_1ms(5);
}else{
lcd_wcmd(0X93);
delay_1ms(5);
lcd_wdat(0x2D);
delay_1ms(5);}
lcd_wcmd(0x94);
lcd_wdat(c+0x30);
delay_1ms(20);
lcd_wdat(d+0x30);
delay_1ms(20);
lcd_wdat(0x2E);
delay_1ms(5);
lcd_wdat(e+0x30);
delay_1ms(20);
lcd_wdat(f+0x30);
delay_1ms(20);
}
void xianshi2()
{
lcd_wcmd(0X8D);
lcd_wdat(m+0x30);
delay_1ms(5);
lcd_wdat(n+0x30);
delay_1ms(5);
lcd_wdat(0x2E);
delay_1ms(5);
lcd_wdat(p+0x30);
delay_1ms(5);
}
void pid();
void main(){
uchar i;
lcd_init(); /*LCD初始化*/
lcd_wcmd(0X80); //设置显示位置为第一行的第1个字符
for(i=0;i<16;i++){lcd_wdat(dis1[i]);} //显示字符
lcd_wcmd(0X90); //设置显示位置为第二行的第2个字符
for(i=0;i<6;i++){lcd_wdat(dis2[i]);}
lcd_wcmd(0X88); //设置显示位置为第三行的第3个字符
for(i=0;i<10;i++){lcd_wdat(dis3[i]);}
lcd_wcmd(0X97); //设置显示位置为第四行的第3个字符
for(i=0;i<2;i++){lcd_wdat(dis4[i]);}
lcd_wcmd(0X8F); //设置显示位置为第五行的第3个字符
for(i=0;i<2;i++){lcd_wdat(dis5[i]);}
IT0=0;
EX0=1;
IT1=0;
EX1=1;
TMOD=0x11;
TH0=chuzhi1>>8;
TL0=chuzhi1;
ET0=1;
EA=1;
TR0=1;
while(1){
if(lowspeed==0){ //初始化
delay(20);
if(lowspeed==0){
chuzhi0=0xb5c8;
chuzhi1=0xfc18;
}
}
if(autospeed==0){
delay(20);
if(autospeed==0){
kaiguan=1;
balance=0x90;
//balance=(char)set_to_dat(setjiaodu);
pid();
}
}
}
}
void shezhi() interrupt 1{ //调节占空比
if(pmw==1){
TH0=chuzhi0>>8;
TL0=chuzhi0;
pmw=0;
}
else{
TH0=chuzhi1>>8;
TL0=chuzhi1;
pmw=1;
}
TF0=0;
}
void shezhi1() interrupt 0{
chuzhi0=0xb5c8;
chuzhi1=0xfc18;
kaiguan=0;
}
void shezhi2() interrupt 3{
time++;
TH1=0X3C;
TL1=0XB0;
TF1=0;
}
/*void shezhi3() interrupt 2{
//balance=0x9c;
} */
void delay(unsigned int a){
unsigned b=50,c=130;
for(;a>0;a--){
for(;b>0;b--)
for(;c>0;c--);
}
}
void pid2();
void pid(){
unsigned char chazhi,chazhi1,chazhi2,button,change,j=3,i;
while(kaiguan==1){
for(i=0;i<20;i++){
jiaodu[i]=ADC0832_Read(0);
delay(200);
}
jiaodu3=jiaodu[0];
jiaodu1=(jiaodu[0]+jiaodu[1]+jiaodu[2]+jiaodu[3]+jiaodu[4]+jiaodu[5]+jiaodu[6]+jiaodu[7]+jiaodu[8]+jiaodu[9])/10;
jiaodu2=(jiaodu[10]+jiaodu[11]+jiaodu[12]+jiaodu[13]+jiaodu[14]+jiaodu[15]+jiaodu[16]+jiaodu[17]+jiaodu[18]+jiaodu[19])/10;
angle=ADC0832da_to_Angle(jiaodu2); //将采集值转换为角度值
chazhi=abs(jiaodu2-balance);
chazhi1=abs(jiaodu1-balance);
chazhi2=abs(jiaodu3-balance);
change=chazhi/8+1;
if((chazhi>=20)&&(button==0)){
button=1;
TH1=0x3c;
TL1=0xb0;
ET1=1;
TR1=1;
}
if((chazhi<=3)&&(button==1)&&(chazhi1<=3)&&(chazhi2<=3)){
button=0;
ET1=0;
TR1=0;
time1=(time*10)/20;
/*此处添加显示平衡时间的子函数，平衡时间为已经设好的变量“time”*/
m=time1/100;
n=time1%100/10;
p=time1%10;
xianshi2();
// time=0;
pid2();
}
c=angle/1000;
d=angle%1000/100;
e=angle%1000%100/10;
f=angle%10;
j--;
if(j==0){
xianshi();
j=3;
} /*此处添加显示当前角度的子程序，角度值为已经设好的变量“jiaodu2”*/
if(jiaodu2=jiaodu1){
if((jiaodu2>balance)&&(chuzhi0<0xb9b0)&&(chazhi>=2)){
chuzhi0=chuzhi0+change; //改变占空比
chuzhi1=chuzhi1-change;
}
if((jiaodu2<balance)&&(chuzhi0>0xb5c8)&&(chazhi>=2)){
chuzhi0=chuzhi0-change;
chuzhi1=chuzhi1+change;
}
}
if(jiaodu2>jiaodu1){
if((jiaodu2>=balance)&&(chuzhi0<0xb9b0)&&(chazhi>=2)){
chuzhi0=chuzhi0+change;
chuzhi1=chuzhi1-change;
}
}
if(jiaodu2<jiaodu1){
if((jiaodu2<=balance)&&(chuzhi0>0xb5c8)&&(chazhi>=2)){
chuzhi0=chuzhi0-change;
chuzhi1=chuzhi1+change;
}
}
}
return;
}
void pid2(){
float derror,error,change;
unsigned char jiaodu0,button,j;
while(1)
{
jiaodu0=ADC0832_Read(0);
c=angle/1000;
d=angle%1000/100;
e=angle%1000%100/10;
f=angle%10;
j--;
xianshi();
error=(float)abs(balance-jiaodu0);
sumerror+=error;
derror=lasterror-pre_error;
pre_error=lasterror;
lasterror=error;
change=Pro*error+I*sumerror+De*derror;
// change=error/10+1;
if(change>500)
{
change=500;
}
if((error>=20)&&(button==0)){
button=1;
TH1=0x3c;
TL1=0xb0;
ET1=1;
TR1=1;
}
if((jiaodu>balance)&&(chuzhi0<0xb9b0)&&(error>=2)){
chuzhi0=chuzhi0-(int)change;
chuzhi1=chuzhi1+(int)change;
}
if((jiaodu<balance)&&(chuzhi0>0xb5c8)&&(error>=2)){
chuzhi0=chuzhi0+(int)change;
chuzhi1=chuzhi1-(int)change;
}
if(error<2){
button=0;
ET1=0;
TR1=0;
time1=(time*10)/20;
/*此处添加显示平衡时间的子函数，平衡时间为已经设好的变量“time”*/
m=time1/100;
n=time1%100/10;
p=time1%10;
xianshi2();
time=0;
}
}
return;
}