projects/rc-micro-car
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

changed mpu6050 code to class for better abstraction

Browse Source
This commit is contained in:
willem oldemans
2020-10-01 22:21:59 +02:00
parent c7fb9ff84b
commit 48b8cefac8
7 changed files with 113 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
src/MPU6050.cpp
View File

@@ -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;
}

54
src/MPU6050.h
View File

@@ -1,15 +1,57 @@
#ifndef MPU6050H
#define MPU6050H
//#include "servos.h"
template <typename T>
struct xyzvector
{
T x;
T y;
T z;
};
void writeDebug();
void readMpu6050Data();
void setupMpu6050();
class _mpu
{
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 );
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

19
src/balancing.cpp
View File

@@ -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;

8
src/balancing.h
View File

@@ -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 );

5
src/main.cpp
View File

@@ -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();

8
src/motors.cpp
View File

@@ -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;

4
src/motors.h
View File

@@ -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);