找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1614|回复: 0
打印 上一主题 下一主题
收起左侧

STM32+mpu6050的iar环境下的demo代码

[复制链接]
跳转到指定楼层
楼主
ID:472635 发表于 2019-1-24 20:44 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
mpu6050的iar环境下的demo

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

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

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

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

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

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

  163. void UART2_Send_Str(unsigned char *s)
  164. {
  165.   unsigned char i=0;  
  166.   
  167.   while(s[i]!='\0')  
  168.   {
  169.     USART_SendData(USART2,s[i]);        
  170.     while( USART_GetFlagStatus(USART2,USART_FLAG_TC)!= SET);  
  171.     i++;                 
  172.   }
  173.   
  174. }
复制代码

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


评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶 踩
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表