Affordable Smart Wheelchair / IMU6050Ver11

a

IMU6050/IMUWheelchair.cpp@13:d66766523196, 2019-07-03 (annotated)

Committer:
jahutchi
Date:
Wed Jul 03 22:09:33 2019 +0000
Revision:
13:d66766523196
Child:
14:365c1c1bf6ee
Functions are working but now figuring out the yaw

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jahutchi 13:d66766523196 1 #include "IMUWheelchair.h"
jahutchi 13:d66766523196 2 static 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() {
jahutchi 13:d66766523196 23 if(imu->testConnection()== 0)
jahutchi 13:d66766523196 24 printf("not connected\r\n");
jahutchi 13:d66766523196 25 else
jahutchi 13:d66766523196 26 printf("connected\r\n");
jahutchi 13:d66766523196 27
jahutchi 13:d66766523196 28 //timer
jahutchi 13:d66766523196 29 t->start();
jahutchi 13:d66766523196 30 }
jahutchi 13:d66766523196 31
jahutchi 13:d66766523196 32 //Get the x component of the angular acceleration from IMU. Stores the component
jahutchi 13:d66766523196 33 //in a float array
jahutchi 13:d66766523196 34 //Returns a double, the value of the x-acceleration (m/s^2)
jahutchi 13:d66766523196 35 double IMUWheelchair::accel_x() {
jahutchi 13:d66766523196 36 imu -> getAccelero(accelD); //Change the values in accelerationArray
jahutchi 13:d66766523196 37 return (double)accelData[0];
jahutchi 13:d66766523196 38 }
jahutchi 13:d66766523196 39
jahutchi 13:d66766523196 40 //Get the y component of the angular acceleration from IMU. Stores the component
jahutchi 13:d66766523196 41 //in a float array
jahutchi 13:d66766523196 42 //Returns a double, the value of the y-acceleration (m/s^2)
jahutchi 13:d66766523196 43 double IMUWheelchair::accel_y() {
jahutchi 13:d66766523196 44 imu -> getAccelero(accelD); //Change the values in accelerationArray
jahutchi 13:d66766523196 45 return (double)accelData[1];
jahutchi 13:d66766523196 46 }
jahutchi 13:d66766523196 47
jahutchi 13:d66766523196 48 //Get the z component of the angular acceleration from IMU. Stores the component
jahutchi 13:d66766523196 49 //in a float array
jahutchi 13:d66766523196 50 //Returns a double, the value of the z-acceleration (m/s^2)
jahutchi 13:d66766523196 51 double IMUWheelchair::accel_z() {
jahutchi 13:d66766523196 52 imu -> getAccelero(accelD); //Change the values in accelerationArray
jahutchi 13:d66766523196 53 return (double)accelData[2];
jahutchi 13:d66766523196 54 }
jahutchi 13:d66766523196 55
jahutchi 13:d66766523196 56 //Get the x component of the angular velocity from IMU's gyroscope. Stores the
jahutchi 13:d66766523196 57 //component in a float array
jahutchi 13:d66766523196 58 //Returns a double, the value of the x-angular velocity (rad/s)
jahutchi 13:d66766523196 59 double IMUWheelchair::gyro_x() {
jahutchi 13:d66766523196 60 imu -> getGyro(gyroD); //Change the values in gyroArray
jahutchi 13:d66766523196 61 return (double)gyroData[0];
jahutchi 13:d66766523196 62
jahutchi 13:d66766523196 63 }
jahutchi 13:d66766523196 64
jahutchi 13:d66766523196 65 //Get the y component of the angular velocity from IMU's gyroscope. Stores the
jahutchi 13:d66766523196 66 //component in a float array
jahutchi 13:d66766523196 67 //Returns a double, the value of the y-angular velocity (rad/s)
jahutchi 13:d66766523196 68 double IMUWheelchair::gyro_y() {
jahutchi 13:d66766523196 69 imu -> getGyro(gyroD); //Change the values in gyroArray
jahutchi 13:d66766523196 70 return (double)gyroData[1];
jahutchi 13:d66766523196 71
jahutchi 13:d66766523196 72 }
jahutchi 13:d66766523196 73
jahutchi 13:d66766523196 74 //Get the z component of the angular velocity from IMU's gyroscope. Stores the
jahutchi 13:d66766523196 75 //component in a float array
jahutchi 13:d66766523196 76 //Returns a double, the value of the z-angular velocity (rad/s)
jahutchi 13:d66766523196 77 double IMUWheelchair::gyro_z() {
jahutchi 13:d66766523196 78 imu -> getGyro(gyroD); //Change the values in gyroArray
jahutchi 13:d66766523196 79 return (double)gyroData[2];
jahutchi 13:d66766523196 80 }
jahutchi 13:d66766523196 81
jahutchi 13:d66766523196 82 //Get the yaw, or the angle turned at a certain time interval
jahutchi 13:d66766523196 83 //Return double, the angle or yaw, (degree)
jahutchi 13:d66766523196 84 double IMUWheelchair::yaw() {
jahutchi 13:d66766523196 85 float gyroZ = IMUWheelchair::gyro_z();
jahutchi 13:d66766523196 86 if(abs(gyroZ) >= .3) {
jahutchi 13:d66766523196 87 total_yaw = (total_yaw - t->read()*gyroZ);
jahutchi 13:d66766523196 88 }
jahutchi 13:d66766523196 89 t->reset();
jahutchi 13:d66766523196 90 if(total_yaw > 360)
jahutchi 13:d66766523196 91 total_yaw -= 360;
jahutchi 13:d66766523196 92 if(total_yaw < 0)
jahutchi 13:d66766523196 93 total_yaw += 360;
jahutchi 13:d66766523196 94 return (double)total_yaw;
jahutchi 13:d66766523196 95 }
jahutchi 13:d66766523196 96
jahutchi 13:d66766523196 97
jahutchi 13:d66766523196 98 double IMUWheelchair::pitch()
jahutchi 13:d66766523196 99 {
jahutchi 13:d66766523196 100 imu->getAccelero(accelD);
jahutchi 13:d66766523196 101 float pitch = atan2 (-accelData[1] ,( sqrt (accelData[0] * accelData[0]) +(accelData[2] *accelData[2])));
jahutchi 13:d66766523196 102 pitch = pitch*57.3;
jahutchi 13:d66766523196 103 return (double)pitch;
jahutchi 13:d66766523196 104 }
jahutchi 13:d66766523196 105
jahutchi 13:d66766523196 106 double IMUWheelchair::roll() {
jahutchi 13:d66766523196 107 imu->getAccelero(accelD);
jahutchi 13:d66766523196 108 float roll = atan2(-accelData[0] ,( sqrt((accelData[1] *accelData[1] ) +
jahutchi 13:d66766523196 109 (accelData[2] * accelData[2]))));
jahutchi 13:d66766523196 110 roll = roll*57.3;
jahutchi 13:d66766523196 111 t->reset();
jahutchi 13:d66766523196 112 return (double)roll;
jahutchi 13:d66766523196 113 }