Affordable Smart Wheelchair / Mbed OS Version1-2

Fully integrated ToF/IMU codes

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

wheelchairControlSideTof/IMU6050/IMUWheelchair.cpp@21:d1faccb96146, 2019-07-09 (annotated)

Committer:
isagmz
Date:
Tue Jul 09 17:52:32 2019 +0000
Revision:
21:d1faccb96146
Finished IMU side sensor code

Who changed what in which revision?

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