From 48b8cefac8e865f7bb2afe76ba62b09edba5cb8f Mon Sep 17 00:00:00 2001 From: willem oldemans Date: Thu, 1 Oct 2020 22:21:59 +0200 Subject: [PATCH] changed mpu6050 code to class for better abstraction --- src/MPU6050.cpp | 113 ++++++++++++++++------------------------------ src/MPU6050.h | 54 +++++++++++++++++++--- src/balancing.cpp | 19 ++++---- src/balancing.h | 8 ++-- src/main.cpp | 5 +- src/motors.cpp | 8 ++-- src/motors.h | 4 +- 7 files changed, 113 insertions(+), 98 deletions(-) diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp index 2a0c94d..033c605 100644 --- a/src/MPU6050.cpp +++ b/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) -const int calibrationPasses = 500; // 500 is useful - -// -// ======================================================================================================= -// PRINT DEBUG DATA -// ======================================================================================================= -// - -void writeDebug() { +void _mpu::print() +{ #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_y -= gyro_y_cal; // 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.x -= gyro_cal.x; // Subtract the offset calibration value from the raw gyro_x value + gyro.y -= gyro_cal.y; // Subtract the offset calibration value from the raw gyro_y 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_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 + 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 + 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. - //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians (not required in this application) + 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) //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_y = (acc_y * 9 + acc_y_raw) / 10; - acc_z = (acc_z * 9 + acc_z_raw) / 10; + acc.x = (acc.x * 9 + acc_raw.x) / 10; + acc.y = (acc.y * 9 + acc_raw.y) / 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 - angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296; //Calculate the pitch angle. + 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. } - 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. + 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. } 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_y_raw = 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.x = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_x variable + acc_raw.y = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_y 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_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.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.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 - gyro_x_cal += 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_z_cal += gyro_z; // Add the gyro z-axis offset to the gyro_z_cal variable + readRaw(); // Read the raw acc and gyro data from the MPU-6050 + gyro_cal.x += gyro.x; // Add the gyro x-axis offset to the gyro_x_cal variable + gyro_cal.y += gyro.y; // Add the gyro y-axis offset to the gyro_y_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_y_cal /= 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.x /= calibrationPasses; // Divide the gyro_x_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_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; } \ No newline at end of file diff --git a/src/MPU6050.h b/src/MPU6050.h index 12f2054..4affe7d 100644 --- a/src/MPU6050.h +++ b/src/MPU6050.h @@ -1,15 +1,57 @@ #ifndef MPU6050H #define MPU6050H -//#include "servos.h" +template +struct xyzvector +{ + T x; + T y; + T z; +}; -void writeDebug(); -void readMpu6050Data(); -void setupMpu6050(); +class _mpu +{ + xyzvector gyro; + xyzvector acc_raw; + xyzvector acc; + long acc_total_vector; + + int temperature; + xyzvector gyro_cal; -float Mpu6050_getangle_pitch( void ); -float Mpu6050_getyaw_rate( void ); + long loop_timer; + 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 \ No newline at end of file diff --git a/src/balancing.cpp b/src/balancing.cpp index dbc2b26..afd23dd 100644 --- a/src/balancing.cpp +++ b/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; diff --git a/src/balancing.h b/src/balancing.h index 701ee29..0616cc2 100644 --- a/src/balancing.h +++ b/src/balancing.h @@ -1,10 +1,12 @@ #ifndef BALANCINGH #define BALANCINGH -void balancing( void ); -void driveMotorsBalancing( void ); +#include "MPU6050.h" + +void balancing( _mpu* mpu ); +void driveMotorsBalancing( _mpu* mpu ); void setupPid( void ); -void mrsc(); +void mrsc(_mpu* mpu); double getAngleOut( void ); diff --git a/src/main.cpp b/src/main.cpp index 21edd6e..b509588 100644 --- a/src/main.cpp +++ b/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(); diff --git a/src/motors.cpp b/src/motors.cpp index 4681c0c..77a6aa3 100644 --- a/src/motors.cpp +++ b/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; diff --git a/src/motors.h b/src/motors.h index f1a280b..337b2b3 100644 --- a/src/motors.h +++ b/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);