• RT-Thread实战笔记-小白一看就会的平衡车教程（附源码）

    摘要

    小伙伴们，停更很久的RT-Thread实战笔记来啦，本章节教大家如何打造一个属于自己的平衡车，废话不多说，来吧，淦！！！

    主要RT-Thread内容

    • RT-Thread

    • PID

    • PWM

    • MPU6050

    • 编码器

    • 定时器

    • 线程

    模组介绍

    利用手中已经积灰多年的小模块，废物利用，打造一个专属的平衡车

    电机驱动模块

    某宝买的L298N电机驱动模组

    或者TB6612，关于这两个模组的介绍就不多说了，大家可以自行百度下哈

    陀螺仪

    陀螺仪选用的是用的比较多的[MPU6050]，目前好像要停产了，价格也越来越贵

    电机

    电机采用的是带有编码器的直流减速电机，价格也略微贵一些

    电池


    主控

    RT-Thread ART-PI控制板

    亚克力板

    亚克力板也是自己设计的尺寸图分享给大家

    软件设计

    接线

    电机驱动接线：

    电机ART-PI
    PWMAPB0
    IN1H14
    IN2C7
    IN3G10
    In4I6
    PWMBPB1
    12V/
    5V5V
    GNDGND

    MPU6050接线

    MPU6050ART-PI
    3.3V3.3V
    GNDGND
    SCLPH11
    SDAPH12

    左电机与电机驱动模组：

    电机ART-PI电机驱动模组
    电机+/OUT1
    编码器电源-GND
    编码器APA8
    编码器BPA9
    编码器电源+3.3V/5V
    电机-/OUT2

    右电机与电机驱动模组：

    电机ART-PI电机驱动模组
    电机+/OUT3
    编码器电源-GND
    编码器APA1
    编码器BPA15
    编码器电源+3.3V/5V
    电机-/OUT4

    软件代码

    代码很多，主要介绍下，具体的大家可以看源码，源码都是开源的哈

    软件包只用了按键和MPU6050的软件包，IIC用的是PH11和PH12

    • MPU6050驱动

    移植的是DMP驱动，也可以用rt-thread软件包里面配置，我是自己移植过来的，也非常的简单，写好接口就可以了

    1. /**
    2.     * @author:小飞哥玩嵌入式-小飞哥
    3.     * @TODO:mpu6050初始化
    4.     * @param NULL
    5.     * @return NULL
    6. */
    7. void mpu_measurement_init(void)
    8. {
    9.     i2c_bus = (struct mpu6xxx_device *) mpu6xxx_init(MPU6050_I2C_BUS_NAME, MPU6050_ADDR); //初始化MPU6050，测量单位为角速度，加速度    while(count++)
    10.  
    11.     rt_int8_t res = 1;
    12.     while (res)
    13.     {
    14.         res = mpu_dmp_init();
    15.         rt_kprintf("\r\nRES = %d\r\n",res);
    16.  
    17.         rt_thread_mdelay(500);
    18.         rt_kprintf("\r\nMPU6050 DMP init Error\r\n");
    19.     }
    20.     rt_kprintf("\r\nMPU6050 DMP init OK\r\n");
    21. }
    22. /**
    23.     * @author:小飞哥玩嵌入式-小飞哥
    24.     * @TODO:MPU写入多字节数据
    25.     * @param NULL
    26.     * @return NULL
    27. */
    28. rt_uint8_t MPU_Write_Len(rt_uint8_t addr,rt_uint8_t reg,rt_uint8_t len,rt_uint8_t *databuf)
    29. {
    30.     rt_int8_t res = 0;
    31. #ifdef RT_USING_I2C
    32.     struct rt_i2c_msg msgs;
    33.     rt_uint8_t buf[50] = {0};
    34. #endif
    35.     buf[0] = reg;
    36.  
    37.     for(int i = 0;i<len;i++)
    38.     {
    39.         buf[i+1]=databuf[i];
    40.     }
    41.  
    42.     if (i2c_bus->bus->type == RT_Device_Class_I2CBUS)
    43.     {
    44.         msgs.addr  = i2c_bus->i2c_addr;    /* slave address */
    45.         msgs.flags = RT_I2C_WR;        /* write flag */
    46.         msgs.buf   = buf;              /* Send data pointer */
    47.         msgs.len   = len+1;
    48.  
    49.         if (rt_i2c_transfer((struct rt_i2c_bus_device *)i2c_bus->bus, &msgs, 1) == 1)
    50.         {
    51.             res = RT_EOK;
    52.         }
    53.         else
    54.         {
    55.             res = -RT_ERROR;
    56.         }
    57.     }
    58. }
    59. /**
    60.     * @author:小飞哥玩嵌入式-小飞哥
    61.     * @TODO:mpu读取多字节数据
    62.     * @param NULL
    63.     * @return NULL
    64. */
    65. rt_uint8_t MPU_Read_Len(rt_uint8_t addr,rt_uint8_t reg,rt_uint8_t len,rt_uint8_t *buf)
    66. {
    67.     rt_int8_t res = 0;
    68. #ifdef RT_USING_I2C
    69.     struct rt_i2c_msg msgs[2];
    70. #endif
    71. #ifdef RT_USING_SPI
    72.     rt_uint8_t tmp;
    73. #endif
    74.     if (i2c_bus->bus->type == RT_Device_Class_I2CBUS)
    75.     {
    76.         msgs[0].addr  = i2c_bus->i2c_addr;    /* Slave address */
    77.         msgs[0].flags = RT_I2C_WR;        /* Write flag */
    78.         msgs[0].buf   = &reg;             /* Slave register address */
    79.         msgs[0].len   = 1;                /* Number of bytes sent */
    80.  
    81.         msgs[1].addr  = i2c_bus->i2c_addr;    /* Slave address */
    82.         msgs[1].flags = RT_I2C_RD;        /* Read flag */
    83.         msgs[1].buf   = buf;              /* Read data pointer */
    84.         msgs[1].len   = len;              /* Number of bytes read */
    85.  
    86.         if (rt_i2c_transfer((struct rt_i2c_bus_device *)i2c_bus->bus, msgs, 2) == 2)
    87.         {
    88.             res = RT_EOK;
    89.         }
    90.         else
    91.         {
    92.             res = -RT_ERROR;
    93.         }
    94.     }
    95.  
    96.     return res;
    97. }

    然后对接到inv_mpu.c里面的接口函数

    • 电机驱动

    1. /**
    2.     * @author:小飞哥玩嵌入式-小飞哥
    3.     * @TODO:小车电机控制初始化
    4.     * @param NULL
    5.     * @return NULL
    6. */
    7. void rt_balanceCar_pinInit(void)
    8. {
    9.     rt_pin_mode(motor_A1, PIN_MODE_OUTPUT );
    10.     rt_pin_mode(motor_A2, PIN_MODE_OUTPUT );
    11.     rt_pin_mode(motor_B1, PIN_MODE_OUTPUT );
    12.     rt_pin_mode(motor_B2, PIN_MODE_OUTPUT );
    13.  
    14.     rt_pin_write(motor_A1,PIN_LOW);
    15.     rt_pin_write(motor_A2,PIN_LOW);
    16.     rt_pin_write(motor_B1,PIN_LOW);
    17.     rt_pin_write(motor_B2,PIN_LOW);
    18. }
    19. /**
    20.     * @author:小飞哥玩嵌入式-小飞哥
    21.     * @TODO:小车左轮前进
    22.     * @param NULL
    23.     * @return NULL
    24. */
    25. void rt_balanceCar_LeftMotorforward(void)
    26. {
    27.     rt_pin_write(motor_B1,PIN_LOW);
    28.     rt_pin_write(motor_B2,PIN_HIGH);
    29. }
    30. /**
    31.     * @author:小飞哥玩嵌入式-小飞哥
    32.     * @TODO:小车左轮后退
    33.     * @param NULL
    34.     * @return NULL
    35. */
    36. void  rt_balanceCar_LeftMotorback(void)
    37. {
    38.   rt_pin_write(motor_B1,PIN_HIGH);
    39.   rt_pin_write(motor_B2,PIN_LOW);
    40. }
    41. /**
    42.     * @author:小飞哥玩嵌入式-小飞哥
    43.     * @TODO:小车右轮前进
    44.     * @param NULL
    45.     * @return NULL
    46. */
    47. void rt_balanceCar_RightMotorforward(void)
    48. {
    49.     rt_pin_write(motor_A1,PIN_LOW);
    50.     rt_pin_write(motor_A2,PIN_HIGH);
    51. }
    52. /**
    53.     * @author:小飞哥玩嵌入式-小飞哥
    54.     * @TODO:小车右轮后退
    55.     * @param NULL
    56.     * @return NULL
    57. */
    58. void  rt_balanceCar_RightMotorback(void)
    59. {   rt_pin_write(motor_A1,PIN_HIGH);
    60.     rt_pin_write(motor_A2,PIN_LOW);
    61. }
    62. /**
    63.     * @author:小飞哥玩嵌入式-小飞哥
    64.     * @TODO:小车整机前进
    65.     * @param NULL
    66.     * @return NULL
    67. */
    68. void rt_balanceCar_forward(void)
    69. {
    70.     rt_pin_write(motor_A1,PIN_HIGH);
    71.     rt_pin_write(motor_A2,PIN_LOW);
    72.     rt_pin_write(motor_B1,PIN_HIGH);
    73.     rt_pin_write(motor_B2,PIN_LOW);
    74. }
    75. /**
    76.     * @author:小飞哥玩嵌入式-小飞哥
    77.     * @TODO:小车整机后退
    78.     * @param NULL
    79.     * @return NULL
    80. */
    81. void rt_balanceCar_back(void)
    82. {
    83.     rt_pin_write(motor_A1,PIN_LOW);
    84.     rt_pin_write(motor_A2,PIN_HIGH);
    85.     rt_pin_write(motor_B1,PIN_LOW);
    86.     rt_pin_write(motor_B2,PIN_HIGH);
    87. }
    88. /**
    89.     * @author:小飞哥玩嵌入式-小飞哥
    90.     * @TODO:小车整机左转
    91.     * @param NULL
    92.     * @return NULL
    93. */
    94. void rt_balanceCar_turnLeft(void)
    95. {
    96.     rt_pin_write(motor_A1,PIN_LOW);
    97.     rt_pin_write(motor_A2,PIN_LOW);
    98.     rt_pin_write(motor_B1,PIN_HIGH);
    99.     rt_pin_write(motor_B2,PIN_LOW);
    100. }
    101. /**
    102.     * @author:小飞哥玩嵌入式-小飞哥
    103.     * @TODO:小车整机右转
    104.     * @param NULL
    105.     * @return NULL
    106. */
    107. void rt_balanceCar_turnRight(void)
    108. {
    109.     rt_pin_write(motor_A1,PIN_HIGH);
    110.     rt_pin_write(motor_A2,PIN_LOW);
    111.     rt_pin_write(motor_B1,PIN_LOW);
    112.     rt_pin_write(motor_B2,PIN_LOW);
    113. }
    114. /**
    115.     * @author:小飞哥玩嵌入式-小飞哥
    116.     * @TODO:小车电机停转
    117.     * @param NULL
    118.     * @return NULL
    119. */
    120. void rt_balanceCar_stop(void)
    121. {
    122.     rt_pin_write(motor_A1,PIN_LOW);
    123.     rt_pin_write(motor_A2,PIN_LOW);
    124.     rt_pin_write(motor_B1,PIN_LOW);
    125.     rt_pin_write(motor_B2,PIN_LOW);
    126. }
    127.

    • PWM控制

    1. /**
    2.     * @author:小飞哥玩嵌入式-小飞哥
    3.     * @TODO:pwm使能
    4.     * @param NULL
    5.     * @return NULL
    6. */
    7. void rt_balanceCar_pwmEnable(void)
    8. {
    9.     rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL3);
    10.     rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL4);
    11. }
    12. /**
    13.     * @author:小飞哥玩嵌入式-小飞哥
    14.     * @TODO:pwm失能
    15.     * @param NULL
    16.     * @return NULL
    17. */
    18. void rt_balanceCar_pwmDisable(void)
    19. {
    20.     rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL3);
    21.     rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL4);
    22. }
    23.  
    24. /**
    25.     * @author:小飞哥玩嵌入式-小飞哥
    26.     * @TODO:pwm输出限幅
    27.     * @param pwm1
    28.     * @param pwm2
    29.     * @return NULL
    30. */
    31. void rt_balanceCar_pwmlimit(rt_int32_t *pwm1,rt_int32_t *pwm2)
    32. {
    33.     if(*pwm1 >= PWM_UPPER_LIMIT)
    34.     {
    35.         *pwm1 = PWM_UPPER_LIMIT;
    36.     }
    37.     else if(*pwm1 <= PWM_LOWER_LIMIT)
    38.     {
    39.         *pwm1 = PWM_LOWER_LIMIT;
    40.     }
    41.  
    42.     if(*pwm2 >= PWM_UPPER_LIMIT)
    43.     {
    44.         *pwm2 = PWM_UPPER_LIMIT;
    45.     }
    46.     else if(*pwm2 <= PWM_LOWER_LIMIT)
    47.     {
    48.         *pwm2 = PWM_LOWER_LIMIT;
    49.     }
    50. }
    51. /**
    52.     * @author:小飞哥玩嵌入式-小飞哥
    53.     * @TODO:pwm设置
    54.     * @param channel1
    55.     * @param channel2
    56.     * @param L_speed
    57.     * @param R_speed
    58.     * @return NULL
    59. */
    60. void rt_balanceCar_pwmSet(rt_uint8_t channel1,rt_uint8_t channel2,rt_int32_t L_speed,rt_int32_t R_speed)
    61. {
    62.     //输出限幅
    63.     rt_balanceCar_pwmlimit(&L_speed,&R_speed);
    64.  
    65.     //pwm设置
    66.     rt_pwm_set(pwm_dev, channel1, PWM_PERIOD, _ABS(L_speed));
    67.     rt_pwm_set(pwm_dev, channel2, PWM_PERIOD, _ABS(R_speed));
    68. /*
    69.     rt_pwm_enable(pwm_dev, channel1);
    70.     rt_pwm_enable(pwm_dev, channel2);
    71. */
    72. }
    73. /**
    74.     * @author:小飞哥玩嵌入式-小飞哥
    75.     * @TODO:pwm初始化
    76.     * @param NULL
    77.     * @return NULL
    78. */
    79. rt_int8_t rt_balanceCar_pwmInit(void)
    80. {
    81.     pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
    82.     if (pwm_dev == RT_NULL)
    83.     {
    84.         rt_kprintf("\r\npwm sample run failed! can't find %s device!\r\n", PWM_DEV_NAME);
    85.         return RT_ERROR;
    86.     }
    87.     rt_kprintf("\r\npwm sample run success! find %s device!\r\n", PWM_DEV_NAME);
    88.  
    89.     //关闭PWM
    90.     //rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL3);
    91.     //rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL4);
    92.     //开启PWM
    93.     rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL3);
    94.     rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL4);
    95.     return RT_EOK;
    96. }

    • 编码器

    编码器驱动是把HAL库的驱动移植过来的，直接复制粘贴就可以了

    1. /* USER CODE END 0 */
    2.  
    3. TIM_HandleTypeDef htim1;
    4. TIM_HandleTypeDef htim2;
    5. /* TIM1 init function */
    6.  
    7. /**
    8.   * @brief TIM1 Initialization Function
    9.   * @param None
    10.   * @retval None
    11.   */
    12. static void MX_TIM1_Init(void)
    13. {
    14.  
    15.   /* USER CODE BEGIN TIM1_Init 0 */
    16.  
    17.   /* USER CODE END TIM1_Init 0 */
    18.  
    19.   TIM_Encoder_InitTypeDef sConfig = {0};
    20.   TIM_MasterConfigTypeDef sMasterConfig = {0};
    21.  
    22.   /* USER CODE BEGIN TIM1_Init 1 */
    23.  
    24.   /* USER CODE END TIM1_Init 1 */
    25.   htim1.Instance = TIM1;
    26.   htim1.Init.Prescaler = 0;
    27.   htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    28.   htim1.Init.Period = 65535;
    29.   htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    30.   htim1.Init.RepetitionCounter = 0;
    31.   htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    32.   sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
    33.   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
    34.   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
    35.   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
    36.   sConfig.IC1Filter = 0;
    37.   sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
    38.   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
    39.   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
    40.   sConfig.IC2Filter = 0;
    41.   if (HAL_TIM_Encoder_Init(&htim1, &sConfig) != HAL_OK)
    42.   {
    43.     Error_Handler();
    44.   }
    45.   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    46.   sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
    47.   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    48.   if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
    49.   {
    50.     Error_Handler();
    51.   }
    52.   /* USER CODE BEGIN TIM1_Init 2 */
    53.  
    54.   /* USER CODE END TIM1_Init 2 */
    55.  
    56. }
    57.  
    58. /**
    59.   * @brief TIM2 Initialization Function
    60.   * @param None
    61.   * @retval None
    62.   */
    63. static void MX_TIM2_Init(void)
    64. {
    65.  
    66.   /* USER CODE BEGIN TIM2_Init 0 */
    67.  
    68.   /* USER CODE END TIM2_Init 0 */
    69.  
    70.   TIM_Encoder_InitTypeDef sConfig = {0};
    71.   TIM_MasterConfigTypeDef sMasterConfig = {0};
    72.  
    73.   /* USER CODE BEGIN TIM2_Init 1 */
    74.  
    75.   /* USER CODE END TIM2_Init 1 */
    76.   htim2.Instance = TIM2;
    77.   htim2.Init.Prescaler = 0;
    78.   htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    79.   htim2.Init.Period = 4294967295;
    80.   htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    81.   htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    82.   sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
    83.   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
    84.   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
    85.   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
    86.   sConfig.IC1Filter = 0;
    87.   sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
    88.   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
    89.   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
    90.   sConfig.IC2Filter = 0;
    91.   if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK)
    92.   {
    93.     Error_Handler();
    94.   }
    95.   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    96.   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    97.   if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
    98.   {
    99.     Error_Handler();
    100.   }
    101.   /* USER CODE BEGIN TIM2_Init 2 */
    102.  
    103.   /* USER CODE END TIM2_Init 2 */
    104.  
    105. }
    106. /**
    107.     * @author:小飞哥玩嵌入式-小飞哥
    108.     * @TODO:清除编码器数值
    109.     * @param NULL
    110.     * @return NULL
    111. */
    112. void encoder_clearCounter(void)
    113. {
    114.     __HAL_TIM_SET_COUNTER(&htim1,0);
    115.     __HAL_TIM_SET_COUNTER(&htim2,0);
    116. }
    117. /**
    118.     * @author:小飞哥玩嵌入式-小飞哥
    119.     * @TODO:获取编码器数值
    120.     * @param out s_encoder_measure
    121.     * @return NULL
    122. */
    123. void encoder_getCounter(rt_int32_t *l_speed,rt_int32_t *r_speed)
    124. {
    125.     *l_speed = ((rt_int32_t)__HAL_TIM_GET_COUNTER(&htim1)-COUNTER_RESET);
    126.     *r_speed = (rt_int32_t)__HAL_TIM_GET_COUNTER(&htim2)-COUNTER_RESET;
    127.  
    128.     encoder_clearCounter();
    129. }
    130. /**
    131.     * @author:小飞哥玩嵌入式-小飞哥
    132.     * @TODO:编码器初始化
    133.     * @param NULL
    134.     * @return NULL
    135. */
    136. int hw_Encoder_init(void)
    137. {
    138.     MX_TIM1_Init();
    139.     MX_TIM2_Init();
    140.  
    141.     HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
    142.     HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);
    143.  
    144.     rt_kprintf("\r\ntim1,tim2 init ok\r\n");
    145. }

    • PID控制算法

    PID采用的是位置式PID，关于位置式PID，本章也不再具体介绍了,主要包括直立环、转向环、速度环三个控制环

    1. /**
    2.     * @author:小飞哥玩嵌入式-小飞哥
    3.     * @TODO:PID参数初始化
    4.     * @param NULL
    5.     * @param NULL
    6.     * @return NULL
    7.      */
    8. void pid_init(void)
    9. {
    10.     s_pid.kp_speed = -0.35;//速度环kp值
    11.     s_pid.kp_stand = -1600*0.6;//直立环kp值
    12.  
    13.  
    14.     s_pid.ki = s_pid.kp_speed/200;
    15.     s_pid.kd = 65*0.6;
    16.  
    17.     s_pid.kp_turn = 20;
    18.  
    19.     s_pid.limit = 800;
    20.     s_pid.err_current = 0;
    21.     s_pid.err_last = 0;
    22.     s_pid.err_sum = 0;
    23.  
    24.     s_pid.lowfilter_rate = 0.7;
    25.  
    26.     s_pid.mid_value = -1;
    27. }
    28.  
    29. /**
    30.     * @author:小飞哥玩嵌入式-小飞哥
    31.     * @TODO:小车直立环控制
    32.     * @param 当前角度
    33.     * @param 目标角度
    34.     * @param 真实角速度
    35.     * @return pwm值
    36. */
    37.  
    38. rt_int32_t balance_stand(float current_angle,float target_angle,float gyro_y)
    39. {
    40.     rt_int32_t s_pwm_out;
    41.  
    42.     s_pwm_out = s_pid.kp_stand *(target_angle - current_angle) + s_pid.kd * gyro_y;
    43.  
    44.     return s_pwm_out ;
    45. }
    46. /**
    47.     * @author:小飞哥玩嵌入式-小飞哥
    48.     * @TODO:小车速度环控制
    49.     * @param 左轮编码器
    50.     * @param 右轮编码器
    51.     * @param 目标值
    52.     * @return pwm值
    53. */
    54. rt_int32_t balance_speed(rt_int32_t encoder_left,rt_int32_t encoder_right,rt_int32_t target)
    55. {
    56.     rt_int32_t s_pwm_out;
    57.     //计算当前误差
    58.     s_pid.err_current = encoder_left + encoder_right - target;
    59.     //低通滤波器，low_filter_out = (1-a)*Ek+a*low_filter_out_last
    60.     s_pid.lowfilter_out = (1-s_pid.lowfilter_rate)*s_pid.err_current + s_pid.lowfilter_out_last*s_pid.lowfilter_rate;
    61.     s_pid.lowfilter_out_last = s_pid.lowfilter_out;
    62.     //速度环偏差积分，积分出位移
    63.     s_pid.err_sum += s_pid.lowfilter_out;
    64.     //积分限幅
    65.     s_pid.err_sum = s_pid.err_sum>50000?50000:((s_pid.err_sum<-50000)?-50000:s_pid.err_sum);
    66.     //速度环计算输出
    67.     s_pwm_out = s_pid.kp_speed * s_pid.lowfilter_out + s_pid.ki * s_pid.err_sum;
    68.  
    69.     return s_pwm_out;
    70. }
    71.  
    72. /**
    73.     * @author:小飞哥玩嵌入式-小飞哥
    74.     * @TODO:小车转向环控制
    75.     * @param gyro_z
    76.     * @return pwm值
    77. */
    78. rt_int32_t balance_turn(rt_int32_t gyro_z)
    79. {
    80.     rt_int32_t pwm_out;
    81.     pwm_out = s_pid.kp_turn*gyro_z;
    82.     return pwm_out;
    83. }
    84.

    最终是主控制代码

    1. /**
    2.     * @author:小飞哥玩嵌入式-小飞哥
    3.     * @TODO:小车控制初始化
    4.     * @param NULL
    5.     * @return NULL
    6. */
    7.  
    8. //机械中值
    9.  
    10. void rt_balanceCar_ctrlinit(void)
    11. {
    12.     //pwm初始化
    13.    rt_balanceCar_pwmInit();
    14.    //电机控制IO初始化
    15.    rt_balanceCar_pinInit();
    16.    //PID参数初始化
    17.    pid_init();
    18.    //MPU6050初始化
    19.    mpu_measurement_init();
    20.    //mpu6050中断初始化
    21.    mpu6050_isr_init();
    22.    //编码器初始化
    23.    hw_Encoder_init();
    24.    //按键初始化
    25.    rt_key_init();
    26. }
    27. //INIT_COMPONENT_EXPORT(rt_balanceCar_ctrlinit);
    28. /**
    29.     * @author:小飞哥玩嵌入式-小飞哥
    30.     * @TODO:小车运动控制
    31.     * @param NULL
    32.     * @return NULL
    33. */
    34. void rt_balanceCar_ctrl(rt_int32_t motor_l,rt_int32_t motor_r)
    35. {
    36.     if(motor_l > 0)
    37.     {
    38.         rt_balanceCar_LeftMotorforward();
    39.     }
    40.     else {
    41.  
    42.         rt_balanceCar_LeftMotorback();
    43.     }
    44.  
    45.     if(motor_r > 0)
    46.     {
    47.         rt_balanceCar_RightMotorforward();
    48.     }
    49.     else {
    50.  
    51.         rt_balanceCar_RightMotorback();
    52.     }
    53.  
    54.     rt_balanceCar_pwmSet(PWM_DEV_CHANNEL3,PWM_DEV_CHANNEL4,motor_l,motor_r);
    55. }
    56.  
    57. /* 线程入口 */
    58. static void thread1_entry(void* parameter)
    59. {
    60.     S_MEASURE_OUT s_measure_out;
    61.     S_ENCODER_MEASURE s_encoder_measure;
    62.     char str[32];
    63.     static rt_int32_t pwm_out = 0;
    64.  
    65.  
    66.     static rt_int32_t pwm_value_stand = 0;
    67.     static rt_int32_t pwm_value_speed = 0;
    68.     static rt_int32_t pwm_value_turn = 0;
    69.  
    70.     while(1)
    71.     {
    72.         //获取编码器数据
    73.         encoder_getCounter(&s_encoder_measure.l_speed,&s_encoder_measure.r_speed);
    74.         if(RT_EOK == rt_sem_take(RT_TIMER_SEM, 0xFFFF))
    75.         {
    76.           Button_Process();     //需要周期调用按键处理函数
    77.  
    78.  
    79.           //获取陀螺仪数据
    80.           //mpu_measurement_out(measure_out);
    81.           mpu6xxx_get_gyro(i2c_bus,&s_measure_out.gyro);
    82.  
    83.           if (0==mpu_dmp_get_data(&s_measure_out.pitch, &s_measure_out.roll, &s_measure_out.yaw) )
    84.           {
    85.               //计算直立环PWM输出
    86.               pwm_value_stand = balance_stand(s_measure_out.pitch,s_pid.mid_value,s_measure_out.gyro.y);
    87.               //计算速度环PWM输出
    88.               pwm_value_speed = balance_speed(s_encoder_measure.l_speed,s_encoder_measure.r_speed,0);
    89.               //计算转向环输出
    90.               pwm_value_turn = balance_turn(s_measure_out.gyro.z);
    91.               //PWM输出
    92.               pwm_out = pwm_value_stand - s_pid.kp_stand*pwm_value_speed;
    93.               //PWM控制
    94.               if(_ABS(s_measure_out.pitch)>30)//倾角>30度，关闭电机
    95.               {
    96.                   rt_balanceCar_stop();
    97.                   pid_init();
    98.               }
    99.               else {
    100.                   rt_balanceCar_ctrl(pwm_out+pwm_value_turn,pwm_out-pwm_value_turn);
    101.             }
    102.           }
    103.           static int i = 0;
    104.           i++;
    105.           if(i%50 == 0)
    106.           {
    107.               i = 0;
    108.  
    109.               rt_kprintf("\r\n\r\n\r\n");
    110.  
    111.                sprintf(str,"pitch=%.2f\r\n",s_measure_out.pitch);
    112.                rt_kprintf(str);
    113.  
    114.                sprintf(str,"roll=%.2f\r\n",s_measure_out.roll);
    115.                rt_kprintf(str);
    116.  
    117.                sprintf(str,"yaw=%.2f\r\n",s_measure_out.yaw);
    118.                rt_kprintf(str);
    119.                rt_kprintf("\r\n\r\n\r\n");
    120.  
    121.  
    122.                sprintf(str,"gyro.x=%d\r\n",s_measure_out.gyro.x);
    123.                rt_kprintf(str);
    124.                sprintf(str,"gyro.y=%d\r\n",s_measure_out.gyro.y);
    125.                rt_kprintf(str);
    126.                sprintf(str,"gyro.z=%d\r\n",s_measure_out.gyro.z);
    127.                rt_kprintf(str);
    128.               rt_kprintf("\r\nencoder_l = %d\r\n",s_encoder_measure.l_speed);
    129.               rt_kprintf("\r\nencoder_r = %d\r\n",s_encoder_measure.r_speed);
    130.               rt_kprintf("\r\ns_pid.kp_stand = %d\r\n",(rt_int32_t)s_pid.kp_stand);
    131.  
    132.           }
    133.  
    134.         }
    135.     }
    136. }
    137. /**
    138.     * @author:小飞哥玩嵌入式-小飞哥
    139.     * @TODO:小车控制线程创建
    140.     * @param NULL
    141.     * @return NULL
    142. */
    143. int balanceCar_sample(void)
    144. {
    145.     static rt_thread_t tid1 = RT_NULL;
    146.  
    147.     rt_balanceCar_ctrlinit();
    148.     /* 创建线程  */
    149.     tid1=rt_thread_create(
    150.                    "thread1",
    151.                    thread1_entry,
    152.                    RT_NULL,
    153.                    THREAD_STACK_SIZE,
    154.                    THREAD_PRIORITY, THREAD_TIMESLICE);
    155.     /* 如果获得线程控制块，启动这个线程 */
    156.     if (tid1 != RT_NULL)
    157.         rt_thread_startup(tid1);
    158.     return 0;
    159. }
    160. /* 导出到 msh 命令列表中 */
    161. INIT_COMPONENT_EXPORT(balanceCar_sample);

    至此，基本代码控制就完成了，内容很多，小飞哥可能会出视频讲，大家可以先自己看源码消化哈

    经验交流

     

    欢迎关注小飞哥玩嵌入式，期待遇到优秀的你

  • 相关阅读:
    使用浏览功能
    CGLIB 动态代理使用
    设计LRU/LFU缓存结构
    读书会招募 | 勇气七日谈，一起在“讨厌”中寻找幸福的真谛
    Flink PostgreSQL CDC配置和常见问题
    python 两个文件对比，以文件1为标准将文件2中有相等的内容整行取出
    vue3中使用element plus
    MySQL数据库的备份和恢复
    回归预测 | MATLAB实现贝叶斯优化门控循环单元(BO-GRU)多输入单输出
    java毕业设计驾考预约系统（附源码、数据库）
  • 原文地址：https://blog.csdn.net/qq_16519885/article/details/125306996