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)
|