中级会员
 
- 积分
- 220
- 金钱
- 220
- 注册时间
- 2016-9-13
- 在线时间
- 58 小时
|

楼主 |
发表于 2017-8-1 22:01:18
|
显示全部楼层
[mw_shl_code=c,true]#include "control.h"
volatile float a[3],w[3],angle[3],T=1642.8;
int temp=0,temp1=0,temp2=0,temp3=0,temp4=0;
float L,H=75.0,R=10,A,Omega=0.003824 ,B, UA=0;;
int PWM1,PWM2,PWM3,PWM4;
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=RESET)
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
}
// angle[0]=Read_DMP(1);
// angle[1]=Read_DMP(2);
Set_Motor_PWM(600,0,0,0);
// UpData_MPU6050();
// if(++temp>200)
// {
// //LED2=!LED2;
// temp=0;
// }
//
Mode1();
}
void Mode1(void)
{
static int t=0;
// int UA=0;
A=atan(R/H)*57.2958f;
B=atan(R/H)*57.2958f;
//UA=A*t;
// UA=atan(R/H)*57.2958f;
// UA=0.1;
t=t+5;
PWM1= PID_SetX(2,0,UA,0);
PWM1=200;
PWM3=-PWM1;
Set_Motor_PWM(PWM1,0,PWM3,0);
}
int PID_SetX(int KP,int KD,float ua,float Angle)
{
int PWM=0;
float Bias,Last_Bias,D_Bias;
Bias=ua-Angle;
D_Bias=Bias-Last_Bias;
PWM=KP*Bias+KD*D_Bias;
Last_Bias=Bias;
return PWM;
}
void Data_Deal(u8 x,u8 y,int DATA)
{ u8 wei[4];
int Temp;
Temp=DATA;
wei[3]=Temp/1000;
wei[2]=Temp%1000/100;
wei[1]=Temp%100/10;
wei[0]=Temp%10;
OLED_ShowNum(x,y,wei[3],1,16);
OLED_ShowNum(x+8,y,wei[2],1,16);
OLED_ShowNum(x+16,y,wei[1],1,16);
OLED_ShowNum(x+24,y,wei[0],1,16);
}
void Set_Motor_PWM(int M1,int M2,int M3,int M4)
{
TIM_SetCompare4(TIM3,M4);
TIM_SetCompare3(TIM3,M3);
TIM_SetCompare2(TIM3,M2);
TIM_SetCompare1(TIM3,M1);
}
void UpData_MPU6050(void)
{
unsigned char Temp[11];
if(sign)
{
memcpy(Temp,Re_buf,11);
sign=0;
if(Re_buf[0]==0x55) //检查帧头
{
switch(Re_buf[1])
{
case 0x51: //标识这个包是加速度包
a[0] = ((short)(Temp[3]<<8 | Temp[2]))/32768.0*16; //X轴加速度
a[1] = ((short)(Temp[5]<<8 | Temp[4]))/32768.0*16; //Y轴加速度
a[2] = ((short)(Temp[7]<<8 | Temp[6]))/32768.0*16; //Z轴加速度
T = ((short)(Temp[9]<<8 | Temp[8]))/340.0+36.25; //温度
break;
case 0x52: //标识这个包是角速度包
w[0] = ((short)(Temp[3]<<8| Temp[2]))/32768.0*2000; //X轴角速度
w[1] = ((short)(Temp[5]<<8| Temp[4]))/32768.0*2000; //Y轴角速度
w[2] = ((short)(Temp[7]<<8| Temp[6]))/32768.0*2000; //Z轴角速度
T = ((short)(Temp[9]<<8| Temp[8]))/340.0+36.25; //温度
break;
case 0x53: //标识这个包是角度包
angle[0] = ((short)(Temp[3]<<8| Temp[2]))/32768.0*180; //X轴滚转角(x 轴)
angle[1] = ((short)(Temp[5]<<8| Temp[4]))/32768.0*180; //Y轴俯仰角(y 轴)
angle[2] = ((short)(Temp[7]<<8| Temp[6]))/32768.0*180; //Z轴偏航角(z 轴)
T = ((short)(Temp[9]<<8| Temp[8]))/340.0+36.25; //温度
//printf("X轴角度:%.2f Y轴角度:%.2f Z轴角度:%.2f\r\n",angle[0],angle[1],angle[2]);
break;
default: break;
}
}
}
}
//void Driver_Motor(int PWM) //PWM为正是逆时针转
//{
// if(PWM<0) AIN2=1, AIN1=0;
// else AIN2=0, AIN1=1;
// TIM_SetCompare4(TIM3,myabs(PWM));
//}
[/mw_shl_code] |
|