Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
wheelchairControlSideTof/IMU6050/IMUWheelchair.h@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?
User | Revision | Line number | New contents of line |
---|---|---|---|
isagmz | 21:d1faccb96146 | 1 | #ifndef IMUWheelchair_H |
isagmz | 21:d1faccb96146 | 2 | #define IMUWheelchair_H |
isagmz | 21:d1faccb96146 | 3 | |
isagmz | 21:d1faccb96146 | 4 | #include "filter.h" |
isagmz | 21:d1faccb96146 | 5 | //#include "mbed.h" |
isagmz | 21:d1faccb96146 | 6 | #include "math.h" |
isagmz | 21:d1faccb96146 | 7 | #include <MPU6050.h> |
isagmz | 21:d1faccb96146 | 8 | |
isagmz | 21:d1faccb96146 | 9 | #define PI 3.141593 |
isagmz | 21:d1faccb96146 | 10 | |
isagmz | 21:d1faccb96146 | 11 | /*#define SDA D14 |
isagmz | 21:d1faccb96146 | 12 | #define SCL D15*/ |
isagmz | 21:d1faccb96146 | 13 | #define SDA PB_9 |
isagmz | 21:d1faccb96146 | 14 | #define SCL PB_8 |
isagmz | 21:d1faccb96146 | 15 | #define SAMPLEFREQ 50 |
isagmz | 21:d1faccb96146 | 16 | #define CAL_TIME 3 |
isagmz | 21:d1faccb96146 | 17 | |
isagmz | 21:d1faccb96146 | 18 | class IMUWheelchair { |
isagmz | 21:d1faccb96146 | 19 | public: |
isagmz | 21:d1faccb96146 | 20 | //The constructor for this class |
isagmz | 21:d1faccb96146 | 21 | IMUWheelchair(Serial* out, Timer* time); |
isagmz | 21:d1faccb96146 | 22 | IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time); |
isagmz | 21:d1faccb96146 | 23 | |
isagmz | 21:d1faccb96146 | 24 | //Set up the IMU, check if it connects |
isagmz | 21:d1faccb96146 | 25 | void setup(); |
isagmz | 21:d1faccb96146 | 26 | |
isagmz | 21:d1faccb96146 | 27 | //Get the x-component of the angular acceleration |
isagmz | 21:d1faccb96146 | 28 | double accel_x(); |
isagmz | 21:d1faccb96146 | 29 | |
isagmz | 21:d1faccb96146 | 30 | //Get the y-component of the angular acceleration |
isagmz | 21:d1faccb96146 | 31 | double accel_y(); |
isagmz | 21:d1faccb96146 | 32 | |
isagmz | 21:d1faccb96146 | 33 | //Get the z-component of the angular acceleration |
isagmz | 21:d1faccb96146 | 34 | double accel_z(); |
isagmz | 21:d1faccb96146 | 35 | |
isagmz | 21:d1faccb96146 | 36 | //Get the x-component of gyro, angular velocity |
isagmz | 21:d1faccb96146 | 37 | double gyro_x(); |
isagmz | 21:d1faccb96146 | 38 | |
isagmz | 21:d1faccb96146 | 39 | //Get the y-component of gyro, angular velocity |
isagmz | 21:d1faccb96146 | 40 | double gyro_y(); |
isagmz | 21:d1faccb96146 | 41 | |
isagmz | 21:d1faccb96146 | 42 | //Get the z-component of gyro, angular velocity |
isagmz | 21:d1faccb96146 | 43 | double gyro_z(); |
isagmz | 21:d1faccb96146 | 44 | |
isagmz | 21:d1faccb96146 | 45 | //Magnometer to find angle relative to North to compare to gyroscope |
isagmz | 21:d1faccb96146 | 46 | //double angle_north(); |
isagmz | 21:d1faccb96146 | 47 | |
isagmz | 21:d1faccb96146 | 48 | //Get the YAW, or angle (theta), direction facing |
isagmz | 21:d1faccb96146 | 49 | double yaw(); |
isagmz | 21:d1faccb96146 | 50 | |
isagmz | 21:d1faccb96146 | 51 | //Get the pitch, (Up and down component) |
isagmz | 21:d1faccb96146 | 52 | double pitch(); |
isagmz | 21:d1faccb96146 | 53 | |
isagmz | 21:d1faccb96146 | 54 | //Get the roll, the tilt |
isagmz | 21:d1faccb96146 | 55 | double roll(); |
isagmz | 21:d1faccb96146 | 56 | |
isagmz | 21:d1faccb96146 | 57 | MPU6050* imu; //The IMU we're testing from, MPU6050 |
isagmz | 21:d1faccb96146 | 58 | |
isagmz | 21:d1faccb96146 | 59 | private: |
isagmz | 21:d1faccb96146 | 60 | Serial* usb; //the connection port |
isagmz | 21:d1faccb96146 | 61 | Timer* t;//to calculate the time |
isagmz | 21:d1faccb96146 | 62 | float accelData[3]; // stores the angular acceleration component |
isagmz | 21:d1faccb96146 | 63 | float gyroData[3]; //stores the gyro data x,y,z |
isagmz | 21:d1faccb96146 | 64 | float* accelD; //Pointer that points to either accelData |
isagmz | 21:d1faccb96146 | 65 | float* gyroD; //Ptr to the gyroData array |
isagmz | 21:d1faccb96146 | 66 | |
isagmz | 21:d1faccb96146 | 67 | float angleData[3]; //Contains the pitch, roll, yaw angle |
isagmz | 21:d1faccb96146 | 68 | float* angleD;//Ptr to angleData array |
isagmz | 21:d1faccb96146 | 69 | |
isagmz | 21:d1faccb96146 | 70 | void calibrate_yaw(); |
isagmz | 21:d1faccb96146 | 71 | |
isagmz | 21:d1faccb96146 | 72 | bool start; |
isagmz | 21:d1faccb96146 | 73 | |
isagmz | 21:d1faccb96146 | 74 | }; |
isagmz | 21:d1faccb96146 | 75 | |
isagmz | 21:d1faccb96146 | 76 | #endif |