Sheila Pham / Mbed OS IMU6050Ver1

Added for gyro and testing

IMU6050/IMUWheelchair.cpp@14:365c1c1bf6ee, 2019-07-04 (annotated)

Committer:
sepham
Date:
Thu Jul 04 18:25:35 2019 +0000
Revision:
14:365c1c1bf6ee
Parent:
13:d66766523196 
IMU tof more gyro and testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jahutchi 13:d66766523196 1 #include "IMUWheelchair.h"
sepham 14:365c1c1bf6ee 2 float total_yaw;
jahutchi 13:d66766523196 3
jahutchi 13:d66766523196 4
jahutchi 13:d66766523196 5 IMUWheelchair::IMUWheelchair(Serial* out, Timer* time)
jahutchi 13:d66766523196 6 {
jahutchi 13:d66766523196 7 imu = new MPU6050(SDA, SCL);
jahutchi 13:d66766523196 8 usb = out;
jahutchi 13:d66766523196 9 t = time;
jahutchi 13:d66766523196 10 start = false;
jahutchi 13:d66766523196 11 }
jahutchi 13:d66766523196 12
jahutchi 13:d66766523196 13 IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){
jahutchi 13:d66766523196 14 usb = out;
jahutchi 13:d66766523196 15 t = time;
jahutchi 13:d66766523196 16 imu = new MPU6050(sda_pin, scl_pin);
jahutchi 13:d66766523196 17 IMUWheelchair::setup();
jahutchi 13:d66766523196 18 accelD = accelData;
jahutchi 13:d66766523196 19 gyroD = gyroData;
jahutchi 13:d66766523196 20 }
jahutchi 13:d66766523196 21
jahutchi 13:d66766523196 22 void IMUWheelchair::setup() {
sepham 14:365c1c1bf6ee 23 imu->setGyroRange(MPU6050_GYRO_RANGE_500);
sepham 14:365c1c1bf6ee 24
jahutchi 13:d66766523196 25 if(imu->testConnection()== 0)
jahutchi 13:d66766523196 26 printf("not connected\r\n");
jahutchi 13:d66766523196 27 else
jahutchi 13:d66766523196 28 printf("connected\r\n");
jahutchi 13:d66766523196 29
jahutchi 13:d66766523196 30 //timer
jahutchi 13:d66766523196 31 t->start();
sepham 14:365c1c1bf6ee 32
jahutchi 13:d66766523196 33 }
jahutchi 13:d66766523196 34
jahutchi 13:d66766523196 35 //Get the x component of the angular acceleration from IMU. Stores the component
jahutchi 13:d66766523196 36 //in a float array
jahutchi 13:d66766523196 37 //Returns a double, the value of the x-acceleration (m/s^2)
jahutchi 13:d66766523196 38 double IMUWheelchair::accel_x() {
jahutchi 13:d66766523196 39 imu -> getAccelero(accelD); //Change the values in accelerationArray
sepham 14:365c1c1bf6ee 40 return (double)accelD[0];
jahutchi 13:d66766523196 41 }
jahutchi 13:d66766523196 42
jahutchi 13:d66766523196 43 //Get the y component of the angular acceleration from IMU. Stores the component
jahutchi 13:d66766523196 44 //in a float array
jahutchi 13:d66766523196 45 //Returns a double, the value of the y-acceleration (m/s^2)
jahutchi 13:d66766523196 46 double IMUWheelchair::accel_y() {
jahutchi 13:d66766523196 47 imu -> getAccelero(accelD); //Change the values in accelerationArray
sepham 14:365c1c1bf6ee 48 return (double)accelD[1];
jahutchi 13:d66766523196 49 }
jahutchi 13:d66766523196 50
jahutchi 13:d66766523196 51 //Get the z component of the angular acceleration from IMU. Stores the component
jahutchi 13:d66766523196 52 //in a float array
jahutchi 13:d66766523196 53 //Returns a double, the value of the z-acceleration (m/s^2)
jahutchi 13:d66766523196 54 double IMUWheelchair::accel_z() {
jahutchi 13:d66766523196 55 imu -> getAccelero(accelD); //Change the values in accelerationArray
sepham 14:365c1c1bf6ee 56 return (double)accelD[2];
jahutchi 13:d66766523196 57 }
jahutchi 13:d66766523196 58
jahutchi 13:d66766523196 59 //Get the x component of the angular velocity from IMU's gyroscope. Stores the
jahutchi 13:d66766523196 60 //component in a float array
jahutchi 13:d66766523196 61 //Returns a double, the value of the x-angular velocity (rad/s)
jahutchi 13:d66766523196 62 double IMUWheelchair::gyro_x() {
sepham 14:365c1c1bf6ee 63
sepham 14:365c1c1bf6ee 64 imu->getGyro(gyroD); //Change the values in gyroArray
sepham 14:365c1c1bf6ee 65 return (double)gyroD[0];
jahutchi 13:d66766523196 66
jahutchi 13:d66766523196 67 }
jahutchi 13:d66766523196 68
jahutchi 13:d66766523196 69 //Get the y component of the angular velocity from IMU's gyroscope. Stores the
jahutchi 13:d66766523196 70 //component in a float array
jahutchi 13:d66766523196 71 //Returns a double, the value of the y-angular velocity (rad/s)
jahutchi 13:d66766523196 72 double IMUWheelchair::gyro_y() {
jahutchi 13:d66766523196 73 imu -> getGyro(gyroD); //Change the values in gyroArray
sepham 14:365c1c1bf6ee 74 return (double)gyroD[1];
jahutchi 13:d66766523196 75
jahutchi 13:d66766523196 76 }
jahutchi 13:d66766523196 77
jahutchi 13:d66766523196 78 //Get the z component of the angular velocity from IMU's gyroscope. Stores the
jahutchi 13:d66766523196 79 //component in a float array
jahutchi 13:d66766523196 80 //Returns a double, the value of the z-angular velocity (rad/s)
jahutchi 13:d66766523196 81 double IMUWheelchair::gyro_z() {
jahutchi 13:d66766523196 82 imu -> getGyro(gyroD); //Change the values in gyroArray
sepham 14:365c1c1bf6ee 83 return (double)gyroD[2];
jahutchi 13:d66766523196 84 }
jahutchi 13:d66766523196 85
sepham 14:365c1c1bf6ee 86
sepham 14:365c1c1bf6ee 87
jahutchi 13:d66766523196 88 //Get the yaw, or the angle turned at a certain time interval
jahutchi 13:d66766523196 89 //Return double, the angle or yaw, (degree)
jahutchi 13:d66766523196 90 double IMUWheelchair::yaw() {
jahutchi 13:d66766523196 91 float gyroZ = IMUWheelchair::gyro_z();
jahutchi 13:d66766523196 92 if(abs(gyroZ) >= .3) {
sepham 14:365c1c1bf6ee 93 printf("t->read(): %lf", t->read());
sepham 14:365c1c1bf6ee 94 total_yaw = total_yaw - t->read()*gyroZ;
sepham 14:365c1c1bf6ee 95
sepham 14:365c1c1bf6ee 96 printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
jahutchi 13:d66766523196 97 }
jahutchi 13:d66766523196 98 t->reset();
jahutchi 13:d66766523196 99 if(total_yaw > 360)
jahutchi 13:d66766523196 100 total_yaw -= 360;
jahutchi 13:d66766523196 101 if(total_yaw < 0)
jahutchi 13:d66766523196 102 total_yaw += 360;
jahutchi 13:d66766523196 103 return (double)total_yaw;
jahutchi 13:d66766523196 104 }
jahutchi 13:d66766523196 105
jahutchi 13:d66766523196 106
jahutchi 13:d66766523196 107 double IMUWheelchair::pitch()
jahutchi 13:d66766523196 108 {
jahutchi 13:d66766523196 109 imu->getAccelero(accelD);
sepham 14:365c1c1bf6ee 110 float pitch = atan2 (-accelD[1] ,( sqrt (accelD[0] * accelD[0]) +(accelD[2] *accelD[2])));
jahutchi 13:d66766523196 111 pitch = pitch*57.3;
jahutchi 13:d66766523196 112 return (double)pitch;
jahutchi 13:d66766523196 113 }
jahutchi 13:d66766523196 114
jahutchi 13:d66766523196 115 double IMUWheelchair::roll() {
jahutchi 13:d66766523196 116 imu->getAccelero(accelD);
sepham 14:365c1c1bf6ee 117 float roll = atan2(-accelD[0] ,( sqrt((accelD[1] *accelD[1] ) +
sepham 14:365c1c1bf6ee 118 (accelD[2] * accelD[2]))));
jahutchi 13:d66766523196 119 roll = roll*57.3;
jahutchi 13:d66766523196 120 t->reset();
jahutchi 13:d66766523196 121 return (double)roll;
jahutchi 13:d66766523196 122 }