![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fully integrated ToF/IMU codes
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
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