STM32+mpu6050的iar環境下的demo代碼

ID:472635 · 發表于 2019-1-24 20:44

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)

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

STM32+mpu6050的iar環境下的demo代碼

評分