MPU6050单轴双浆姿态平衡PID调戏之MPU6050初始化篇

这个其实是为了直观的观察PID的调节过程，因为本人对于数学并不擅长，又说PID是比较依赖经验，看着老外搭建了一个实验的设备，我也画了葫芦做了一个。以下是需要的设备介绍。
在这里插入图片描述
需要的设备

硬件介绍

树莓派3b
MPU6050
PCA9685
2个有浆无刷电机（浆要两个不一样的，转的时候要抵消）

编程平台

树莓派运行官方Linux系统，编辑器用交叉编译工具（arm-linux-g++）。

MPU6050之初始化

这里就不介绍6050的各种功能了。树莓派与MPU6050通过IIC接口连接，供电也是用树莓派的3.3v接口。其实早在2个月前已经完成了功能，但是由于MPU6050的接线问题，导致初始化通不过，而且写入的数据和读出的数据对不上，一度以为是IIC库，或者是模块坏了，有时候灵光，有时候不灵光。MPU6050接线已经要稳可靠。期间还烧了一块MPU9250。

void MPU6050lib::initMPU6050()
{
  uint8_t c = 0;
  delay(1000);
  for(;;)
  {
    printf("INIT MPU6050\n");
    WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); //复位
    delay(100);
    c =  ReadByte(MPU6050_ADDRESS, PWR_MGMT_1);
    if(c != 0x40)
      continue;
    delay(100);
    WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);//唤醒
    delay(100);  
    WriteByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);//设置时钟 
    delay(100);
    c =  ReadByte(MPU6050_ADDRESS, PWR_MGMT_1);
    if(c != 0x01)
      continue;
    delay(100);
    //DLPF_CFG，设置低通滤波，我一度以为是这里出问题。后来想到是不是杜邦线接线不稳。
    WriteByte(MPU6050_ADDRESS, CONFIG, DLPF_CFG);
#if defined MPU_DEBUG
    c =  ReadByte(MPU6050_ADDRESS, CONFIG);
    printf("CONFIG =%x\n", c);
#endif  
    if(c != DLPF_CFG)
      continue;
    // Set sample rate = gyroscope output rate 1000/(1 + SMPLRT_DIV)
    // 0x04 use 200hz ,0x07 use 125hz
    //SMPLRT_DIV       0x19
    delay(100);
    WriteByte(MPU6050_ADDRESS, SMPLRT_DIV, SMPLRT_DIV_RATE);   
    delay(100);
#if defined MPU_DEBUG
    c =  ReadByte(MPU6050_ADDRESS, SMPLRT_DIV);
    printf("SMPLRT_DIV0x13 =%x\n", c);
#endif
    if(c != SMPLRT_DIV_RATE)
      continue;  
    delay(100);
  
    uint8_t c =  ReadByte(MPU6050_ADDRESS, GYRO_CONFIG);
    WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); //  
    WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18); //  
    delay(100);
    //SetGscale = 0x03,这里设置2000的量程。
    WriteByte(MPU6050_ADDRESS, GYRO_CONFIG, SetGscale << 3); //  
#if defined MPU_DEBUG
  c =  ReadByte(MPU6050_ADDRESS, GYRO_CONFIG);
  printf("GYRO_CONFIG is 0x18 =%x\n", c);
#endif
    if(c != (3<<3))
      continue;   
    delay(100);
    
    c =  ReadByte(MPU6050_ADDRESS, ACCEL_CONFIG);
    WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); 
    delay(100);
    WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18);  
    WriteByte(MPU6050_ADDRESS, ACCEL_CONFIG, SetAscale << 3);  
    delay(100);
#if defined MPU_DEBUG
  c =  ReadByte(MPU6050_ADDRESS, ACCEL_CONFIG);
  printf("ACCEL_CONFIG0=%x\n", c);
#endif
    delay(100);
    WriteByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22);
    delay(100);
    WriteByte(MPU6050_ADDRESS, INT_ENABLE, 0x00);    //关闭所有中断
    delay(100);
    WriteByte(MPU6050_ADDRESS, USER_CTRL,0X00);//I2C主模式关闭
    delay(100);
    WriteByte(MPU6050_ADDRESS, FIFO_EN,0X00);//关闭FIFO
    delay(100);
    WriteByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);
    delay(100);
    if(c == 0x00)
      break;
  }
}

这么复杂的初始化，主要是经常发现6050初始化常常失败，一开始以为是接线不牢固什么的，直到前两天才怀疑是I2C库文件的问题，后来改了WiringPi库。神奇的有效果了，再也没有出现问题，6050的原始数据也非常稳定。
在这里插入图片描述
MPU6050读写函数实现

//wiringPiI2CWriteReg8，改成WiringPi后的代码实现，参数address可以省略，这里没改。
uint8_t MPU6050lib::WriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
   
    if(wiringPiI2CWriteReg8(fd,subAddress,data) < 0)
        return -1;
    return 0;
}

uint8_t MPU6050lib::ReadByte(uint8_t address, uint8_t subAddress)
{
 
    uint8_t data;  
    data = wiringPiI2CReadReg8(fd, subAddress);
                
  return data; 
}

uint8_t MPU6050lib::ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest)
{  
  
  for(uint8_t i = 0; i < count; i++)
  {
    dest[i] = ReadByte(address, subAddress + i);
  }   
  return 0;
}

如果要移植到各自平台，改上面3个函数，基本差不多可以用了。

void MPU6050lib::readAccelData(int16_t *destination)
{
  uint8_t rawData[6];  // X,Y,Z,数据都放在这里
  ReadBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);   
  destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ;  
  destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ;
  destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ;
}

void MPU6050lib::readGyroData(int16_t *destination)
{
  uint8_t rawData[6];  // X,Y,Z,数据都放在这里
  ReadBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);   
  destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ;  
  destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ;
  destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ;
}

做姿态解算前的准备

接下来在sensor.c的ms_update()调用函数进入循环即可。

int ms_update() {
	mpu.readAccelData(accelCount);
	accelCount[0] -= ACCEL_OFFSET_X; //先平放输出300次，求平均值
	accelCount[1] -= ACCEL_OFFSET_Y;
	accelCount[2] += ACCEL_OFFSET_Z;//平放数据理想情况是16384，这里加上3776 = 16384
	ax = (float)(accelCount[0] )*aRes; //aRes = 1/16384 ,这里转换数据。
	ay = (float)(accelCount[1] )*aRes; 
	az = (float)(accelCount[2] )*aRes; 
    //delay_ms(5);	
  mpu.readGyroData(gyroCount);
	gyroCount[0] -= GYRO_OFFSET_X;
	gyroCount[1] -= GYRO_OFFSET_Y;
	gyroCount[2] -= GYRO_OFFSET_Z;
	gx = (float)(gyroCount[0] )*gRes ; //同上面的情况。
	gy = (float)(gyroCount[1] )*gRes ;  
	gz = (float)(gyroCount[2] )*gRes ;  
	Mahony_update_6X(gx, gy, gz, ax, ay, az); //滤波
	Mahony_computeAngles();
    //printf("%f, %f\n", getRoll(), getPitch());
	delay_ms(15); //
	return 0;
}

打印ROLL角的数据

在这里插入图片描述看着不平滑吧，但是输出的角度大概旧在-0.1- 0.05之间，自我感觉挺平稳的了。下一步是调戏解算代码的参数，或者是使用不同的解算方法，比如是卡尔漫，一阶，二阶滤波。

MPU6050单轴双浆姿态平衡PID调戏之MPU6050初始化篇

猜你喜欢