![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Header file and functions
Revision 13:d66766523196, committed 2019-07-03
- Comitter:
- jahutchi
- Date:
- Wed Jul 03 22:09:33 2019 +0000
- Parent:
- 12:6efce6d008f8
- Commit message:
- Functions are working but now figuring out the yaw
Changed in this revision
--- a/IMU6050/IMU6050.cpp Wed Jul 03 17:56:22 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,79 +0,0 @@ -#include "IMU6050.h" -IMU6050::IMU6050(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){ - accelD = accelData; - gyroD = gyroData; -} - -void IMU6050::setup() { - -} - -//Get the x component of the angular acceleration from IMU. Stores the component -//in a float array -//Returns a double, the value of the x-acceleration (m/s^2) -double IMU6050::accel_x() { - imu -> getAccelero(accelD); //Change the values in accelerationArray - return (double)accelData[0]; -} - -//Get the y component of the angular acceleration from IMU. Stores the component -//in a float array -//Returns a double, the value of the y-acceleration (m/s^2) -double IMU6050::accel_y() { - imu -> getAccelero(accelD); //Change the values in accelerationArray - return (double)accelData[1]; -} - -//Get the z component of the angular acceleration from IMU. Stores the component -//in a float array -//Returns a double, the value of the z-acceleration (m/s^2) -double IMU6050::accel_z() { - imu -> getAccelero(accelD); //Change the values in accelerationArray - return (double)accelData[2]; -} - -//Get the x component of the angular velocity from IMU's gyroscope. Stores the -//component in a float array -//Returns a double, the value of the x-angular velocity (rad/s) -double IMU6050::gyro_x() { - imu -> getGyro(gyroD); //Change the values in gyroArray - return (double)gyroData[0]; - -} - -//Get the y component of the angular velocity from IMU's gyroscope. Stores the -//component in a float array -//Returns a double, the value of the y-angular velocity (rad/s) -double IMU6050::gyro_y() { - imu -> getGyro(gyroD); //Change the values in gyroArray - return (double)gyroData[1]; - -} - -//Get the z component of the angular velocity from IMU's gyroscope. Stores the -//component in a float array -//Returns a double, the value of the z-angular velocity (rad/s) -double IMU6050::gyro_z() { - imu -> getGyro(gyroD); //Change the values in gyroArray - return (double)gyroData[2]; -} - -//Get the yaw, or the angle turned at a certain time interval -//Return double, the angle or yaw, (degree) - -/* -double IMU6050::yaw() { - - imu -> computeAngle(); - return -} - -double IMU6050::pitch() { - -} - -double IMU6050::roll() { - -} - -*/ \ No newline at end of file
--- a/IMU6050/IMU6050.h Wed Jul 03 17:56:22 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,74 +0,0 @@ -#ifndef IMU6050_H -#define IMU6050_H - -#include "filter.h" -//#include "mbed.h" -#include "math.h" -#include <MPU6050.h> - -#define PI 3.141593 - -/*#define SDA D14 -#define SCL D15*/ -#define SDA PB_9 -#define SCL PB_8 -#define SAMPLEFREQ 50 -#define CAL_TIME 3 - -class IMU6050 { - public: - //The constructor for this class - IMU6050(Serial* out, Timer* time); - IMU6050(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time); - - //Set up the IMU, check if it connects - void setup(); - - //Get the x-component of the angular acceleration - double accel_x(); - - //Get the y-component of the angular acceleration - double accel_y(); - - //Get the z-component of the angular acceleration - double accel_z(); - - //Get the x-component of gyro, angular velocity - double gyro_x(); - - //Get the y-component of gyro, angular velocity - double gyro_y(); - - //Get the z-component of gyro, angular velocity - double gyro_z(); - - //Magnometer to find angle relative to North to compare to gyroscope - //double angle_north(); - - //Get the YAW, or angle (theta), direction facing - double yaw(); - - //Get the pitch, (Up and down component) - double pitch(); - - //Get the roll, the tilt - double roll(); - - MPU6050* imu; //The IMU we're testing from, MPU6050 - - private: - Serial* usb; //the connection port - Timer* t;//to calculate the time - float accelData[3]; // stores the angular acceleration component - float gyroData[3]; //stores the gyro data x,y,z - float* accelD; //Pointer that points to either accelData - float* gyroD; //Ptr to the gyroData array - - float angleData[3]; //Contains the pitch, roll, yaw angle - float* angleD;//Ptr to angleData array - - void calibrate_yaw(); - -}; - -#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU6050/IMUWheelchair.cpp Wed Jul 03 22:09:33 2019 +0000 @@ -0,0 +1,113 @@ +#include "IMUWheelchair.h" +static float total_yaw; + + +IMUWheelchair::IMUWheelchair(Serial* out, Timer* time) +{ + imu = new MPU6050(SDA, SCL); + usb = out; + t = time; + start = false; +} + +IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){ + usb = out; + t = time; + imu = new MPU6050(sda_pin, scl_pin); + IMUWheelchair::setup(); + accelD = accelData; + gyroD = gyroData; +} + +void IMUWheelchair::setup() { + if(imu->testConnection()== 0) + printf("not connected\r\n"); + else + printf("connected\r\n"); + + //timer + t->start(); +} + +//Get the x component of the angular acceleration from IMU. Stores the component +//in a float array +//Returns a double, the value of the x-acceleration (m/s^2) +double IMUWheelchair::accel_x() { + imu -> getAccelero(accelD); //Change the values in accelerationArray + return (double)accelData[0]; +} + +//Get the y component of the angular acceleration from IMU. Stores the component +//in a float array +//Returns a double, the value of the y-acceleration (m/s^2) +double IMUWheelchair::accel_y() { + imu -> getAccelero(accelD); //Change the values in accelerationArray + return (double)accelData[1]; +} + +//Get the z component of the angular acceleration from IMU. Stores the component +//in a float array +//Returns a double, the value of the z-acceleration (m/s^2) +double IMUWheelchair::accel_z() { + imu -> getAccelero(accelD); //Change the values in accelerationArray + return (double)accelData[2]; +} + +//Get the x component of the angular velocity from IMU's gyroscope. Stores the +//component in a float array +//Returns a double, the value of the x-angular velocity (rad/s) +double IMUWheelchair::gyro_x() { + imu -> getGyro(gyroD); //Change the values in gyroArray + return (double)gyroData[0]; + +} + +//Get the y component of the angular velocity from IMU's gyroscope. Stores the +//component in a float array +//Returns a double, the value of the y-angular velocity (rad/s) +double IMUWheelchair::gyro_y() { + imu -> getGyro(gyroD); //Change the values in gyroArray + return (double)gyroData[1]; + +} + +//Get the z component of the angular velocity from IMU's gyroscope. Stores the +//component in a float array +//Returns a double, the value of the z-angular velocity (rad/s) +double IMUWheelchair::gyro_z() { + imu -> getGyro(gyroD); //Change the values in gyroArray + return (double)gyroData[2]; +} + +//Get the yaw, or the angle turned at a certain time interval +//Return double, the angle or yaw, (degree) +double IMUWheelchair::yaw() { + float gyroZ = IMUWheelchair::gyro_z(); + if(abs(gyroZ) >= .3) { + total_yaw = (total_yaw - t->read()*gyroZ); + } + t->reset(); + if(total_yaw > 360) + total_yaw -= 360; + if(total_yaw < 0) + total_yaw += 360; + return (double)total_yaw; +} + + +double IMUWheelchair::pitch() + { + imu->getAccelero(accelD); + float pitch = atan2 (-accelData[1] ,( sqrt (accelData[0] * accelData[0]) +(accelData[2] *accelData[2]))); + pitch = pitch*57.3; + return (double)pitch; +} + +double IMUWheelchair::roll() { + imu->getAccelero(accelD); + float roll = atan2(-accelData[0] ,( sqrt((accelData[1] *accelData[1] ) + + (accelData[2] * accelData[2])))); + roll = roll*57.3; + t->reset(); + return (double)roll; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU6050/IMUWheelchair.h Wed Jul 03 22:09:33 2019 +0000 @@ -0,0 +1,76 @@ +#ifndef IMUWheelchair_H +#define IMUWheelchair_H + +#include "filter.h" +//#include "mbed.h" +#include "math.h" +#include <MPU6050.h> + +#define PI 3.141593 + +/*#define SDA D14 +#define SCL D15*/ +#define SDA PB_9 +#define SCL PB_8 +#define SAMPLEFREQ 50 +#define CAL_TIME 3 + +class IMUWheelchair { + public: + //The constructor for this class + IMUWheelchair(Serial* out, Timer* time); + IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time); + + //Set up the IMU, check if it connects + void setup(); + + //Get the x-component of the angular acceleration + double accel_x(); + + //Get the y-component of the angular acceleration + double accel_y(); + + //Get the z-component of the angular acceleration + double accel_z(); + + //Get the x-component of gyro, angular velocity + double gyro_x(); + + //Get the y-component of gyro, angular velocity + double gyro_y(); + + //Get the z-component of gyro, angular velocity + double gyro_z(); + + //Magnometer to find angle relative to North to compare to gyroscope + //double angle_north(); + + //Get the YAW, or angle (theta), direction facing + double yaw(); + + //Get the pitch, (Up and down component) + double pitch(); + + //Get the roll, the tilt + double roll(); + + MPU6050* imu; //The IMU we're testing from, MPU6050 + + private: + Serial* usb; //the connection port + Timer* t;//to calculate the time + float accelData[3]; // stores the angular acceleration component + float gyroData[3]; //stores the gyro data x,y,z + float* accelD; //Pointer that points to either accelData + float* gyroD; //Ptr to the gyroData array + + float angleData[3]; //Contains the pitch, roll, yaw angle + float* angleD;//Ptr to angleData array + + void calibrate_yaw(); + + bool start; + +}; + +#endif \ No newline at end of file
--- a/MPU6050.cpp Wed Jul 03 17:56:22 2019 +0000 +++ b/MPU6050.cpp Wed Jul 03 22:09:33 2019 +0000 @@ -207,7 +207,7 @@ data[2]=(float)temp[2] / 3752.9; } if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ - data[0]=(float)temp[0] / 1879.3;; + data[0]=(float)temp[0] / 1879.3; data[1]=(float)temp[1] / 1879.3; data[2]=(float)temp[2] / 1879.3; } @@ -306,7 +306,7 @@ this->write(MPU6050_RA_INT_ENABLE, temp); } -///function for disbling interrupts on MPU6050 INT pin, when the data is ready to take +///function for disabling interrupts on MPU6050 INT pin, when the data is ready to take void MPU6050::disableInt ( void ){ char temp; temp = this->read(MPU6050_RA_INT_ENABLE);
--- a/main.cpp Wed Jul 03 17:56:22 2019 +0000 +++ b/main.cpp Wed Jul 03 22:09:33 2019 +0000 @@ -1,17 +1,24 @@ #include <MPU6050.h> -#include "IMU6050.h" +#include "IMUWheelchair.h" MPU6050 IMU(D14, D15); Serial pc(USBTX, USBRX, 57600); //Serial Monitor -float dataA[3]; -float* data = dataA; +float dataGyro[3]; +float *dataG = dataGyro; + +float dataYaw[3]; +float *dataY = dataYaw; + int main() { IMU.setGyroRange(MPU6050_GYRO_RANGE_500); pc.printf("Imu test connections %d\r\n", IMU.testConnection()); while(1){ - IMU.getGyro(data); - pc.printf("x: %f, y: %f, z: %f\r\n", dataA[0]*180/3.14, dataA[1]*180/3.14, dataA[2]*180/3.14); + IMU.getGyro(dataG); + // pc.printf("x: %f, y: %f, z: %f\r\n", dataGyro[0]*180/3.14, dataGyro[1]*180/3.14, dataGyro[2]*180/3.14); wait(.2); + + IMU.getAcceleroAngle(dataY); + pc.printf("x: %f, y: %f, z: %f\r\n", dataG[0], dataG[1], dataG[2]); } }