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

用户8913398

发布于 2022-11-16 15:37:23

8040

发布于 2022-11-16 15:37:23

文章被收录于专栏：嵌入式实验基地

演示视频在这里

摘要

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

源码下载链接在文末评论区哈！！！

主要RT-Thread内容

RT-Thread
PID
PWM
MPU6050
编码器
定时器
线程

模组介绍

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

电机驱动模块

某宝买的L298N电机驱动模组

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

陀螺仪

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

电机

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

电池

3串1并18650动力电池

主控

RT-Thread ART-PI控制板

亚克力板

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

软件设计

接线

电机驱动接线：

电机	ART-PI
PWMA	PB0
IN1	H14
IN2	C7
IN3	G10
In4	I6
PWMB	PB1
12V	/
5V	5V
GND	GND

MPU6050接线

MPU6050	ART-PI
3.3V	3.3V
GND	GND
SCL	PH11
SDA	PH12

左电机与电机驱动模组：

电机	ART-PI	电机驱动模组
电机+	/	OUT1
编码器电源-	GND
编码器A	PA8
编码器B	PA9
编码器电源+	3.3V/5V
电机-	/	OUT2

右电机与电机驱动模组：

电机	ART-PI	电机驱动模组
电机+	/	OUT3
编码器电源-	GND
编码器A	PA1
编码器B	PA15
编码器电源+	3.3V/5V
电机-	/	OUT4

软件代码

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

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

MPU6050驱动

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

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:mpu6050初始化
    * @param NULL
    * @return NULL
*/
void mpu_measurement_init(void)
{
    i2c_bus = (struct mpu6xxx_device *) mpu6xxx_init(MPU6050_I2C_BUS_NAME, MPU6050_ADDR); //初始化MPU6050，测量单位为角速度，加速度    while(count++)

    rt_int8_t res = 1;
    while (res)
    {
        res = mpu_dmp_init();
        rt_kprintf("\r\nRES = %d\r\n",res);

        rt_thread_mdelay(500);
        rt_kprintf("\r\nMPU6050 DMP init Error\r\n");
    }
    rt_kprintf("\r\nMPU6050 DMP init OK\r\n");
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:MPU写入多字节数据
    * @param NULL
    * @return NULL
*/
rt_uint8_t MPU_Write_Len(rt_uint8_t addr,rt_uint8_t reg,rt_uint8_t len,rt_uint8_t *databuf)
{
    rt_int8_t res = 0;
#ifdef RT_USING_I2C
    struct rt_i2c_msg msgs;
    rt_uint8_t buf[50] = {0};
#endif
    buf[0] = reg;

    for(int i = 0;i<len;i++)
    {
        buf[i+1]=databuf[i];
    }

    if (i2c_bus->bus->type == RT_Device_Class_I2CBUS)
    {
        msgs.addr  = i2c_bus->i2c_addr;    /* slave address */
        msgs.flags = RT_I2C_WR;        /* write flag */
        msgs.buf   = buf;              /* Send data pointer */
        msgs.len   = len+1;

        if (rt_i2c_transfer((struct rt_i2c_bus_device *)i2c_bus->bus, &msgs, 1) == 1)
        {
            res = RT_EOK;
        }
        else
        {
            res = -RT_ERROR;
        }
    }
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:mpu读取多字节数据
    * @param NULL
    * @return NULL
*/
rt_uint8_t MPU_Read_Len(rt_uint8_t addr,rt_uint8_t reg,rt_uint8_t len,rt_uint8_t *buf)
{
    rt_int8_t res = 0;
#ifdef RT_USING_I2C
    struct rt_i2c_msg msgs[2];
#endif
#ifdef RT_USING_SPI
    rt_uint8_t tmp;
#endif
    if (i2c_bus->bus->type == RT_Device_Class_I2CBUS)
    {
        msgs[0].addr  = i2c_bus->i2c_addr;    /* Slave address */
        msgs[0].flags = RT_I2C_WR;        /* Write flag */
        msgs[0].buf   = &reg;             /* Slave register address */
        msgs[0].len   = 1;                /* Number of bytes sent */

        msgs[1].addr  = i2c_bus->i2c_addr;    /* Slave address */
        msgs[1].flags = RT_I2C_RD;        /* Read flag */
        msgs[1].buf   = buf;              /* Read data pointer */
        msgs[1].len   = len;              /* Number of bytes read */

        if (rt_i2c_transfer((struct rt_i2c_bus_device *)i2c_bus->bus, msgs, 2) == 2)
        {
            res = RT_EOK;
        }
        else
        {
            res = -RT_ERROR;
        }
    }

    return res;
}

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

电机驱动

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车电机控制初始化
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_pinInit(void)
{
    rt_pin_mode(motor_A1, PIN_MODE_OUTPUT );
    rt_pin_mode(motor_A2, PIN_MODE_OUTPUT );
    rt_pin_mode(motor_B1, PIN_MODE_OUTPUT );
    rt_pin_mode(motor_B2, PIN_MODE_OUTPUT );

    rt_pin_write(motor_A1,PIN_LOW);
    rt_pin_write(motor_A2,PIN_LOW);
    rt_pin_write(motor_B1,PIN_LOW);
    rt_pin_write(motor_B2,PIN_LOW);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车左轮前进
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_LeftMotorforward(void)
{
    rt_pin_write(motor_B1,PIN_LOW);
    rt_pin_write(motor_B2,PIN_HIGH);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车左轮后退
    * @param NULL
    * @return NULL
*/
void  rt_balanceCar_LeftMotorback(void)
{
  rt_pin_write(motor_B1,PIN_HIGH);
  rt_pin_write(motor_B2,PIN_LOW);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车右轮前进
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_RightMotorforward(void)
{
    rt_pin_write(motor_A1,PIN_LOW);
    rt_pin_write(motor_A2,PIN_HIGH);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车右轮后退
    * @param NULL
    * @return NULL
*/
void  rt_balanceCar_RightMotorback(void)
{   rt_pin_write(motor_A1,PIN_HIGH);
    rt_pin_write(motor_A2,PIN_LOW);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车整机前进
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_forward(void)
{
    rt_pin_write(motor_A1,PIN_HIGH);
    rt_pin_write(motor_A2,PIN_LOW);
    rt_pin_write(motor_B1,PIN_HIGH);
    rt_pin_write(motor_B2,PIN_LOW);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车整机后退
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_back(void)
{
    rt_pin_write(motor_A1,PIN_LOW);
    rt_pin_write(motor_A2,PIN_HIGH);
    rt_pin_write(motor_B1,PIN_LOW);
    rt_pin_write(motor_B2,PIN_HIGH);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车整机左转
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_turnLeft(void)
{
    rt_pin_write(motor_A1,PIN_LOW);
    rt_pin_write(motor_A2,PIN_LOW);
    rt_pin_write(motor_B1,PIN_HIGH);
    rt_pin_write(motor_B2,PIN_LOW);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车整机右转
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_turnRight(void)
{
    rt_pin_write(motor_A1,PIN_HIGH);
    rt_pin_write(motor_A2,PIN_LOW);
    rt_pin_write(motor_B1,PIN_LOW);
    rt_pin_write(motor_B2,PIN_LOW);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车电机停转
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_stop(void)
{
    rt_pin_write(motor_A1,PIN_LOW);
    rt_pin_write(motor_A2,PIN_LOW);
    rt_pin_write(motor_B1,PIN_LOW);
    rt_pin_write(motor_B2,PIN_LOW);
}

PWM控制

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:pwm使能
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_pwmEnable(void)
{
    rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL3);
    rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL4);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:pwm失能
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_pwmDisable(void)
{
    rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL3);
    rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL4);
}

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:pwm输出限幅
    * @param pwm1
    * @param pwm2
    * @return NULL
*/
void rt_balanceCar_pwmlimit(rt_int32_t *pwm1,rt_int32_t *pwm2)
{
    if(*pwm1 >= PWM_UPPER_LIMIT)
    {
        *pwm1 = PWM_UPPER_LIMIT;
    }
    else if(*pwm1 <= PWM_LOWER_LIMIT)
    {
        *pwm1 = PWM_LOWER_LIMIT;
    }

    if(*pwm2 >= PWM_UPPER_LIMIT)
    {
        *pwm2 = PWM_UPPER_LIMIT;
    }
    else if(*pwm2 <= PWM_LOWER_LIMIT)
    {
        *pwm2 = PWM_LOWER_LIMIT;
    }
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:pwm设置
    * @param channel1
    * @param channel2
    * @param L_speed
    * @param R_speed
    * @return NULL
*/
void rt_balanceCar_pwmSet(rt_uint8_t channel1,rt_uint8_t channel2,rt_int32_t L_speed,rt_int32_t R_speed)
{
    //输出限幅
    rt_balanceCar_pwmlimit(&L_speed,&R_speed);

    //pwm设置
    rt_pwm_set(pwm_dev, channel1, PWM_PERIOD, _ABS(L_speed));
    rt_pwm_set(pwm_dev, channel2, PWM_PERIOD, _ABS(R_speed));
/*
    rt_pwm_enable(pwm_dev, channel1);
    rt_pwm_enable(pwm_dev, channel2);
*/
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:pwm初始化
    * @param NULL
    * @return NULL
*/
rt_int8_t rt_balanceCar_pwmInit(void)
{
    pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
    if (pwm_dev == RT_NULL)
    {
        rt_kprintf("\r\npwm sample run failed! can't find %s device!\r\n", PWM_DEV_NAME);
        return RT_ERROR;
    }
    rt_kprintf("\r\npwm sample run success! find %s device!\r\n", PWM_DEV_NAME);

    //关闭PWM
    //rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL3);
    //rt_pwm_disable(pwm_dev, PWM_DEV_CHANNEL4);
    //开启PWM
    rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL3);
    rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL4);
    return RT_EOK;
}

编码器

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

/* USER CODE END 0 */

TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
/* TIM1 init function */

/**
  * @brief TIM1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM1_Init(void)
{

  /* USER CODE BEGIN TIM1_Init 0 */

  /* USER CODE END TIM1_Init 0 */

  TIM_Encoder_InitTypeDef sConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM1_Init 1 */

  /* USER CODE END TIM1_Init 1 */
  htim1.Instance = TIM1;
  htim1.Init.Prescaler = 0;
  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim1.Init.Period = 65535;
  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim1.Init.RepetitionCounter = 0;
  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
  sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC1Filter = 0;
  sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC2Filter = 0;
  if (HAL_TIM_Encoder_Init(&htim1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM1_Init 2 */

  /* USER CODE END TIM1_Init 2 */

}

/**
  * @brief TIM2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM2_Init(void)
{

  /* USER CODE BEGIN TIM2_Init 0 */

  /* USER CODE END TIM2_Init 0 */

  TIM_Encoder_InitTypeDef sConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM2_Init 1 */

  /* USER CODE END TIM2_Init 1 */
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 4294967295;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
  sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC1Filter = 0;
  sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC2Filter = 0;
  if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */

  /* USER CODE END TIM2_Init 2 */

}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:清除编码器数值
    * @param NULL
    * @return NULL
*/
void encoder_clearCounter(void)
{
    __HAL_TIM_SET_COUNTER(&htim1,0);
    __HAL_TIM_SET_COUNTER(&htim2,0);
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:获取编码器数值
    * @param out s_encoder_measure
    * @return NULL
*/
void encoder_getCounter(rt_int32_t *l_speed,rt_int32_t *r_speed)
{
    *l_speed = ((rt_int32_t)__HAL_TIM_GET_COUNTER(&htim1)-COUNTER_RESET);
    *r_speed = (rt_int32_t)__HAL_TIM_GET_COUNTER(&htim2)-COUNTER_RESET;

    encoder_clearCounter();
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:编码器初始化
    * @param NULL
    * @return NULL
*/
int hw_Encoder_init(void)
{
    MX_TIM1_Init();
    MX_TIM2_Init();

    HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);

    rt_kprintf("\r\ntim1,tim2 init ok\r\n");
}

PID控制算法

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

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:PID参数初始化
    * @param NULL
    * @param NULL
    * @return NULL
     */
void pid_init(void)
{
    s_pid.kp_speed = -0.35;//速度环kp值
    s_pid.kp_stand = -1600*0.6;//直立环kp值


    s_pid.ki = s_pid.kp_speed/200;
    s_pid.kd = 65*0.6;

    s_pid.kp_turn = 20;

    s_pid.limit = 800;
    s_pid.err_current = 0;
    s_pid.err_last = 0;
    s_pid.err_sum = 0;

    s_pid.lowfilter_rate = 0.7;

    s_pid.mid_value = -1;
}

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车直立环控制
    * @param 当前角度
    * @param 目标角度
    * @param 真实角速度
    * @return pwm值
*/

rt_int32_t balance_stand(float current_angle,float target_angle,float gyro_y)
{
    rt_int32_t s_pwm_out;

    s_pwm_out = s_pid.kp_stand *(target_angle - current_angle) + s_pid.kd * gyro_y;

    return s_pwm_out ;
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车速度环控制
    * @param 左轮编码器
    * @param 右轮编码器
    * @param 目标值
    * @return pwm值
*/
rt_int32_t balance_speed(rt_int32_t encoder_left,rt_int32_t encoder_right,rt_int32_t target)
{
    rt_int32_t s_pwm_out;
    //计算当前误差
    s_pid.err_current = encoder_left + encoder_right - target;
    //低通滤波器，low_filter_out = (1-a)*Ek+a*low_filter_out_last
    s_pid.lowfilter_out = (1-s_pid.lowfilter_rate)*s_pid.err_current + s_pid.lowfilter_out_last*s_pid.lowfilter_rate;
    s_pid.lowfilter_out_last = s_pid.lowfilter_out;
    //速度环偏差积分，积分出位移
    s_pid.err_sum += s_pid.lowfilter_out;
    //积分限幅
    s_pid.err_sum = s_pid.err_sum>50000?50000:((s_pid.err_sum<-50000)?-50000:s_pid.err_sum);
    //速度环计算输出
    s_pwm_out = s_pid.kp_speed * s_pid.lowfilter_out + s_pid.ki * s_pid.err_sum;

    return s_pwm_out;
}

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车转向环控制
    * @param gyro_z
    * @return pwm值
*/
rt_int32_t balance_turn(rt_int32_t gyro_z)
{
    rt_int32_t pwm_out;
    pwm_out = s_pid.kp_turn*gyro_z;
    return pwm_out;
}

最终是主控制代码

/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车控制初始化
    * @param NULL
    * @return NULL
*/

//机械中值

void rt_balanceCar_ctrlinit(void)
{
    //pwm初始化
   rt_balanceCar_pwmInit();
   //电机控制IO初始化
   rt_balanceCar_pinInit();
   //PID参数初始化
   pid_init();
   //MPU6050初始化
   mpu_measurement_init();
   //mpu6050中断初始化
   mpu6050_isr_init();
   //编码器初始化
   hw_Encoder_init();
   //按键初始化
   rt_key_init();
}
//INIT_COMPONENT_EXPORT(rt_balanceCar_ctrlinit);
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车运动控制
    * @param NULL
    * @return NULL
*/
void rt_balanceCar_ctrl(rt_int32_t motor_l,rt_int32_t motor_r)
{
    if(motor_l > 0)
    {
        rt_balanceCar_LeftMotorforward();
    }
    else {

        rt_balanceCar_LeftMotorback();
    }

    if(motor_r > 0)
    {
        rt_balanceCar_RightMotorforward();
    }
    else {

        rt_balanceCar_RightMotorback();
    }

    rt_balanceCar_pwmSet(PWM_DEV_CHANNEL3,PWM_DEV_CHANNEL4,motor_l,motor_r);
}

/* 线程入口 */
static void thread1_entry(void* parameter)
{
    S_MEASURE_OUT s_measure_out;
    S_ENCODER_MEASURE s_encoder_measure;
    char str[32];
    static rt_int32_t pwm_out = 0;


    static rt_int32_t pwm_value_stand = 0;
    static rt_int32_t pwm_value_speed = 0;
    static rt_int32_t pwm_value_turn = 0;

    while(1)
    {
        //获取编码器数据
        encoder_getCounter(&s_encoder_measure.l_speed,&s_encoder_measure.r_speed);
        if(RT_EOK == rt_sem_take(RT_TIMER_SEM, 0xFFFF))
        {
          Button_Process();     //需要周期调用按键处理函数


          //获取陀螺仪数据
          //mpu_measurement_out(measure_out);
          mpu6xxx_get_gyro(i2c_bus,&s_measure_out.gyro);

          if (0==mpu_dmp_get_data(&s_measure_out.pitch, &s_measure_out.roll, &s_measure_out.yaw) )
          {
              //计算直立环PWM输出
              pwm_value_stand = balance_stand(s_measure_out.pitch,s_pid.mid_value,s_measure_out.gyro.y);
              //计算速度环PWM输出
              pwm_value_speed = balance_speed(s_encoder_measure.l_speed,s_encoder_measure.r_speed,0);
              //计算转向环输出
              pwm_value_turn = balance_turn(s_measure_out.gyro.z);
              //PWM输出
              pwm_out = pwm_value_stand - s_pid.kp_stand*pwm_value_speed;
              //PWM控制
              if(_ABS(s_measure_out.pitch)>30)//倾角>30度，关闭电机
              {
                  rt_balanceCar_stop();
                  pid_init();
              }
              else {
                  rt_balanceCar_ctrl(pwm_out+pwm_value_turn,pwm_out-pwm_value_turn);
            }
          }
          static int i = 0;
          i++;
          if(i%50 == 0)
          {
              i = 0;

              rt_kprintf("\r\n\r\n\r\n");

               sprintf(str,"pitch=%.2f\r\n",s_measure_out.pitch);
               rt_kprintf(str);

               sprintf(str,"roll=%.2f\r\n",s_measure_out.roll);
               rt_kprintf(str);

               sprintf(str,"yaw=%.2f\r\n",s_measure_out.yaw);
               rt_kprintf(str);
               rt_kprintf("\r\n\r\n\r\n");


               sprintf(str,"gyro.x=%d\r\n",s_measure_out.gyro.x);
               rt_kprintf(str);
               sprintf(str,"gyro.y=%d\r\n",s_measure_out.gyro.y);
               rt_kprintf(str);
               sprintf(str,"gyro.z=%d\r\n",s_measure_out.gyro.z);
               rt_kprintf(str);
              rt_kprintf("\r\nencoder_l = %d\r\n",s_encoder_measure.l_speed);
              rt_kprintf("\r\nencoder_r = %d\r\n",s_encoder_measure.r_speed);
              rt_kprintf("\r\ns_pid.kp_stand = %d\r\n",(rt_int32_t)s_pid.kp_stand);

          }

        }
    }
}
/**
    * @author:小飞哥玩嵌入式-小飞哥
    * @TODO:小车控制线程创建
    * @param NULL
    * @return NULL
*/
int balanceCar_sample(void)
{
    static rt_thread_t tid1 = RT_NULL;

    rt_balanceCar_ctrlinit();
    /* 创建线程  */
    tid1=rt_thread_create(
                   "thread1",
                   thread1_entry,
                   RT_NULL,
                   THREAD_STACK_SIZE,
                   THREAD_PRIORITY, THREAD_TIMESLICE);
    /* 如果获得线程控制块，启动这个线程 */
    if (tid1 != RT_NULL)
        rt_thread_startup(tid1);
    return 0;
}
/* 导出到 msh 命令列表中 */
INIT_COMPONENT_EXPORT(balanceCar_sample);

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

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2022-06-16，如有侵权请联系 cloudcommunity@tencent.com 删除

pid

thread

本文分享自小飞哥玩嵌入式微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

pid

thread

登录后参与评论

0 条评论

热度