Affordable Smart Wheelchair / IMU6050Ver11

a

IMU6050/IMUWheelchair.cpp@17:f2e2692762ac, 2019-07-09 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 09 23:36:51 2019 +0000
Revision:
17:f2e2692762ac
Parent:
15:b7cac36c4cce 
gg

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