标题:
STM32+mpu6050的iar环境下的demo代码
[打印本页]
作者:
Mr_Rice
时间:
2019-1-24 20:44
标题:
STM32+mpu6050的iar环境下的demo代码
mpu6050的iar环境下的demo
单片机源程序如下:
#include "sys.h"
int16_t pid_calc(PID *pid)
{
float out;
float ep, ei, ed;
pid->e_0 = pid->target - pid->feedback;
ep = pid->e_0 - pid->e_1;
ei = pid->e_0;
ed = pid->e_0 - 2*pid->e_1 + pid->e_2;
out = pid->Kp*ep + pid->Ki*ei + pid->Kd*ed;
out = range(out, -pid->limit, pid->limit);
pid->e_2 = pid->e_1;
pid->e_1 = pid->e_0;
return (int16_t)out;
}
s16 PIDCalc(LocationPID *pp,float NextPoint)
{
float dError,Error;
Error=pp->SetPoint-NextPoint;
pp->SumError+=Error;
dError=Error-pp->LastError;
pp->PreError=pp->LastError;
pp->LastError=Error;
return (s16)(pp->Kp*Error+pp->Ki*pp->SumError+pp->Kd*dError);
}
void pid_init(PID *pid)
{
pid->limit=1000;
pid->target=0.0;
pid->feedback=0.0;
pid->Kp=6.0;
pid->Ki=0.0;
pid->Kd=0.0;
pid->e_0=0.0;
pid->e_1=0.0;
pid->e_2=0.0;
}
void Location_PID_init(LocationPID *pp)
{
pp->Kp=6.0;
pp->Kd=0;
pp->Ki=0;
pp->SetPoint=0;
}
#if PID_Select
void Motor_Control(PID *pid,s16 *motor1,s16 *motor2,uint16_t throttle,float roll)
{
if(roll>60||roll<-60)
{
motorFlag=0;
throttle=1000;
TIM_SetCompare3(TIM4,throttle);
TIM_SetCompare4(TIM4,throttle);
}
else
{
s16 pid_put_value;
pid->feedback=roll;
pid_put_value=pid_calc(pid);
*motor1+=pid_put_value;
*motor2-=pid_put_value;
if(*motor1+throttle<1000)
{
*motor1=0;
}
if(*motor2+throttle<1000)
{
*motor2=0;
}
if(*motor1+throttle>2000)
{
*motor1=2000-throttle;
}
if(*motor2+throttle>2000)
{
*motor2=2000-throttle;
}
TIM_SetCompare3(TIM4,throttle+*motor1);
TIM_SetCompare4(TIM4,throttle+*motor2);
}
}
#else
void Motor_Control(LocationPID *pid,s16 *motor1,s16 *motor2,uint16_t throttle,float roll)
{
if(roll>60||roll<-60)
{
motorFlag=0;
throttle=1000;
TIM_SetCompare3(TIM4,throttle);
TIM_SetCompare4(TIM4,throttle);
}
else
{
s16 pid_put_value;
pid_put_value=PIDCalc(pid,roll);
*motor1=pid_put_value;
*motor2=-pid_put_value;
if(*motor1+throttle<1000)
{
*motor1=0;
}
if(*motor2+throttle<1000)
{
*motor2=0;
}
if(*motor1+throttle>2000)
{
*motor1=2000-throttle;
}
if(*motor2+throttle>2000)
{
*motor2=2000-throttle;
}
TIM_SetCompare3(TIM4,throttle+*motor1);
TIM_SetCompare4(TIM4,throttle+*motor2);
}
}
#endif
void String_Process(StringStruct *str)
{
printf("2_%s\r\n",str->buff);
if(str->buff[0] == '_')
{
for (int i = 0; i < str->length;i++ )
{
if (str->buff[i] == '_')
{
str->buff[i] = '\0';
strArr.buff[strArr.length] = ++i;
strArr.length++;
}
}
for (int i = 0; i < strArr.length; i++)
{
char *singleData;
singleData = str->buff + strArr.buff[i];
receivedPid[i]=atof(singleData);
}
pid_update_flag=1;
}
else if(str->buff[0] == 'w')
{
char *word;
word=str->buff+1;
switch(*word)
{
case 'f':motorFlag=1;break;
case 'w':throttle++;break;
case 's':throttle--;break;
case 'd':throttle+=10;break;
case 'a':throttle-=10;break;
case 'z':throttle+=100;break;
case 'c':throttle-=100;break;
case 'g':LED1(OFF);break;
case 'k':LED1(ON);break;
case 'v':motorFlag=0;throttle=1000;TIM_SetCompare3(TIM4,throttle);TIM_SetCompare4(TIM4,throttle);break;
}
printf("2_throttle:%d\r\n",throttle);
}
str->length=0;
}
void UART2_Send_Str(unsigned char *s)
{
unsigned char i=0;
while(s[i]!='\0')
{
USART_SendData(USART2,s[i]);
while( USART_GetFlagStatus(USART2,USART_FLAG_TC)!= SET);
i++;
}
}
复制代码
所有资料51hei提供下载:
6050DEMO_DMA.7z
(2.69 MB, 下载次数: 19)
2019-1-25 00:36 上传
点击文件名下载附件
下载积分: 黑币 -5
欢迎光临 (http://www.51hei.com/bbs/)
Powered by Discuz! X3.1