Acceleration with encoder and IMU

Code/STM32/Core/Src/Components/Robot.cpp

@@ -18,7 +18,7 @@ extern TIM_HandleTypeDef htim4;
 extern I2C_HandleTypeDef hi2c1;
 extern SerialDebug debug;
 
-uint8_t buf[64];
+uint8_t buf[92];
 
 Robot::Robot() {
 
@@ -47,12 +47,23 @@ void Robot::controlCallback(){
 	motor0.pid(convertedSpeed0);
 	motor1.pid(convertedSpeed1);
 #endif
-	sprintf((char*)buf, ">Speed:%+0.6e,%+0.6e,%+0.6e,%+0.6e",
+	imu0.update();
+	float imux = imu0.getAccX();
+	float imuy = imu0.getAccY();
+	if (isnan(imux)){
+		imux = 0;
+	}
+	if (isnan(imuy)){
+		imuy = 0;
+	}
+	sprintf((char*)buf, ">Speed:%+0.6e,%+0.6e,%+0.6e,%+0.6e,%+0.6e,%+0.6e",
 			motor0.getCurrentRadPerSecond()/CONVERSION,
 			motor1.getCurrentRadPerSecond()/CONVERSION,
 			desiredSpeed0,
-			desiredSpeed1);
-	HAL_UART_Transmit_DMA(&huart1, buf, 64);
+			desiredSpeed1,
+			imux,
+			imuy);
+	HAL_UART_Transmit_DMA(&huart1, buf, 92);
 }
 
 void Robot::print_roll_pitch_yaw() {