摘要
小伙伴们,停更很久的RT-Thread实战笔记来啦,本章节教大家如何打造一个属于自己的平衡车,废话不多说,来吧,淦!!!
主要RT-Thread内容
-
RT-Thread -
PID -
PWM -
MPU6050 -
编码器 -
定时器 -
线程
模组介绍
利用手中已经积灰多年的小模块,废物利用,打造一个专属的平衡车
电机驱动模块
某宝买的L298N电机驱动模组
或者TB6612,关于这两个模组的介绍就不多说了,大家可以自行百度下哈
陀螺仪
陀螺仪选用的是用的比较多的[MPU6050],目前好像要停产了,价格也越来越贵
电机
电机采用的是带有编码器的直流减速电机,价格也略微贵一些
电池
主控
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
移植的是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???=?®?????????????/*?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);
}
/**
????*?@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,本章也不再具体介绍了,主要包括直立环、转向环、速度环三个控制环
/**
????*?@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);
至此,基本代码控制就完成了,内容很多,小飞哥可能会出视频讲,大家可以先自己看源码消化哈
经验交流
?
欢迎关注小飞哥玩嵌入式,期待遇到优秀的你
|