add mpu6886

src/Default_firmware.ino

@@ -78,6 +78,7 @@ void auto_tune_gyro_offset() {
 	preferences.putFloat("gyroXoffset", gyroXoffset);
 	preferences.putFloat("gyroYoffset", gyroYoffset);
 	preferences.putFloat("gyroZoffset", gyroZoffset);
+	preferences.putFloat("angleOffset", 0.0);
 	preferences.end();
 }
 
@@ -116,6 +117,8 @@ void setup() {
 			m5bala.imu->setGyroOffsets( preferences.getFloat("gyroXoffset"), 
 										preferences.getFloat("gyroYoffset"), 
 										preferences.getFloat("gyroZoffset"));
+			m5bala.setAngleOffset(preferences.getFloat("angleOffset"));
+			Serial.printf("angleOffset save :%f \r\n", preferences.getFloat("angleOffset"));
 		}
 	}
 	preferences.end();
@@ -142,6 +145,14 @@ void loop() {
 		draw_waveform();
 	}
 
+	if(M5.BtnB.wasPressed()) {
+		float offSetSave = -(m5bala.getImuAngle());
+		m5bala.setAngleOffset(offSetSave);
+		preferences.begin("m5bala-cfg", false);
+		preferences.putFloat("angleOffset", offSetSave);
+		Serial.printf("angleOffset save :%f \r\n", offSetSave);
+		preferences.end();
+	}
 	// M5Bala balance run
 	m5bala.run();
 

src/M5Bala.cpp

@@ -20,6 +20,7 @@ void M5Bala::begin() {
 	imu = new MPU6050(*wire);
 	imu->begin();
 	imu_id = i2c_readByte(MPU6050_ADDR, MPU6050_WHO_AM_I);
+	Serial.printf("imu_id:%x\r\n", imu_id);
   	// imu.calcGyroOffsets(true);
 	// imu.setGyroOffsets(-2.40, -0.41, 1.07); // FIRE
 	// imu->setGyroOffsets(5.0, 0.50, -2.6); // M5GO
@@ -138,15 +139,23 @@ void M5Bala::PIDCompute() {
 }
 
 void M5Bala::run() {
+
 	if (micros() >= loop_interval) {
 		loop_interval = micros() + 10000;
 
 		// Attitude sample
 		imu->update();
 		pitch = imu->getAngleX() + angle_offset;
+
+		Serial.printf("angle_offset:%.2f pitch:%.2f \r\n", angle_offset, pitch);
 
-		if (imu_id == MPU9250_ID)
+		if (imu_id == 0x19 || imu_id == MPU9250_ID) {
+			//Serial.printf("pitch here\r\n");
 			pitch = -pitch;
+		}
+
+
+		//Serial.printf("pitch: %f\r\n", pitch);
 		// #ifndef MPU6050_IMU
 		// pitch = -pitch;
 		// #endif
@@ -159,14 +168,15 @@ void M5Bala::run() {
 
 		// Encoder sample
 		readEncder();
-
+		
 		// PID Compute
 		PIDCompute();
 
 		// Motor out
 		setMotor(pwm_out0 + forward_offset + left_offset, 
 				 pwm_out1 + forward_offset + right_offset);
 	}
+
 }
 
 void M5Bala::stop() {

src/M5Bala.h

@@ -55,6 +55,7 @@ class M5Bala {
 		int16_t getSpeed1() { return speed_input1; };
 		int16_t getOut0() { return pwm_out0; };
 		int16_t getOut1() { return pwm_out1; };
+		float getImuAngle() { return imu->getAngleX(); }
 
 		MPU6050 *imu;
 		int16_t left_offset, right_offset;
@@ -71,7 +72,7 @@ class M5Bala {
 		float yaw, pitch, roll;
 		int16_t speed_input0, speed_input1;
 		int16_t pwm_out0, pwm_out1;
-		int8_t angle_offset;
+		float angle_offset;
 		uint32_t loop_interval;
 		float K1, K2, K3, K4, K5;
 		uint8_t imu_id;