设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › 基于stm32f103rct6的PID算法控制四驱车程序—基于HAL库 ...
返回列表 发新帖
查看: 3494|回复: 3
收起左侧

基于stm32f103rct6的PID算法控制四驱车程序—基于HAL库的

[复制链接]
ID:507807 发表于 2019-7-30 20:41 | 显示全部楼层 |阅读模式
STM32F103RCT6PID算法控制四个电机，基于HAL，希望可以给各位一些帮助，仅供参考，请勿他用

单片机源程序如下:
  1. /* USER CODE END Header */

  3. /* Includes ------------------------------------------------------------------*/
  4. #include "main.h"
  5. #include "dma.h"
  6. #include "tim.h"
  7. #include "usart.h"
  8. #include "gpio.h"

  10. /* Private includes ----------------------------------------------------------*/
  11. /* USER CODE BEGIN Includes */
  12. #include "string.h"
  13. #include "Motr_conter.h"
  14. #include "pid.h"
  15. /* USER CODE END Includes */

  17. /* Private typedef -----------------------------------------------------------*/
  18. /* USER CODE BEGIN PTD */

  20. /* USER CODE END PTD */

  22. /* Private define ------------------------------------------------------------*/
  23. /* USER CODE BEGIN PD */

  25. /* USER CODE END PD */

  27. /* Private macro -------------------------------------------------------------*/
  28. /* USER CODE BEGIN PM */

  30. /* USER CODE END PM */

  32. /* Private variables ---------------------------------------------------------*/

  34. /* USER CODE BEGIN PV */

  36. /* USER CODE END PV */

  38. /* Private function prototypes -----------------------------------------------*/
  39. void SystemClock_Config(void);
  40. /* USER CODE BEGIN PFP */

  42. /* USER CODE END PFP */

  44. /* Private user code ---------------------------------------------------------*/
  45. /* USER CODE BEGIN 0 */
  46.                                                
  47. uint8_t Left_Front_or_Right_rear;                        //用于保存电机行驶方向的变量
  48. uint8_t RX_buff[100],Start_Flag=0;                //接收缓冲区，和电机运行开始的标志位
  49. int16_t pwm_duth_A=0,pwm_duth_B=0,pwm_duth_C=0,pwm_duth_D=0;//四个电机的pwm变量
  50. uint16_t save_Ecoder_A=0,save_Ecoder_B=0,save_Ecoder_C=0,save_Ecoder_D=0;        //保存四个电机的编码值

  52. extern PID sPID;                        //PID结构体变量，用于改变目标值
  53. /* USER CODE END 0 */

  55. /**
  56.   * @brief  The application entry point.
  57.   * @retval int
  58.   */
  59. int main(void)
  60. {
  61.   /* USER CODE BEGIN 1 */
  62. uint8_t TX_buff[500]={"今天是个好日子，天气有点凉快！！\n"};

  64.   /* USER CODE END 1 */
  65.   

  67.   /* MCU Configuration--------------------------------------------------------*/

  69.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  70.   HAL_Init();

  72.   /* USER CODE BEGIN Init */

  74.   /* USER CODE END Init */

  76.   /* Configure the system clock */
  77.   SystemClock_Config();

  79.   /* USER CODE BEGIN SysInit */

  81.   /* USER CODE END SysInit */

  83.   /* Initialize all configured peripherals */
  84.   MX_GPIO_Init();                                                //GPIO的初始化
  85.   MX_DMA_Init();                                                //DMA的初始化
  86.   MX_TIM2_Init();                                                //定时器2的初始化
  87.   MX_USART1_UART_Init();                //串口1的初始化
  88.   MX_TIM1_Init();                                                //定时器1的初始化
  89.   MX_TIM6_Init();                                                //定时器6的初始化
  90. MX_USART2_UART_Init();                        //串口2的初始化
  91.   MX_TIM3_Init();                                                //定时器3的初始化
  92.   MX_TIM4_Init();                                                //定时器4的初始化
  93.   MX_TIM5_Init();                                                //定时器5的初始化
  94.   MX_TIM8_Init();                                                //定时器8的初始化
  95.   /* USER CODE BEGIN 2 */
  96.         Motor_Stop;                                                                //电机初始化状态位停止的
  97.         HAL_TIM_Base_Start_IT(&htim6);                                                                                                                                        //启动定时器
  98.         HAL_UART_Transmit(&huart2,TX_buff,strlen((char*)TX_buff),1000);        //串口2发送一串数据，表示串口2工作正常
  99.         HAL_UART_Transmit(&huart1,TX_buff,strlen((char*)TX_buff),100);        //串口发送数据测试        ，表示串口1工作正常
  100.         printf("电机编码器测试，测试通过\n");                                                                                                                //发送实验测试名称
  101.         IncPIDInit();                                                                                                                                                                                                                //pPID参数初始化
  102.         HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);                                                                //开始电机D编码器接口计数
  103.         HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);                                                                //开启电机C的编码器接口计数
  104.         HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);                                                                //开启电机B的编码器接口计数
  105.         HAL_TIM_Encoder_Start(&htim5,TIM_CHANNEL_ALL);                                                                //开启电机C的编码器接口计数
  106.         HAL_UART_Receive_DMA(&huart2,RX_buff,5);                                                                                        //开启串口2的DMA接收数据
  107.   /* USER CODE END 2 */

  109.   /* Infinite loop */
  110.   /* USER CODE BEGIN WHILE */
  111.   while (1)
  112.   {
  113.     /* USER CODE END WHILE */

  115.     /* USER CODE BEGIN 3 */
  116.                 if(RX_buff[0]==0x31)
  117.                 {
  118.                         if(RX_buff[1]==0x31&&RX_buff[2]==0x31&&RX_buff[3]==0x31)        Derection_Control(1,3,1,3);                                //左前行驶
  119.                 }
  120.                 if(RX_buff[0]==0x32)
  121.                 {
  122.                         if(RX_buff[1]==0x32&&RX_buff[2]==0x32&&RX_buff[3]==0x32)        Derection_Control(1,1,1,1);                                //向前行驶
  123.                 }
  124.                 if(RX_buff[0]==0x33)
  125.                 {
  126.                         if(RX_buff[1]==0x33&&RX_buff[2]==0x33&&RX_buff[3]==0x33)        Derection_Control(3,1,3,1);                                //右前行驶
  127.                 }
  128.                 if(RX_buff[0]==0x34)
  129.                 {
  130.                         if(RX_buff[1]==0x34&&RX_buff[2]==0x34&&RX_buff[3]==0x34)        Derection_Control(1,2,1,2);                                //向左平行行驶
  131.                 }
  132.                 if(RX_buff[0]==0x35)
  133.                 {
  134.                         if(RX_buff[1]==0x35&&RX_buff[2]==0x35&&RX_buff[3]==0x35)        Derection_Control(1,2,2,1);                                //顺时针旋转
  135.                 }
  136.                 if(RX_buff[0]==0x36)
  137.                 {
  138.                         if(RX_buff[1]==0x36&&RX_buff[2]==0x36&&RX_buff[3]==0x36)        Derection_Control(2,1,2,1);                                //向右平行行驶
  139.                 }
  140.                 if(RX_buff[0]==0x37)
  141.                 {
  142.                         if(RX_buff[1]==0x37&&RX_buff[2]==0x37&&RX_buff[3]==0x37)        Derection_Control(3,2,3,2);                                //左后行驶
  143.                 }
  144.                 if(RX_buff[0]==0x38)
  145.                 {
  146.                         if(RX_buff[1]==0x38&&RX_buff[2]==0x38&&RX_buff[3]==0x38)        Derection_Control(2,2,2,2);                                //向后行驶
  147.                 }
  148.                 if(RX_buff[0]==0x39)
  149.                 {
  150.                         if(RX_buff[1]==0x39&&RX_buff[2]==0x39&&RX_buff[3]==0x39)        Derection_Control(2,3,2,3);                                //右后行驶
  151.                 }
  152.                 if(RX_buff[0]==0x30)
  153.                 {
  154.                         if(RX_buff[1]==0x30&&RX_buff[2]==0x30&&RX_buff[3]==0x30)        Derection_Control(2,1,1,2);                                //逆时针旋转
  155.                 }
  156.                 if(RX_buff[0]==0x2E)
  157.                 {
  158.                         if(RX_buff[1]==0x31&&RX_buff[2]==0x32&&RX_buff[3]==0x33)        {Derection_Control(3,3,3,3);        }                //停止
  159.                 }
  160.                 if(RX_buff[0]==0x2B)
  161.                 {
  162.                         sPID.SetPoint_A=sPID.SetPoint_B=sPID.SetPoint_C=sPID.SetPoint_D=(RX_buff[1]-0x30)*1000+(RX_buff[2]-0x30)*100+(RX_buff[3]-0x30)*10+(RX_buff[4]-0x30);                //设置行驶的速度
  163.                 }                       
  164.   }
  165.   /* USER CODE END 3 */
  166. }

  168. /**
  169.   * @brief System Clock Configuration
  170.   * @retval None
  171.   */
  172. void SystemClock_Config(void)
  173. {
  174.   RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  175.   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  177.   /** Initializes the CPU, AHB and APB busses clocks
  178.   */
  179.   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  180.   RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  181.   RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  182.   RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  183.   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  184.   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  185.   RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  186.   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  187.   {
  188.     Error_Handler();
  189.   }
  190.   /** Initializes the CPU, AHB and APB busses clocks
  191.   */
  192.   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
  193.                               |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  194.   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  195.   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  196.   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  197.   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  199.   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  200.   {
  201.     Error_Handler();
  202.   }
  203.   /** Enables the Clock Security System
  204.   */
  205.   HAL_RCC_EnableCSS();
  206. }

  208. /* USER CODE BEGIN 4 */
  209.         uint8_t direction_A,direction_B,direction_C,direction_D;                                        //保存四个电机方向的变量
  210.         uint32_t Code_val_A=0,Code_val_B=0,Code_val_C=0,Code_val_D=0;                        //保存四个编码器计数器的的值
  211.         uint16_t last_Code_val_A=0,last_Code_val_B=0,last_Code_val_C=0,last_Code_val_D=0;//保存上一秒计数器的值，与下一秒的相比较到底计数了多少次
  212. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  213. {                                                                        
  214.        
  215.         if(htim->Instance==TIM6)                                                                                                        //判断是不是定时器6达到更新事件
  216.         {
  217.                 if(Start_Flag==1)                                                                                                                                //判断电机的行驶标志位
  218.                 {
  219.                 direction_A=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim5);//获取电机A转动的方向
  220.                 direction_B=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim4);//获取电机B 的旋转方向
  221.                 direction_C=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim3);//获取电机C 的旋转方向
  222.                 direction_D=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim2);//获取电机D的旋转方向
  223.                 Code_val_A=__HAL_TIM_GET_COUNTER(&htim5);                                                //读取电机A的计数器的值
  224.                 Code_val_B=__HAL_TIM_GET_COUNTER(&htim4);                                                //读取电机B的计数器的值
  225.                 Code_val_C=__HAL_TIM_GET_COUNTER(&htim3);                                                //读取电机C的计数器的值
  226.                 Code_val_D=__HAL_TIM_GET_COUNTER(&htim2);                                                //读取电机D的计数器的值
  227.                 if(direction_A==0)  save_Ecoder_A=(Code_val_A>=last_Code_val_A?Code_val_A-last_Code_val_A:Code_val_A+65535-last_Code_val_A);                                                                                                 //方向为正定时器向上计数
  228.                 if(direction_A==1)        save_Ecoder_A=(Code_val_A>last_Code_val_A?last_Code_val_A+65535-Code_val_A:last_Code_val_A-Code_val_A);                                                                                                        //方向为反转代表计数器是向下计数                       
  229.                 if(direction_B==0)  save_Ecoder_B=(Code_val_B>=last_Code_val_B?Code_val_B-last_Code_val_B:Code_val_B+65535-last_Code_val_B);                                                                                                 //方向为正定时器向上计数
  230.                 if(direction_B==1)        save_Ecoder_B=(Code_val_B>last_Code_val_B?last_Code_val_B+65535-Code_val_B:last_Code_val_B-Code_val_B);                                                                                                        //方向为反转代表计数器是向下计数
  231.                 if(direction_C==0)  save_Ecoder_C=(Code_val_C>=last_Code_val_C?Code_val_C-last_Code_val_C:Code_val_C+65535-last_Code_val_C);                                                                                                 //方向为正定时器向上计数
  232.                 if(direction_C==1)        save_Ecoder_C=(Code_val_C>last_Code_val_C?last_Code_val_C+65535-Code_val_C:last_Code_val_C-Code_val_C);                                                                                                        //方向为反转代表计数器是向下计数
  233.                 if(direction_D==0)  save_Ecoder_D=(Code_val_D>=last_Code_val_D?Code_val_D-last_Code_val_D:Code_val_D+65535-last_Code_val_D);                                                                                                 //方向为正定时器向上计数
  234.                 if(direction_D==1)        save_Ecoder_D=(Code_val_D>last_Code_val_D?last_Code_val_D+65535-Code_val_D:last_Code_val_D-Code_val_D);                                                                                                        //方向为反转代表计数器是向下计数
  235.                 if(Left_Front_or_Right_rear==0)                                                        //如果小车是按照前后左右的某一个方向行驶，将ABCD四个电机进行PID控制
  236.                 {
  237.                 pwm_duth_A+=(IncPIDCalc_C(save_Ecoder_A));        //电机A的PID算法值返回
  238.                 pwm_duth_B+=(IncPIDCalc_B(save_Ecoder_B));        //电机B的PID算法值返回
  239.                 pwm_duth_C+=(IncPIDCalc_C(save_Ecoder_C));        //电机C的PID算法值返回
  240.                 pwm_duth_D+=(IncPIDCalc_D(save_Ecoder_D));        //电机D的PID算法值返回
  241.                 }
  242.                 if(Left_Front_or_Right_rear==1)                                                                        //电机向左前或者右后行驶
  243.                 {
  244.                         pwm_duth_A+=(IncPIDCalc_C(save_Ecoder_A));                //就只进行电机AC的PID算法，不进行电机BD的算法
  245.                         pwm_duth_C+=(IncPIDCalc_C(save_Ecoder_C));                //就只进行电机AC的PID算法，不进行电机BD的算法
  246.                 }                       
  247.                 if(Left_Front_or_Right_rear==2)                                                                        //电机向右前或者左后行驶
  248.                 {
  249.                         pwm_duth_B+=(IncPIDCalc_B(save_Ecoder_B));                //就只进行电机BD的PID算法，不进行电机AC的PID算法
  250.                         pwm_duth_D+=(IncPIDCalc_D(save_Ecoder_D));                //就只进行电机BD的PID算法，不进行电机AC的PID算法
  251.                 }
  252.                 if(pwm_duth_A<0) pwm_duth_A=-pwm_duth_A;                                //处理电机A，当PWM值为负时变为正
  253.                 if(pwm_duth_B<0) pwm_duth_B=-pwm_duth_B;                                //处理电机B，当PWM值为负时变为正
  254.                 if(pwm_duth_C<0) pwm_duth_C=-pwm_duth_C;                                //处理电机C，当PWM值为负时变为正       
  255.                 if(pwm_duth_D<0) pwm_duth_D=-pwm_duth_D;                                //处理电机D，当PWM值为负时变为正
  256.                 if(pwm_duth_A>=7199)        pwm_duth_A=7200*4/6;                        //限制电机A的速度为最大占空比的百分之六十六
  257.                 if(pwm_duth_B>=7199)        pwm_duth_B=7200*4/6;                        //限制电机B的速度为最大占空比的百分之六十六                               
  258.                 if(pwm_duth_C>=7199)        pwm_duth_C=7200*4/6;                        //限制电机C的速度为最大占空比的百分之六十六
  259.                 if(pwm_duth_D>=7199)        pwm_duth_D=7200*4/6;                        //限制电机D的速度为最大占空比的百分之六十六
  260.                 Set_compare(motor_A,pwm_duth_A);                                                                //将电机A经过PID算法之后得到的PWM赋给电机，让电机真正的按照目标运行起来
  261.                 Set_compare(motor_B,pwm_duth_B);                                                                //将电机B经过PID算法之后得到的PWM赋给电机，让电机真正的按照目标运行起来
  262.                 Set_compare(motor_C,pwm_duth_C);                                                                //将电机C经过PID算法之后得到的PWM赋给电机，让电机真正的按照目标运行起来
  263.                 Set_compare(motor_D,pwm_duth_D);                                                                //将电机D经过PID算法之后得到的PWM赋给电机，让电机真正的按照目标运行起来
  264.                 last_Code_val_A=Code_val_A;                                                                                        //保存电机A的编码值，便于进行脉冲值的计算
  265.                 last_Code_val_B=Code_val_B;                                                                                        //保存电机B的编码值，便于进行脉冲值的计算
  266.                 last_Code_val_C=Code_val_C;                                                                                        //保存电机C的编码值，便于进行脉冲值的计算
  267.                 last_Code_val_D=Code_val_D;                                                                                        //保存电机D的编码值，便于进行脉冲值的计算

  269.                 }
  270.         }
  271. }




  276. void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
  277. {
  278.         if(huart->Instance==USART1)                                                                                                                        //判断是不是串口1接收完成
  279.         {
  280. //                        HAL_UART_Receive_IT(&huart1,RX_buff,8);
  281.         }
  282.         if(huart->Instance==USART2)                                                                                                                        //判断是否是串口2接收完成
  283.         {

  285.                 HAL_UART_Transmit_DMA(&huart2,RX_buff,5);                                                //DMA发送数据
  286.         }
  287. }
  288. void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)                        //发送完成回调函数
  289. {
  290.         if(huart->Instance==USART2)
  291.         {
  292.                 HAL_UART_Receive_DMA(&huart2,RX_buff,5);                                                                //使能DMA接收数据
  293.         }
  294. }
  295. /* USER CODE END 4 */

  297. /**
  298.   * @brief  This function is executed in case of error occurrence.
  299.   * @retval None
  300.   */
  301. void Error_Handler(void)
  302. {
  303.   /* USER CODE BEGIN Error_Handler_Debug */
  304.   /* User can add his own implementation to report the HAL error return state */

  306.   /* USER CODE END Error_Handler_Debug */
  307. }

  309. #ifdef  USE_FULL_ASSERT
  310. /**
  311.   * @brief  Reports the name of the source file and the source line number
  312.   *         where the assert_param error has occurred.
  313.   * @param  file: pointer to the source file name
  314.   * @param  line: assert_param error line source number
  315.   * @retval None
  316.   */
  317. void assert_failed(uint8_t *file, uint32_t line)
  318. {
  319.   /* USER CODE BEGIN 6 */
  320.   /* User can add his own implementation to report the file name and line number,
  321.      tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  322.   /* USER CODE END 6 */
  323. }
  324. #endif /* USE_FULL_ASSERT */

  326. /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
复制代码

所有资料51hei提供下载:
蓝牙+PID控制的小车.7z (264.82 KB, 下载次数: 90)
回复

使用道具 举报

ID:1 发表于 2019-7-30 23:28 | 显示全部楼层
本帖需要重新编辑补全电路原理图,源码,详细说明与图片即可获得100+黑币(帖子下方有编辑按钮)
回复

使用道具 举报

ID:584531 发表于 2019-8-17 14:45 | 显示全部楼层
膜拜大佬
回复

使用道具 举报

ID:80689 发表于 2019-9-28 20:37 | 显示全部楼层
资料注释很详细！赞！！
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

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