changed mpu6050 code to class for better abstraction

src/MPU6050.cpp

@@ -25,35 +25,10 @@
     7ms loop time with 8MHz MCU clock. --> You can measure your loop time with loopDuration()
 */
 
-//
-// =======================================================================================================
-// GLOBAL VARIABLES
-// =======================================================================================================
-//
-
-// 6050 variables
-int gyro_x, gyro_y, gyro_z;
-long acc_x_raw, acc_y_raw, acc_z_raw;
-long acc_x, acc_y, acc_z, acc_total_vector;
-int temperature;
-long gyro_x_cal, gyro_y_cal, gyro_z_cal;
-long loop_timer;
-float angle_pitch, angle_roll;
-bool set_gyro_angles;
-float angle_roll_acc, angle_pitch_acc;
-float yaw_rate;
 
 
-// configuration variables (you may have to change them)
+void _mpu::print() 
-const int calibrationPasses = 500; // 500 is useful
+{
-
-//
-// =======================================================================================================
-// PRINT DEBUG DATA
-// =======================================================================================================
-//
-
-void writeDebug() {
 #ifdef DEBUG
   static unsigned long lastPrint;
   if (millis() - lastPrint >= 250) {
@@ -75,38 +50,39 @@ void writeDebug() {
 // =======================================================================================================
 //
 
-void processMpu6050Data( void ) {
+void _mpu::processData( void ) {
 
-    vehicleConfig* cfg = getConfigptr();
+  vehicleConfig* cfg = getConfigptr();
 
-  gyro_x -= gyro_x_cal;                                                // Subtract the offset calibration value from the raw gyro_x value
+  gyro.x -= gyro_cal.x;                                                // Subtract the offset calibration value from the raw gyro_x value
-  gyro_y -= gyro_y_cal;                                                // Subtract the offset calibration value from the raw gyro_y value
+  gyro.y -=  gyro_cal.y;                                                // Subtract the offset calibration value from the raw gyro_y value
-  gyro_z -= gyro_z_cal;                                                // Subtract the offset calibration value from the raw gyro_z value
+  gyro.z -= gyro_cal.z;                                                // Subtract the offset calibration value from the raw gyro_z value
 
   //Gyro angle calculations
   //0.0000611 * 4 = 1 / (125Hz / 65.5)
-  angle_pitch += gyro_x * 0.0004885;                                   // Calculate the traveled pitch angle and add this to the angle_pitch variable
+  angle_pitch += gyro.x * 0.0004885;                                   // Calculate the traveled pitch angle and add this to the angle_pitch variable
-  angle_roll += gyro_y * 0.0004885;                                    // Calculate the traveled roll angle and add this to the angle_roll variable
+  angle_roll += gyro.y * 0.0004885;                                    // Calculate the traveled roll angle and add this to the angle_roll variable
-  yaw_rate = gyro_z * 0.0004885;                                       // Yaw rate in degrees per second
+  yaw_rate = gyro.z * 0.0004885;                                       // Yaw rate in degrees per second
 
-  if (cfg->getvehicleType() == balancingthing) { // Those calculations are only required for the self balancing robot. Otherwise we can save some processing time.
+  if (cfg->getvehicleType() == balancingthing)  // Those calculations are only required for the self balancing robot. Otherwise we can save some processing time.
-    //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians (not required in this application)
+  {
+   //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians (not required in this application)
     //angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               // If the IMU has yawed transfer the roll angle to the pitch angle
     //angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               // If the IMU has yawed transfer the pitch angle to the roll angle
 
     //Accelerometer reading averaging to improve vibration resistance (added by TheDIYGuy999)
-    acc_x = (acc_x * 9 + acc_x_raw) / 10;
+    acc.x = (acc.x * 9 + acc_raw.x) / 10;
-    acc_y = (acc_y * 9 + acc_y_raw) / 10;
+    acc.y = (acc.y * 9 + acc_raw.y) / 10;
-    acc_z = (acc_z * 9 + acc_z_raw) / 10;
+    acc.z = (acc.z * 9 + acc_raw.z) / 10;
 
     //Accelerometer angle calculations
-    acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); // Calculate the total accelerometer vector
+    acc_total_vector = sqrt((acc.x * acc.x) + (acc.y * acc.y) + (acc.z * acc.z)); // Calculate the total accelerometer vector
     //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
-    if (abs(acc_y) < acc_total_vector) {                                      //Prevent the asin function to produce a NaN
+    if (abs(acc.y) < acc_total_vector) {                                      //Prevent the asin function to produce a NaN
-      angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;       //Calculate the pitch angle.
+      angle_pitch_acc = asin((float)acc.y / acc_total_vector) * 57.296;       //Calculate the pitch angle.
     }
-    if (abs(acc_x) < acc_total_vector) {                                      //Prevent the asin function to produce a NaN
+    if (abs(acc.x) < acc_total_vector) {                                      //Prevent the asin function to produce a NaN
-      angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;       //Calculate the roll angle.
+      angle_roll_acc = asin((float)acc.x / acc_total_vector) * -57.296;       //Calculate the roll angle.
     }
 
     if (set_gyro_angles) {                                               // If the IMU is already started (zero drift protection)
@@ -128,30 +104,31 @@ void processMpu6050Data( void ) {
 //
 
 // Sub function allows setup to call it without delay
-void readMpu6050Raw() {
+void _mpu::readRaw( void ) 
+{
   Wire.beginTransmission(0x68);                                        // Start communicating with the MPU-6050
   Wire.write(0x3B);                                                    // Send the requested starting register
   Wire.endTransmission();                                              // End the transmission
   Wire.requestFrom(0x68, 14);                                          // Request 14 bytes from the MPU-6050
   while (Wire.available() < 14);                                       // Wait until all the bytes are received
-  acc_x_raw = Wire.read() << 8 | Wire.read();                          // Add the low and high byte to the acc_x variable
+  acc_raw.x = Wire.read() << 8 | Wire.read();                          // Add the low and high byte to the acc_x variable
-  acc_y_raw = Wire.read() << 8 | Wire.read();                          // Add the low and high byte to the acc_y variable
+  acc_raw.y = Wire.read() << 8 | Wire.read();                          // Add the low and high byte to the acc_y variable
-  acc_z_raw = Wire.read() << 8 | Wire.read();                          // Add the low and high byte to the acc_z variable
+  acc_raw.z = Wire.read() << 8 | Wire.read();                          // Add the low and high byte to the acc_z variable
   temperature = Wire.read() << 8 | Wire.read();                        // Add the low and high byte to the temperature variable
-  gyro_x = Wire.read() << 8 | Wire.read();                             // Add the low and high byte to the gyro_x variable
+  gyro.x = Wire.read() << 8 | Wire.read();                             // Add the low and high byte to the gyro_x variable
-  gyro_y = Wire.read() << 8 | Wire.read();                             // Add the low and high byte to the gyro_y variable
+  gyro.y = Wire.read() << 8 | Wire.read();                             // Add the low and high byte to the gyro_y variable
-  gyro_z = Wire.read() << 8 | Wire.read();                             // Add the low and high byte to the gyro_z variable
+  gyro.z = Wire.read() << 8 | Wire.read();                             // Add the low and high byte to the gyro_z variable
 }
 
 // Main function
-void readMpu6050Data() {
+void _mpu::run() {
   static unsigned long lastReading;
   if (micros() - lastReading >= 8000) {                                // Read the data every 8000us (equals 125Hz)
     lastReading = micros();
 
-    readMpu6050Raw();                                                  // Read RAW data
+    readRaw();                                                  // Read RAW data
 
-    processMpu6050Data();                                              // Process the MPU 6050 data
+    processData();                                              // Process the MPU 6050 data
   }
 }
 
@@ -161,7 +138,7 @@ void readMpu6050Data() {
 // =======================================================================================================
 //
 
-void setupMpu6050() {
+void _mpu::begin() {
 
   vehicleConfig* cfg = getConfigptr();
 
@@ -205,17 +182,17 @@ void setupMpu6050() {
 #ifdef DEBUG
       if (cal_int % (calibrationPasses / 32) == 0)Serial.print(".");   // Print a dot every X readings
 #endif
-      readMpu6050Raw();                                                // Read the raw acc and gyro data from the MPU-6050
+      readRaw();                                                // Read the raw acc and gyro data from the MPU-6050
-      gyro_x_cal += gyro_x;                                            // Add the gyro x-axis offset to the gyro_x_cal variable
+      gyro_cal.x += gyro.x;                                            // Add the gyro x-axis offset to the gyro_x_cal variable
-      gyro_y_cal += gyro_y;                                            // Add the gyro y-axis offset to the gyro_y_cal variable
+      gyro_cal.y += gyro.y;                                            // Add the gyro y-axis offset to the gyro_y_cal variable
-      gyro_z_cal += gyro_z;                                            // Add the gyro z-axis offset to the gyro_z_cal variable
+      gyro_cal.z += gyro.z;                                            // Add the gyro z-axis offset to the gyro_z_cal variable
       lastGyroCal = micros();
       cal_int ++;
     }
   }
-  gyro_x_cal /= calibrationPasses;                                      // Divide the gyro_x_cal variable by X to get the avarage offset
+  gyro_cal.x /= calibrationPasses;                                      // Divide the gyro_x_cal variable by X to get the avarage offset
-  gyro_y_cal /= calibrationPasses;                                      // Divide the gyro_y_cal variable by X to get the avarage offset
+  gyro_cal.y /= calibrationPasses;                                      // Divide the gyro_y_cal variable by X to get the avarage offset
-  gyro_z_cal /= calibrationPasses;                                      // Divide the gyro_z_cal variable by X to get the avarage offset
+  gyro_cal.z /= calibrationPasses;                                      // Divide the gyro_z_cal variable by X to get the avarage offset
 
 #ifdef DEBUG
 
@@ -228,14 +205,4 @@ void setupMpu6050() {
 
   Serial.println("done!");
 #endif
-}
-
-float Mpu6050_getangle_pitch( void )
-{
-  return angle_pitch;
-}
-
-float Mpu6050_getyaw_rate( void )
-{
-  return yaw_rate;
 }

src/MPU6050.h

@@ -1,15 +1,57 @@
 #ifndef MPU6050H
 #define MPU6050H
 
-//#include "servos.h"
+template <typename T>
+struct xyzvector
+{
+	T x;
+	T y;
+	T z;
+};
 
-void writeDebug();
+class _mpu
-void readMpu6050Data();
+{
-void setupMpu6050();
+	xyzvector<int> gyro;
+	xyzvector<long> acc_raw;
+	xyzvector<long> acc;
+	long acc_total_vector;
+	
+	int temperature;
+	xyzvector<long> gyro_cal;
 
-float Mpu6050_getangle_pitch( void );
+	long loop_timer;
-float Mpu6050_getyaw_rate( void );
+	float angle_pitch;
+	float angle_roll;
+	bool set_gyro_angles;
+	float angle_roll_acc;
+	float angle_pitch_acc;
+	float yaw_rate;
+	
+	const int calibrationPasses = 500;
 
+	void readRaw( void );
 
+public:
+	_mpu(){}
+
+	void processData( void );
+
+	void run( void );
+
+	void begin( void );
+
+	float getangle_pitch( void )
+	{
+		return angle_pitch;
+	}
+
+	float getyaw_rate( void )
+	{
+		return yaw_rate;
+	}
+
+	void print( void );
+
+};
 
 #endif //MPU6050H

src/balancing.cpp

@@ -54,14 +54,14 @@ void setupPid() {
 // =======================================================================================================
 //
 
-void balancing() {
+void balancing(_mpu* mpu) {
 
   vehicleConfig* cfg = getConfigptr();
   RcData* data = getDataptr();
   // Read sensor data
-  readMpu6050Data();
+  mpu->run();
 
-  angleMeasured = Mpu6050_getangle_pitch() - cfg->gettiltCalibration();
+  angleMeasured = mpu->getangle_pitch() - cfg->gettiltCalibration();
 
   // Read speed pot with 0.2s fader
   static unsigned long lastPot;
@@ -91,7 +91,7 @@ void balancing() {
   anglePid.Compute();
 
   // Send the calculated values to the motors
-  driveMotorsBalancing();
+  driveMotorsBalancing(mpu);
 
   // Display PID variables on the transmitter OLED (for debugging only, comment out the corresponding variables in checkBattery() in this case)
   //loopDuration(); // compute the loop time
@@ -108,13 +108,14 @@ void balancing() {
 // =======================================================================================================
 // For cars with stability control (steering overlay depending on gyro yaw rate)
 
-void mrsc() {
+void mrsc(_mpu* mpu) 
+{
 
   vehicleConfig* cfg = getConfigptr();
   RcData* data = getDataptr();
 
   // Read sensor data
-  readMpu6050Data();
+  mpu->run();
 
   // If the MRSC gain is a fixed value, read it!
 #ifdef MRSC_FIXED
@@ -123,7 +124,7 @@ void mrsc() {
 
   // Compute steering compensation overlay
   int turnRateSetPoint = data->axis1 - 50;  // turnRateSetPoint = steering angle (0 to 100) - 50 = -50 to 50
-  int turnRateMeasured = Mpu6050_getyaw_rate() * abs(data->axis3 - 50); // degrees/s * speed
+  int turnRateMeasured = mpu->getyaw_rate() * abs(data->axis3 - 50); // degrees/s * speed
   int steeringAngle = turnRateSetPoint + (turnRateMeasured * data->pot / 100);  // Compensation depending on the pot value
 
   steeringAngle = constrain (steeringAngle, -50, 50); // range = -50 to 50
@@ -143,13 +144,13 @@ void mrsc() {
 }
 
 
-void driveMotorsBalancing() 
+void driveMotorsBalancing(_mpu* mpu) 
 {
   vehicleConfig* cfg = getConfigptr();
   RcData* data = getDataptr();
 
   // The steering overlay is in degrees per second, controlled by the MPU 6050 yaw rate and yoystick axis 1
-  int steering = ((data->axis1 - 50) / 7) - Mpu6050_getyaw_rate(); // -50 to 50 / 8 = 7°/s - yaw_rate
+  int steering = ((data->axis1 - 50) / 7) - mpu->getyaw_rate(); // -50 to 50 / 8 = 7°/s - yaw_rate
   steering = constrain(steering, -7, 7);
   int speed = angleOutput + 50;
 

src/balancing.h

@@ -1,10 +1,12 @@
 #ifndef BALANCINGH
 #define BALANCINGH
 
-void balancing( void );
+#include "MPU6050.h"
-void driveMotorsBalancing( void );
+
+void balancing( _mpu* mpu );
+void driveMotorsBalancing( _mpu* mpu  );
 void setupPid( void );
-void mrsc();
+void mrsc(_mpu* mpu);
 
 double getAngleOut( void );
 

src/main.cpp

@@ -39,6 +39,7 @@ const float codeVersion = 3.4; // Software revision (see https://github.com/TheD
 
 radio_c radio(CLIENT_ADDRESS,PIN_NRF_CE, PIN_NRF_CSN);
 
+_mpu mpu;
 
 
 void setup() 
@@ -91,7 +92,7 @@ void setup()
   if (cfg->usesMPU()) // Only for self balancing vehicles and cars with MRSC
   { 
     // setup MPU 6050 accelerometer / gyro setup
-    setupMpu6050();
+    mpu.begin();
 
     // PID controller setup
     setupPid();
@@ -150,7 +151,7 @@ void loop() {
   writeServos();
 
   // Drive the motors
-  drive();
+  drive(&mpu);
 
   // Battery check
   checkBattery();

src/motors.cpp

@@ -6,7 +6,7 @@
 #include "lights.h"
 #include "steeringCurves.h"
 #include "balancing.h"
-
+#include "MPU6050.h"
 
 #include "MC34933.h"
 #include "PWMFrequency.h"
@@ -121,7 +121,7 @@ bool motorBrakeActive(motor_t motor)
   return getMotorptr(motor)->brakeActive();
 }
 
-void drive()
+void drive(_mpu* mpu)
 {
   vehicleConfig* cfg = getConfigptr();
 
@@ -135,7 +135,7 @@ void drive()
 
     case carwithMRSC:
     {
-      mrsc(); // Car with MSRC stabilty control
+      mrsc(mpu); // Car with MSRC stabilty control
     }
     break;
 
@@ -147,7 +147,7 @@ void drive()
 
     case balancingthing:
     {
-      balancing(); // Self balancing robot
+      balancing(mpu); // Self balancing robot
     }
     break;
 

src/motors.h

@@ -1,6 +1,8 @@
 #ifndef MOTORSH
 #define MOTORSH
 
+#include "MPU6050.h"
+
 #include "MC34933.h" 
 
 typedef enum motor_e
@@ -21,7 +23,7 @@ void setupMotors();
 void driveMotorsCar();
 void driveMotorsForklift();
 void driveMotorsSteering();
-void drive();
+void drive(_mpu* mpu);