动量轮平衡自行车实现笔记

手搓手柄驱动量轮平衡小车

摘要

最近闲暇想玩点小玩艺，想到了网上一时火起的平衡小车，苦于没有结构件，买整套的吧，感觉对于咱们手艺人来讲有点羞耻，看看淘宝上有纯结构件卖，于是乎先把硬件板画了起来，后来硬件所有资源调通后，再购买结构件回来时，这版硬件居然能勉强固定两个螺丝，玩上了视频中的一版。在整个玩的过程中，有些调试问题，记录下来。

一、stm32外设I2C驱mpu6050的问题

我用的是STM32F103C6的 I2C1 映射到PB8,PB9，用函数读MPU6050寄存器时，发现每第二次读时，I2C总线死在发起start信号后。后来看了手册后，需要在操作完之后，加一条代码：

LL_I2C_AcknowledgeNextData(I2Cx, LL_I2C_ACK);

即整个函数如下

uint8_t MPU6050_ReadReg(uint8_t reg)
{
    uint8_t value; 
    // __disable_irq();    
    LL_GPIO_AF_EnableRemap_I2C1();
    LL_I2C_GenerateStartCondition(I2Cx);
    while(!LL_I2C_IsActiveFlag_SB(I2Cx));
    //USART1_SendString("2");
    
    LL_I2C_TransmitData8(I2Cx, MPU6050_ADDR_WRITE);   
    while(!LL_I2C_IsActiveFlag_ADDR(I2Cx));
    LL_I2C_ClearFlag_ADDR(I2Cx);
    LL_I2C_TransmitData8(I2Cx, reg);
    while(!LL_I2C_IsActiveFlag_BTF(I2Cx));

    LL_I2C_GenerateStartCondition(I2Cx);
    while(!LL_I2C_IsActiveFlag_SB(I2Cx));
    LL_I2C_TransmitData8(I2Cx, MPU6050_ADDR_READ);    
    while(!LL_I2C_IsActiveFlag_ADDR(I2Cx));
    LL_I2C_ClearFlag_ADDR(I2Cx);    
    
    LL_I2C_AcknowledgeNextData(I2Cx, LL_I2C_NACK);
    LL_I2C_GenerateStopCondition(I2Cx);
    
    while(!LL_I2C_IsActiveFlag_RXNE(I2Cx));
    value = LL_I2C_ReceiveData8(I2Cx);
    LL_I2C_AcknowledgeNextData(I2Cx, LL_I2C_ACK);  
    return value;
   
}

二、手柄驱动小记

手上有一款手柄，一直想用单片机来驱玩玩，正好这次就画到这种小板上去了，搜了下usb host芯片，最后选用了ch370T，但最后去找技术支持和官网上去下资料时，有点懵B了，给的都是374的资料，也没有手柄的程序，最后在电脑上监控usb数据流，再用ch370把每个数据流发给手柄u转无线器，庆幸最后驱动成功。获取数据正确。但是整体方案上有点问题就是用手柄驱动方向舵机时，舵机会使5V电压下降，致ch370驱动U口死机。也就是说，后续方案再做时，电池供电转5V时，需要稳压。

三、获取pitch roll  yaw角度

mpu6050只能获取角速率和角加速度，以及温度。这些数据是不能直接参与到平衡pid算法中的，需要转换出roll横滚角度。网上有很多方法，我直接移植了motion_driver_6.12。我的电路板的陀螺仪转换坐标系如下

// 陀螺仪方向设置
static signed char gyro_orientation[9] = { 0, -1, 0,
                                           1, 0, 0,
                                           0, 0, 1};

初始化及获取角度代码如下：

uint8_t mpu_dmp_init(void)
{
	uint8_t res=0;
	if(mpu_init(NULL)==0)	//初始化MPU6050
	{        
		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
		if(res)return 1;
		res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
		if(res)return 2;
		res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	//设置采样率
		if(res)return 3;
		res=dmp_load_motion_driver_firmware();		//加载dmp固件
		if(res)return 4;
		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
		if(res)return 5;
		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//设置dmp功能
		    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
		    DMP_FEATURE_GYRO_CAL);
		if(res)return 6;
		res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//设置DMP输出速率(最大不超过200Hz)
		if(res)return 7;
		res=run_self_test();		//自检
		if(res)return 8;
		res=mpu_set_dmp_state(1);	//使能DMP
		if(res)return 9;
	}
	return 0;
}

//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
uint8_t mpu_dmp_get_data(IMU_Parameter *IMU_Data)
{
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
	unsigned long sensor_timestamp;
	short gyro[3], accel[3], sensors;
	unsigned char more;
	long quat[4];
	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;

    IMU_Data->Gyro_X = gyro[0];
    IMU_Data->Gyro_Y = gyro[1];
    IMU_Data->Gyro_Z = gyro[2];

    IMU_Data->Accel_X = accel[0];
    IMU_Data->Accel_Y = accel[1];
    IMU_Data->Accel_Z = accel[2];

	if(sensors&INV_WXYZ_QUAT)
	{
		q0 = quat[0] / q30;	//q30格式转换为浮点数
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30;
		//计算得到俯仰角/横滚角/航向角
		IMU_Data->Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		IMU_Data->Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		IMU_Data->Yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw

        // usart1_report_imu(accel[0],accel[1],accel[2],gyro[0],gyro[1],gyro[2],IMU_Data->Angle_X,IMU_Data->Angle_Y,IMU_Data->Angle_Z);
	}else return 2;
	return 0;
}

以上三个问题解决，其它的基本没什么问题了。蓝牙我作的是ECB01H2，在电脑上用qt写一版调试工具。如视频中所见。