标题: STM32+mpu6050的iar环境下的demo代码 [打印本页]
作者: Mr_Rice    时间: 2019-1-24 20:44
标题: STM32+mpu6050的iar环境下的demo代码
mpu6050的iar环境下的demo

单片机源程序如下:
  1. #include "sys.h"

  3. int16_t pid_calc(PID *pid)  
  4. {  
  5.   float out;  
  6.   float ep, ei, ed;  
  7.   
  8.   pid->e_0 = pid->target - pid->feedback;  
  9.   ep = pid->e_0  - pid->e_1;  
  10.   ei = pid->e_0;  
  11.   ed = pid->e_0 - 2*pid->e_1 + pid->e_2;  
  12.   out = pid->Kp*ep + pid->Ki*ei + pid->Kd*ed;  
  13.   out = range(out, -pid->limit, pid->limit);  
  14.   pid->e_2 = pid->e_1;  
  15.   pid->e_1 = pid->e_0;  
  16.   return (int16_t)out;  
  17. }

  19. s16 PIDCalc(LocationPID *pp,float NextPoint)
  20. {
  21.   float dError,Error;
  22.   Error=pp->SetPoint-NextPoint;
  23.   pp->SumError+=Error;
  24.   dError=Error-pp->LastError;
  25.   pp->PreError=pp->LastError;
  26.   pp->LastError=Error;
  27.   return (s16)(pp->Kp*Error+pp->Ki*pp->SumError+pp->Kd*dError);
  28. }

  30. void pid_init(PID *pid)
  31. {
  32.   pid->limit=1000;
  33.   pid->target=0.0;
  34.   pid->feedback=0.0;
  35.   pid->Kp=6.0;
  36.   pid->Ki=0.0;
  37.   pid->Kd=0.0;
  38.   pid->e_0=0.0;
  39.   pid->e_1=0.0;
  40.   pid->e_2=0.0;
  41. }

  43. void Location_PID_init(LocationPID *pp)
  44. {
  45.   pp->Kp=6.0;
  46.   pp->Kd=0;
  47.   pp->Ki=0;
  48.   pp->SetPoint=0;
  49. }
  50. #if PID_Select
  51. void Motor_Control(PID *pid,s16 *motor1,s16 *motor2,uint16_t throttle,float roll)
  52. {
  53.   if(roll>60||roll<-60)
  54.   {
  55.     motorFlag=0;
  56.     throttle=1000;
  57.     TIM_SetCompare3(TIM4,throttle);
  58.     TIM_SetCompare4(TIM4,throttle);
  59.   }
  60.   else
  61.   {
  62.     s16 pid_put_value;
  63.     pid->feedback=roll;
  64.     pid_put_value=pid_calc(pid);
  65.     *motor1+=pid_put_value;
  66.     *motor2-=pid_put_value;
  67.     if(*motor1+throttle<1000)
  68.     {
  69.       *motor1=0;
  70.     }
  71.     if(*motor2+throttle<1000)
  72.     {
  73.       *motor2=0;
  74.     }
  75.     if(*motor1+throttle>2000)
  76.     {
  77.       *motor1=2000-throttle;
  78.     }
  79.     if(*motor2+throttle>2000)
  80.     {
  81.       *motor2=2000-throttle;
  82.     }
  83.     TIM_SetCompare3(TIM4,throttle+*motor1);
  84.     TIM_SetCompare4(TIM4,throttle+*motor2);
  85.   }
  86. }
  87. #else
  88. void Motor_Control(LocationPID *pid,s16 *motor1,s16 *motor2,uint16_t throttle,float roll)
  89. {
  90.   if(roll>60||roll<-60)
  91.   {
  92.     motorFlag=0;
  93.     throttle=1000;
  94.     TIM_SetCompare3(TIM4,throttle);
  95.     TIM_SetCompare4(TIM4,throttle);
  96.   }
  97.   else
  98.   {
  99.     s16 pid_put_value;
  100.     pid_put_value=PIDCalc(pid,roll);
  101.     *motor1=pid_put_value;
  102.     *motor2=-pid_put_value;
  103.     if(*motor1+throttle<1000)
  104.     {
  105.       *motor1=0;
  106.     }
  107.     if(*motor2+throttle<1000)
  108.     {
  109.       *motor2=0;
  110.     }
  111.     if(*motor1+throttle>2000)
  112.     {
  113.       *motor1=2000-throttle;
  114.     }
  115.     if(*motor2+throttle>2000)
  116.     {
  117.       *motor2=2000-throttle;
  118.     }
  119.     TIM_SetCompare3(TIM4,throttle+*motor1);
  120.     TIM_SetCompare4(TIM4,throttle+*motor2);
  121.   }
  122. }
  123. #endif

  125. void String_Process(StringStruct *str)
  126. {
  127.   printf("2_%s\r\n",str->buff);
  128.   if(str->buff[0] == '_')
  129.   {
  130.     for (int i = 0; i < str->length;i++ )
  131.     {
  132.       if (str->buff[i] == '_')
  133.       {
  134.         str->buff[i] = '\0';
  135.         strArr.buff[strArr.length] = ++i;
  136.         strArr.length++;
  137.       }
  138.     }
  139.     for (int i = 0; i < strArr.length; i++)
  140.     {
  141.       char *singleData;
  142.       singleData = str->buff + strArr.buff[i];
  143.       receivedPid[i]=atof(singleData);
  144.     }
  145.     pid_update_flag=1;
  146.   }
  147.   else if(str->buff[0] == 'w')
  148.   {
  149.     char *word;
  150.     word=str->buff+1;
  151.     switch(*word)
  152.     {
  153.     case 'f':motorFlag=1;break;
  154.     case 'w':throttle++;break;
  155.     case 's':throttle--;break;
  156.     case 'd':throttle+=10;break;
  157.     case 'a':throttle-=10;break;
  158.     case 'z':throttle+=100;break;
  159.     case 'c':throttle-=100;break;
  160.     case 'g':LED1(OFF);break;
  161.     case 'k':LED1(ON);break;
  162.     case 'v':motorFlag=0;throttle=1000;TIM_SetCompare3(TIM4,throttle);TIM_SetCompare4(TIM4,throttle);break;
  163.     }
  164.     printf("2_throttle:%d\r\n",throttle);
  165.   }
  166.   str->length=0;
  167. }

  169. void UART2_Send_Str(unsigned char *s)
  170. {
  171.   unsigned char i=0;  
  172.   
  173.   while(s[i]!='\0')  
  174.   {
  175.     USART_SendData(USART2,s[i]);        
  176.     while( USART_GetFlagStatus(USART2,USART_FLAG_TC)!= SET);  
  177.     i++;                 
  178.   }
  179.   
  180. }
复制代码

所有资料51hei提供下载:
6050DEMO_DMA.7z (2.69 MB, 下载次数: 19)






欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1