Revision 21:d1faccb96146, committed 2019-07-09

Comitter:

isagmz

Date:

Tue Jul 09 17:52:32 2019 +0000

Parent:

20:3c1b58654e67

Commit message:

Finished IMU side sensor code

Changed in this revision

diff -r 3c1b58654e67 -r d1faccb96146 main.cpp
--- a/main.cpp	Tue Jul 02 21:17:03 2019 +0000
+++ b/main.cpp	Tue Jul 09 17:52:32 2019 +0000
@@ -47,9 +47,12 @@
 &sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12};                 // Puts ToF sensor pointers into an array
 VL53L1X** ToFT = ToF;
 
+Timer imuTimer;   
+IMUWheelchair IMU(&pc,&imuTimer); //initialize IMU
+
 Timer t;                                // Initialize time object t
 EventQueue queue;                       // Class to organize threads
-Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT);                    // Initialize wheelchair object
+Wheelchair smart(xDir,yDir, &pc, &t, &imuTimer, &wheel, &wheelS, ToFT);                    // Initialize wheelchair object
 Thread compass;                         // Thread for compass
 Thread velocity;                        // Thread for velosity
 Thread  ToFSafe;                        // Thread for safety stuff

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/IMU6050/IMUWheelchair.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/IMU6050/IMUWheelchair.cpp	Tue Jul 09 17:52:32 2019 +0000
@@ -0,0 +1,123 @@
+#include "IMUWheelchair.h"
+float total_yaw;
+
+
+IMUWheelchair::IMUWheelchair(Serial* out, Timer* time)
+{
+    imu = new MPU6050(SDA, SCL);
+    usb = out;
+    t = time;
+    start = false;
+    IMUWheelchair::setup();
+
+}
+
+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();
+
+}
+
+void IMUWheelchair::setup() {
+    imu->setGyroRange(MPU6050_GYRO_RANGE_1000);
+    accelD = accelData;
+    gyroD = gyroData;
+    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)accelD[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)accelD[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)accelD[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)gyroD[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)gyroD[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)gyroD[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 = .4+(IMUWheelchair::gyro_x())*180/3.141593;
+    if(abs(gyroZ) >= .5) {
+     //printf("t->read(): %lf, gyroscope %lf, change %lf\r\n", t->read(), gyroZ, t->read()*gyroZ*2.25);
+        total_yaw = total_yaw - t->read()*gyroZ;
+     //printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, 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 (-accelD[1] ,( sqrt (accelD[0] * accelD[0]) +(accelD[2]  *accelD[2])));
+    pitch = pitch*57.3;
+    return (double)pitch;
+}
+
+double IMUWheelchair::roll() {
+    imu->getAccelero(accelD);
+    float roll = atan2(-accelD[0]  ,( sqrt((accelD[1] *accelD[1] ) + 
+ (accelD[2] * accelD[2]))));
+    roll = roll*57.3;
+    t->reset();
+    return (double)roll;
+}

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/IMU6050/IMUWheelchair.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/IMU6050/IMUWheelchair.h	Tue Jul 09 17:52:32 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

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/IMU6050/MPU6050.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/IMU6050/MPU6050.cpp	Tue Jul 09 17:52:32 2019 +0000
@@ -0,0 +1,317 @@
+/**
+ * Includes
+ */
+#include "MPU6050.h"
+
+MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
+    this->setSleepMode(false);
+    
+    //Initializations:
+    currentGyroRange = 0;
+    currentAcceleroRange=0;
+    alpha = ALPHA;
+}
+
+//--------------------------------------------------
+//-------------------General------------------------
+//--------------------------------------------------
+
+void MPU6050::write(char address, char data) {
+    char temp[2];
+    temp[0]=address;
+    temp[1]=data;
+    
+    connection.write(MPU6050_ADDRESS * 2,temp,2);
+}
+
+char MPU6050::read(char address) {
+    char retval;
+    connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
+    connection.read(MPU6050_ADDRESS * 2, &retval, 1);
+    return retval;
+}
+
+void MPU6050::read(char address, char *data, int length) {
+    connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
+    connection.read(MPU6050_ADDRESS * 2, data, length);
+}
+
+void MPU6050::setSleepMode(bool state) {
+    char temp;
+    temp = this->read(MPU6050_PWR_MGMT_1_REG);
+    if (state == true)
+        temp |= 1<<MPU6050_SLP_BIT;
+    if (state == false)
+        temp &= ~(1<<MPU6050_SLP_BIT);
+    this->write(MPU6050_PWR_MGMT_1_REG, temp);
+}
+
+bool MPU6050::testConnection( void ) {
+    char temp;
+    temp = this->read(MPU6050_WHO_AM_I_REG);
+    return (temp == (MPU6050_ADDRESS & 0xFE));
+}
+
+void MPU6050::setBW(char BW) {
+    char temp;
+    BW=BW & 0x07;
+    temp = this->read(MPU6050_CONFIG_REG);
+    temp &= 0xF8;
+    temp = temp + BW;
+    this->write(MPU6050_CONFIG_REG, temp);
+}
+
+void MPU6050::setI2CBypass(bool state) {
+    char temp;
+    temp = this->read(MPU6050_INT_PIN_CFG);
+    if (state == true)
+        temp |= 1<<MPU6050_BYPASS_BIT;
+    if (state == false)
+        temp &= ~(1<<MPU6050_BYPASS_BIT);
+    this->write(MPU6050_INT_PIN_CFG, temp);
+}
+
+//--------------------------------------------------
+//----------------Accelerometer---------------------
+//--------------------------------------------------
+
+void MPU6050::setAcceleroRange( char range ) {
+    char temp;
+    range = range & 0x03;
+    currentAcceleroRange = range;
+    
+    temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
+    temp &= ~(3<<3);
+    temp = temp + (range<<3);
+    this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
+}
+
+int MPU6050::getAcceleroRawX( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+    
+int MPU6050::getAcceleroRawY( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU6050::getAcceleroRawZ( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU6050::getAcceleroRaw( int *data ) {
+    char temp[6];
+    this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
+    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU6050::getAccelero( float *data ) {
+    int temp[3];
+    this->getAcceleroRaw(temp);
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
+        data[0]=(float)temp[0] / 16384.0 * 9.81;
+        data[1]=(float)temp[1] / 16384.0 * 9.81;
+        data[2]=(float)temp[2] / 16384.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
+        data[0]=(float)temp[0] / 8192.0 * 9.81;
+        data[1]=(float)temp[1] / 8192.0 * 9.81;
+        data[2]=(float)temp[2] / 8192.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
+        data[0]=(float)temp[0] / 4096.0 * 9.81;
+        data[1]=(float)temp[1] / 4096.0 * 9.81;
+        data[2]=(float)temp[2] / 4096.0 * 9.81;
+        }
+    if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
+        data[0]=(float)temp[0] / 2048.0 * 9.81;
+        data[1]=(float)temp[1] / 2048.0 * 9.81;
+        data[2]=(float)temp[2] / 2048.0 * 9.81;
+        }
+    
+    #ifdef DOUBLE_ACCELERO
+        data[0]*=2;
+        data[1]*=2;   
+        data[2]*=2;
+    #endif   
+}
+//--------------------------------------------------
+//------------------Gyroscope-----------------------
+//--------------------------------------------------
+void MPU6050::setGyroRange( char range ) {
+    char temp;
+    currentGyroRange = range;
+    range = range & 0x03;
+    temp = this->read(MPU6050_GYRO_CONFIG_REG);
+    temp &= ~(3<<3);
+    temp = temp + range<<3;
+    this->write(MPU6050_GYRO_CONFIG_REG, temp);
+}
+
+int MPU6050::getGyroRawX( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+    
+int MPU6050::getGyroRawY( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+int MPU6050::getGyroRawZ( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+void MPU6050::getGyroRaw( int *data ) {
+    char temp[6];
+    this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
+    data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+    data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+    data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU6050::getGyro( float *data ) {
+    int temp[3];
+    this->getGyroRaw(temp);
+    if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
+        data[0]=(float)temp[0] / 301.0;
+        data[1]=(float)temp[1] / 301.0;
+        data[2]=(float)temp[2] / 301.0;
+        }               //7505.5
+    if (currentGyroRange == MPU6050_GYRO_RANGE_500){
+        data[0]=(float)temp[0] / 3752.9;
+        data[1]=(float)temp[1] / 3752.9;
+        data[2]=(float)temp[2] / 3752.9;
+        }
+    if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
+        data[0]=(float)temp[0]/ 1879.3;
+        data[1]=(float)temp[1] / 1879.3;
+        data[2]=(float)temp[2] / 1879.3;
+
+        }
+    if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
+        data[0]=(float)temp[0] / 939.7;
+        data[1]=(float)temp[1] / 939.7;
+        data[2]=(float)temp[2] / 939.7;
+        }
+
+}
+//--------------------------------------------------
+//-------------------Temperature--------------------
+//--------------------------------------------------
+int MPU6050::getTempRaw( void ) {
+    short retval;
+    char data[2];
+    this->read(MPU6050_TEMP_H_REG, data, 2);
+    retval = (data[0]<<8) + data[1];
+    return (int)retval;
+}
+
+float MPU6050::getTemp( void ) {
+    float retval;
+    retval=(float)this->getTempRaw();
+    retval=(retval+521.0)/340.0+35.0;
+    return retval;
+}
+
+/**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more.
+ function for getting angles in degrees from accelerometer
+*/
+void MPU6050::getAcceleroAngle( float *data ) {
+    float temp[3];
+    this->getAccelero(temp);
+    
+    data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
+    data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
+    data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on
+    
+//    data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES;  //This spits out values between -180 to 180 (360 degrees)
+//    data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES;    //but it takes longer and system gets unstable when angles ~90 degrees
+}
+
+///function for getting offset values for the gyro & accelerometer
+void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
+    float gyro[3];      
+    float accAngle[3];
+    
+    for (int i = 0; i < 3; i++) {
+        accOffset[i] = 0.0;     //initialise offsets to 0.0
+        gyroOffset[i] = 0.0;
+    }
+    
+    for (int i = 0; i < sampleSize; i++){
+        this->getGyro(gyro); //take real life measurements  
+        this->getAcceleroAngle (accAngle);
+        
+        for (int j = 0; j < 3; j++){
+            *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
+            *(gyroOffset+j) += gyro[j]/sampleSize;
+        }
+        wait (0.01);    //wait between each reading for accuracy 
+    } 
+}
+
+///function for computing angles for roll, pitch anf yaw
+void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){
+    float gyro[3];
+    float accAngle[3];
+    
+    this->getGyro(gyro);                //get gyro value in rad/s
+    this->getAcceleroAngle(accAngle);   //get angle from accelerometer
+    
+    for (int i = 0; i < 3; i++){
+        gyro[i] -= gyroOffset[i];       //substract offset values
+        accAngle[i] -= accOffset[i];    
+    }
+    
+    //apply filters on pitch and roll to get accurate angle values
+    angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 
+    angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS];
+    
+    //calculate Yaw using just the gyroscope values - inaccurate
+    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval;     
+}
+
+///function for setting a different Alpha value, which is used in complemetary filter calculations
+void MPU6050::setAlpha(float val){
+    alpha = val;   
+}
+
+///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take
+void MPU6050::enableInt( void ){
+    char temp;
+    temp = this->read(MPU6050_RA_INT_ENABLE);
+    temp |= 0x01;
+    this->write(MPU6050_RA_INT_ENABLE, temp);
+}
+
+///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);
+    temp &= 0xFE;
+    this->write(MPU6050_RA_INT_ENABLE, temp);
+}
\ No newline at end of file

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/IMU6050/MPU6050.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/IMU6050/MPU6050.h	Tue Jul 09 17:52:32 2019 +0000
@@ -0,0 +1,333 @@
+/*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output. 
+If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
+*/
+
+
+#ifndef MPU6050_H
+#define MPU6050_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+
+/**
+ * Defines
+ */
+#ifndef MPU6050_ADDRESS
+    #define MPU6050_ADDRESS             0x68 // address pin low (GND), default for InvenSense evaluation board
+#endif
+
+#ifdef MPU6050_ES
+        #define DOUBLE_ACCELERO
+#endif  
+
+/**
+ * Registers
+ */
+ #define MPU6050_CONFIG_REG         0x1A
+ #define MPU6050_GYRO_CONFIG_REG    0x1B
+ #define MPU6050_ACCELERO_CONFIG_REG    0x1C
+  
+ #define MPU6050_INT_PIN_CFG        0x37
+ 
+ #define MPU6050_ACCEL_XOUT_H_REG   0x3B
+ #define MPU6050_ACCEL_YOUT_H_REG   0x3D
+ #define MPU6050_ACCEL_ZOUT_H_REG   0x3F
+ 
+ #define MPU6050_TEMP_H_REG         0x41
+ 
+ #define MPU6050_GYRO_XOUT_H_REG    0x43
+ #define MPU6050_GYRO_YOUT_H_REG    0x45
+ #define MPU6050_GYRO_ZOUT_H_REG    0x47
+ 
+ 
+ 
+ #define MPU6050_PWR_MGMT_1_REG     0x6B
+ #define MPU6050_WHO_AM_I_REG       0x75
+ 
+                 
+ 
+ /**
+  * Definitions
+  */
+#define MPU6050_SLP_BIT             6
+#define MPU6050_BYPASS_BIT         1
+
+#define MPU6050_BW_256              0
+#define MPU6050_BW_188              1
+#define MPU6050_BW_98               2
+#define MPU6050_BW_42               3
+#define MPU6050_BW_20               4
+#define MPU6050_BW_10               5
+#define MPU6050_BW_5                6
+
+#define MPU6050_ACCELERO_RANGE_2G   0
+#define MPU6050_ACCELERO_RANGE_4G   1
+#define MPU6050_ACCELERO_RANGE_8G   2
+#define MPU6050_ACCELERO_RANGE_16G  3
+
+#define MPU6050_GYRO_RANGE_250      0
+#define MPU6050_GYRO_RANGE_500      1
+#define MPU6050_GYRO_RANGE_1000     2
+#define MPU6050_GYRO_RANGE_2000     3
+
+//interrupt address
+#define MPU6050_RA_INT_ENABLE 0x38  
+//define how the accelerometer is placed on surface
+#define X_AXIS 1
+#define Y_AXIS 2
+#define Z_AXIS 0
+//translation from radians to degrees
+#define RADIANS_TO_DEGREES 180/3.1415926536
+//constant used for the complementary filter, which ranges usually from 0.9 to 1.0
+#define ALPHA 0.96   //filter constant
+//scale the gyro
+#define GYRO_SCALE 2.7176 
+
+/** MPU6050 IMU library.
+  *
+  * Example:
+  * @code
+  * Later, maybe
+  * @endcode
+  */
+class MPU6050 {
+    public:
+     /**
+     * Constructor.
+     *
+     * Sleep mode of MPU6050 is immediatly disabled
+     *
+     * @param sda - mbed pin to use for the SDA I2C line.
+     * @param scl - mbed pin to use for the SCL I2C line.
+     */
+     MPU6050(PinName sda, PinName scl);
+     
+
+     /**
+     * Tests the I2C connection by reading the WHO_AM_I register. 
+     *
+     * @return True for a working connection, false for an error
+     */     
+     bool testConnection( void );
+     
+     /**
+     * Sets the bandwidth of the digital low-pass filter 
+     *
+     * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5
+     * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
+     *
+     * @param BW - The three bits that set the bandwidth (use the predefined macros)
+     */     
+     void setBW( char BW );
+     
+     /**
+     * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus) 
+     *
+     * @param state - Enables/disables the I2C bypass mode
+     */     
+     void setI2CBypass ( bool state );
+     
+     /**
+     * Sets the Accelero full-scale range
+     *
+     * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G
+     *
+     * @param range - The two bits that set the full-scale range (use the predefined macros)
+     */
+     void setAcceleroRange(char range);
+     
+     /**
+     * Reads the accelero x-axis.
+     *
+     * @return 16-bit signed integer x-axis accelero data
+     */   
+     int getAcceleroRawX( void );
+     
+     /**
+     * Reads the accelero y-axis.
+     *
+     * @return 16-bit signed integer y-axis accelero data
+     */   
+     int getAcceleroRawY( void );
+     
+     /**
+     * Reads the accelero z-axis.
+     *
+     * @return 16-bit signed integer z-axis accelero data
+     */   
+     int getAcceleroRawZ( void );
+     
+     /**
+     * Reads all accelero data.
+     *
+     * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getAcceleroRaw( int *data );
+     
+     /**
+     * Reads all accelero data, gives the acceleration in m/s2
+     *
+     * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+     *
+     * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getAccelero( float *data );
+     
+     /**
+     * Sets the Gyro full-scale range
+     *
+     * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000
+     *
+     * @param range - The two bits that set the full-scale range (use the predefined macros)
+     */
+     void setGyroRange(char range);
+
+     /**
+     * Reads the gyro x-axis.
+     *
+     * @return 16-bit signed integer x-axis gyro data
+     */   
+     int getGyroRawX( void );
+     
+     /**
+     * Reads the gyro y-axis.
+     *
+     * @return 16-bit signed integer y-axis gyro data
+     */   
+     int getGyroRawY( void );
+     
+     /**
+     * Reads the gyro z-axis.
+     *
+     * @return 16-bit signed integer z-axis gyro data
+     */   
+     int getGyroRawZ( void );
+     
+     /**
+     * Reads all gyro data.
+     *
+     * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getGyroRaw( int *data );  
+     
+     /**
+     * Reads all gyro data, gives the gyro in rad/s
+     *
+     * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
+     *
+     * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
+     */   
+     void getGyro( float *data);     
+     
+     /**
+     * Reads temperature data.
+     *
+     * @return 16 bit signed integer with the raw temperature register value
+     */  
+     int getTempRaw( void );
+     
+     /**
+     * Returns current temperature
+     *
+     * @returns float with the current temperature
+     */  
+     float getTemp( void );
+
+     /**
+     * Sets the sleep mode of the MPU6050 
+     *
+     * @param state - true for sleeping, false for wake up
+     */     
+     void setSleepMode( bool state );
+     
+     
+     /**
+     * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU. 
+     *
+     * @param adress - register address to write to
+     * @param data - data to write
+     */
+     void write( char address, char data);
+     
+     /**
+     * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU. 
+     *
+     * @param adress - register address to write to
+     * @return - data from the register specified by RA
+     */
+     char read( char adress);
+     
+     /**
+     * Read multtiple regigsters from the device, more efficient than using multiple normal reads. 
+     *
+     * @param adress - register address to write to
+     * @param length - number of bytes to read
+     * @param data - pointer where the data needs to be written to 
+     */
+     void read( char adress, char *data, int length);
+     
+    /**
+    * function for calculating the angle from the accelerometer.
+    * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
+    * for pitch, roll and one more direction.. (NOT YAW!)
+    *
+    * @param data - angle calculated using only accelerometer
+    *
+    */ 
+    void getAcceleroAngle( float *data );
+     
+     
+     /**function which allows to produce the offset values for gyro and accelerometer.
+     * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
+     * but offset for accelerometer is calculated in angles... later on might change that
+     * function simply takes the number of samples to be taken and calculated the average
+     * 
+     * @param accOffset - accelerometer offset in angle
+     * @param gyroOffset - gyroscope offset in rad/s
+     * @param sampleSize - number of samples to be taken for calculating offsets
+     *
+     */
+     void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
+     
+     /**
+     * function for computing the angle, when accelerometer angle offset and gyro offset are both known.
+     * we also need to know how much time passed from previous calculation to now
+     * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
+     * if anyone need smth different, they can update this library...
+     *
+     * @param angle - calculated accurate angle from complemetary filter
+     * @param accOffset - offset in angle for the accelerometer
+     * @param gyroOffset - offset in rad/s for the gyroscope
+     * @param interval - time before previous angle calculation and now
+     *
+     */
+     void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval);
+     
+     ///function, which enables interrupts on MPU6050 INT pin
+     void enableInt( void );
+     
+     ///disables interrupts
+     void disableInt( void );
+     
+     /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
+     *
+     * @param val - value the alpha (complementary filter constant) should be set to
+     *
+     */
+     void setAlpha(float val);
+     
+     private:
+
+     I2C connection;
+     char currentAcceleroRange;
+     char currentGyroRange;
+     float alpha;   
+     
+};
+
+
+
+#endif

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/IMU6050/filter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/IMU6050/filter.cpp	Tue Jul 09 17:52:32 2019 +0000
@@ -0,0 +1,52 @@
+float lowPass(float sample)
+{
+    static const float a[4] = {1.00000000e+00,-2.77555756e-16,3.33333333e-01,-1.85037171e-17};
+    static const float b[4] = {0.16666667,0.5,0.5,0.16666667};
+// x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
+    static volatile float x[4] = {0};
+// x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
+    static volatile float y[4] = {0};
+    x[0] = sample;
+// Calculate the output filtered signal based on a weighted sum of previous inputs/outputs
+    y[0] = (b[0]*x[0]+b[1]*x[1]+b[2]*x[2]+b[3]*x[3])-(a[1]*y[1]+a[2]*y[2]+a[3]*y[3]);
+    y[0] /= a[0];
+// Shift the input signals by one timestep to prepare for the next call to this function
+    x[3] = x[2];
+    x[2] = x[1];
+    x[1] = x[0];
+// Shift the previously calculated output signals by one time step to prepare for the next call to this function
+    y[3] = y[2];
+    y[2] = y[1];
+    y[1] = y[0];
+    return y[0];
+}
+
+float boxcar(float sample)
+{
+    static const int boxcarWidth = 30; // Change this value to alter boxcar length
+    static float recentSamples[boxcarWidth] = {0}; // hold onto recent samples
+    static int readIndex = 0; // the index of the current reading
+    static float total = 0; // the running total
+    static float average = 0; // the average
+// subtract the last reading:
+    total = total - recentSamples[readIndex];
+// add new sample to list (overwrite oldest sample)
+    recentSamples[readIndex] = sample;
+// add the reading to the total:
+    total = total + recentSamples[readIndex];
+// advance to the next position in the array:
+    readIndex = readIndex + 1;
+// if we're at the end of the array...
+    if (readIndex >= boxcarWidth) {
+// ...wrap around to the beginning:
+        readIndex = 0;
+    }
+// calculate the average:
+    average = total / boxcarWidth;
+// send it to the computer as ASCII digits
+    return average;
+}
+float complement(float x, float y , float ratio)
+{
+    return (ratio*x + (1 - ratio)*y);
+}
\ No newline at end of file

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/IMU6050/filter.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/IMU6050/filter.h	Tue Jul 09 17:52:32 2019 +0000
@@ -0,0 +1,7 @@
+#ifndef FILTER_H
+#define FILTER_H
+
+float lowPass(float sample);
+float complement(float x, float y , float ratio);
+float boxcar(float sample);
+#endif
\ No newline at end of file

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/chair_BNO055.lib
--- a/wheelchairControlSideTof/chair_BNO055.lib	Tue Jul 02 21:17:03 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/jvfausto/code/chair_BNO055/#ce8aa8208590

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/wheelchair.cpp
--- a/wheelchairControlSideTof/wheelchair.cpp	Tue Jul 02 21:17:03 2019 +0000
+++ b/wheelchairControlSideTof/wheelchair.cpp	Tue Jul 09 17:52:32 2019 +0000
@@ -85,8 +85,8 @@
         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
     }
    
-    int sensor1 = ToFV[0];
-    int sensor4 = ToFV[3];
+    int sensor1 = ToFV[0]; //front sensor
+    int sensor4 = ToFV[3]; //front sensor
     //out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]);
     if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || 
     2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && 
@@ -121,16 +121,16 @@
     else
         forwardSafety = 0;
         
-    /*-------Side Tof begin----------*/
-    
+    /*-----------------------------Side Tof begin-----------------------------*/
     int sensor3 = ToFV[2]; //front left
     int sensor6 = ToFV[5]; //front right
     int sensor9 = ToFV[8]; //back
     int sensor12 = ToFV[11]; //back
     
-    //float currAngularVelocity = IMU DATA; //Current angular velocity from IMU
-    //float angle; //from IMU YAW, convert to cm
-    //float arcLength = angle * WHEELCHAIR_RADIUS; //S = r*Ө
+    double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU
+    double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads
+    double arcLength = WheelchairRadius * currAngularVelocity * 
+                       currAngularVelocity / (2 * maxAngularDeceleration); //S = r*Ө, in cm
     
     //Clear the front side first, else continue going straight or can't turn
     //After clearing the front sideand movinf forward, check if can clear
@@ -139,7 +139,7 @@
     //Check if can clear side
     
     //When either sensors too close to the wall, can't turn
-    if(sensor3 <= MIN_WALL_LENGTH) {
+    if(sensor3 <= minWallLength) {
         leftSafety = 1;
         out-> printf("Detecting wall to the left!\n");
     }
@@ -147,7 +147,7 @@
         leftSafety = 0;
     }
     
-    if(sensor6 <= MIN_WALL_LENGTH) {
+    if(sensor6 <= minWallLength) {
         rightSafety = 1;
         out-> printf("Detecting wall to the right!\n");
     }
@@ -155,21 +155,66 @@
         rightSafety = 0;
     }
     
-    //Check whether safe to keep turnin, user control <-- make sure 
-    //currAngularVelocity is in correct units. Know the exact moment you can
-    //stop the chair going at a certain speed before its too late
-    //else if((currAngularVelocity * currAngularVelocity > 2 * 
-    //         MAX_ANGULAR_DECELERATION * angle) && (sensor3 <= angle ||
-      //       sensor6 <= angle)) {
-     //   sideSafety = 1; //Not safe to turn
-    //}
-    //Safe to continue turning
+    //Check whether safe to keep turning 
+    // Know the exact moment you can stop the chair going at a certain speed 
+    // before its too late
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor3/10 <= arcLength + 10)) {
+        leftSafety = 1; //Not safe to turn left
+        out-> printf("Too fast to the left!\n");
+    }
+    else{
+        leftSafety = 0;
+       }
+    if((currAngularVelocity * currAngularVelocity > 2 * 
+        maxAngularDeceleration * angle) && (sensor6/10 <= arcLength + 10)) {
+        rightSafety = 1; //Not safe to turn right
+        out-> printf("Too fast to the right!\n");
+    }
+    else{
+        rightSafety = 0;
+        }
+      
+    //Safe to continue turning 
+    //Check if can turn left and back side sensors
+    
+    
+    //Check the back sensor
+    int sensor7 = ToFV[0]; //back sensor NOTTT SURE
+    int sensor8 = ToFV[3]; //back sensor
     
-    /*-------Side Tof end -----------*/
+    if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 || 
+    2 * maxDecelerationSlow*sensor8 < curr_vel*curr_vel*1000*1000) && 
+    (sensor7 < 1500 || sensor8 < 1500)) ||
+    550 > sensor7 || 550 > sensor8)
+    {
+        //out->printf("i am in danger\r\n");
+        if(x->read() > def)
+        {
+            x->write(def);
+            backwardSafety = 1;          // You cannot move forward
+        }
+    }
+    //When going to fast to stop from wall
+    else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor7 < curr_vel*curr_vel*1000*1000 || 
+    2 * maxDecelerationFast*sensor8 < curr_vel*curr_vel*1000*1000) && 
+    (sensor7 < 1500 || sensor8 < 1500)) ||
+    550 > sensor7 || 550 > sensor8)
+    {
+        //out->printf("i am in danger\r\n");
+        if(x->read() > def)
+        {
+            x->write(def);
+            backwardSafety = 1;
+        }
+    }
+    
+    
+    /*----------------------------Side Tof end -------------------------------*/
 }
 
 /* Constructor for Wheelchair class */
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, 
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, Timer*imuT, QEI* qei, QEI* qeiS, 
 VL53L1X** ToFT)
 {
     x_position = 0;
@@ -181,7 +226,7 @@
     /* Initializes IMU Library */
     out = pc;                                                                           // "out" is called for serial monitor
     out->printf("on\r\n");
-    imu = new chair_BNO055(pc, time);
+    imu = new IMUWheelchair(pc, imuT);
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
     wheelS = qeiS;                                                                      // "wheel" is called for encoder

diff -r 3c1b58654e67 -r d1faccb96146 wheelchairControlSideTof/wheelchair.h
--- a/wheelchairControlSideTof/wheelchair.h	Tue Jul 02 21:17:03 2019 +0000
+++ b/wheelchairControlSideTof/wheelchair.h	Tue Jul 09 17:52:32 2019 +0000
@@ -3,7 +3,7 @@
 /*************************************************************************
 *            Importing libraries into wheelchair.h                       *
 **************************************************************************/
-#include "chair_BNO055.h"
+#include "IMUWheelchair.h"
 #include "PID.h"
 #include "QEI.h"
 #include "VL53L1X.h"
@@ -36,13 +36,13 @@
 #define ToFSensorNum 12
 
 /*************************************************************************
-*IMU definitions for turning wheelchair
+*IMU definitions for turning wheelchair*
 **************************************************************************/
-#define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm)
-#define  MAX_ANGULAR_DECELERATION 60 //found through testing, max 
+#define WheelchairRadius 95 //distance from center of rotation to edge of wheelchair
+#define maxAngularDeceleration 1.04 //found through testing, max 
                                      //acceleration at which chair can 
-                                     //stop while turning. In degree per sec
-#define MIN_WALL_LENGTH 100 // minimum distance from wall to ToF (mm)
+                                     //stop while turning. In rads per sec
+#define minWallLength 100 // minimum distance from wall to ToF (mm)
 /*************************************************************************
 *                                                                        *
 *                         Wheelchair class                               *
@@ -57,7 +57,7 @@
     *   serial for printout, and time. This is also used to initialize some  *
     *   variables for the timer,encoders and time-of-flight sensors          *
     **************************************************************************/
-    Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS,
+    Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, Timer* imuT, QEI* wheel, QEI* wheelS,
                VL53L1X** ToF);
 
     /*************************************************************************
@@ -214,6 +214,7 @@
     double vel;
     double test1, test2;
     bool forwardSafety;
+    bool backwardSafety;//Check if can move backward
     bool leftSafety; //to check if can turn left
     bool rightSafety; //to check if can turn right
     double curr_yaw, curr_velS;                                                            // Variable that contains current relative angle
@@ -236,7 +237,7 @@
 
     DigitalIn* e_button;                //Pointer to e_button
 
-    chair_BNO055* imu;                  // Pointer to IMU
+    IMUWheelchair* imu;                  // Pointer to IMU
     Serial* out;                        // Pointer to Serial Monitor
     Timer* ti;                          // Pointer to the timer
     QEI* wheel;                         // Pointer to encoder