Affordable Smart Wheelchair / BNOWrapper

Added BNO080Wheelchair.h

Dependents:   BNO080_program wheelchaircontrol8 Version1-9 BNO080_program

BNO080Wheelchair.cpp@10:9275e6f7bf1b, 2019-07-30 (annotated)

Committer:
t1jain
Date:
Tue Jul 30 18:33:45 2019 +0000
Revision:
10:9275e6f7bf1b
Parent:
8:217f510db255
Child:
11:dbe6d8d0ceb1
Updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t1jain 10:9275e6f7bf1b 1 #include "BNO080Wheelchair.h"
t1jain 10:9275e6f7bf1b 2 float total_yaw;
sepham 8:217f510db255 3
t1jain 10:9275e6f7bf1b 4 //The constructor for the BNO080 imu. Needs 7 parameters
t1jain 10:9275e6f7bf1b 5 BNO080Wheelchair::BNO080Wheelchair(Serial *debugPort, PinName sdaPin,
sepham 8:217f510db255 6 PinName sclPin, PinName intPin, PinName rstPin,
sepham 8:217f510db255 7 uint8_t i2cAddress, int i2cPortpeed) {
t1jain 10:9275e6f7bf1b 8 imu = new BNO080(usb,
t1jain 10:9275e6f7bf1b 9 SDA,
t1jain 10:9275e6f7bf1b 10 SCL,
t1jain 10:9275e6f7bf1b 11 INT_PIN,
t1jain 10:9275e6f7bf1b 12 RST_PIN,
t1jain 10:9275e6f7bf1b 13 0x4a,
t1jain 10:9275e6f7bf1b 14 100000);
sepham 8:217f510db255 15 //setUp
sepham 8:217f510db255 16
sepham 8:217f510db255 17 }
sepham 8:217f510db255 18 //Check if all the
t1jain 10:9275e6f7bf1b 19 void BNO080Wheelchair::setup() {
sepham 8:217f510db255 20 imu -> begin();
t1jain 10:9275e6f7bf1b 21 }
t1jain 10:9275e6f7bf1b 22
t1jain 10:9275e6f7bf1b 23 //Returns a double, the value of the x-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 24 double BNO080Wheelchair::gyro_x() {
t1jain 10:9275e6f7bf1b 25 imu -> updateData(); // hasNewData()?
t1jain 10:9275e6f7bf1b 26 return (double)gyroRotation[0];
t1jain 10:9275e6f7bf1b 27 }
t1jain 10:9275e6f7bf1b 28
t1jain 10:9275e6f7bf1b 29 //Get the y component of the angular velocity from IMU. Stores the component
t1jain 10:9275e6f7bf1b 30 //in a float array
t1jain 10:9275e6f7bf1b 31 //Returns a double, the value of the y-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 32 double BNO080Wheelchair::gyro_y() {
t1jain 10:9275e6f7bf1b 33 imu -> updateData(); // hasNewData()?
t1jain 10:9275e6f7bf1b 34 return (double)gyroRotation[1];
t1jain 10:9275e6f7bf1b 35 }
t1jain 10:9275e6f7bf1b 36
t1jain 10:9275e6f7bf1b 37 //Get the z component of the angular velocity from IMU. Stores the component
t1jain 10:9275e6f7bf1b 38 //in a float array
t1jain 10:9275e6f7bf1b 39 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 40 double BNO080Wheelchair::gyro_z() {
t1jain 10:9275e6f7bf1b 41 imu -> updateData(); // hasNewData()?
t1jain 10:9275e6f7bf1b 42 return (double)gyroRotation[2];
t1jain 10:9275e6f7bf1b 43 }
t1jain 10:9275e6f7bf1b 44
t1jain 10:9275e6f7bf1b 45 //Get the x component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 46 //in a float array
t1jain 10:9275e6f7bf1b 47 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 48 double BNO080Wheelchair::accel_x() {
t1jain 10:9275e6f7bf1b 49 imu -> updateData(); // hasNewData()?
t1jain 10:9275e6f7bf1b 50 return (double)linearAcceleration[0];
t1jain 10:9275e6f7bf1b 51 }
t1jain 10:9275e6f7bf1b 52
t1jain 10:9275e6f7bf1b 53 //Get the y component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 54 //in a float array
t1jain 10:9275e6f7bf1b 55 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 56 double BNO080Wheelchair::accel_y() {
t1jain 10:9275e6f7bf1b 57 imu -> updateData(); // hasNewData()?
t1jain 10:9275e6f7bf1b 58 return (double)linearAcceleration[1];
t1jain 10:9275e6f7bf1b 59 }
t1jain 10:9275e6f7bf1b 60
t1jain 10:9275e6f7bf1b 61 //Get the z component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 62 //in a float array
t1jain 10:9275e6f7bf1b 63 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 64 double BNO080Wheelchair::accel_z() {
t1jain 10:9275e6f7bf1b 65 imu -> updateData(); // hasNewData()?
t1jain 10:9275e6f7bf1b 66 return (double)linearAcceleration[2];
t1jain 10:9275e6f7bf1b 67 }
t1jain 10:9275e6f7bf1b 68
t1jain 10:9275e6f7bf1b 69 double BNO080Wheelchair::yaw() {
t1jain 10:9275e6f7bf1b 70
t1jain 10:9275e6f7bf1b 71 float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593;
t1jain 10:9275e6f7bf1b 72 if(abs(gyroZ) >= .5) {
t1jain 10:9275e6f7bf1b 73 //printf("t->read(): %lf, gyroscope %lf, change %lf\r\n", t->read(), gyroZ, t->read()*gyroZ*2.25);
t1jain 10:9275e6f7bf1b 74 total_yaw = total_yaw - t->read()*gyroZ;
t1jain 10:9275e6f7bf1b 75 //printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
t1jain 10:9275e6f7bf1b 76 }
t1jain 10:9275e6f7bf1b 77 t->reset();
t1jain 10:9275e6f7bf1b 78 if(total_yaw > 360)
t1jain 10:9275e6f7bf1b 79 total_yaw -= 360;
t1jain 10:9275e6f7bf1b 80 if(total_yaw < 0)
t1jain 10:9275e6f7bf1b 81 total_yaw += 360;
t1jain 10:9275e6f7bf1b 82 return (double)total_yaw;
t1jain 10:9275e6f7bf1b 83 }
t1jain 10:9275e6f7bf1b 84
t1jain 10:9275e6f7bf1b 85 double BNO080Wheelchair::compass() {
t1jain 10:9275e6f7bf1b 86 imu -> updateData();
t1jain 10:9275e6f7bf1b 87
t1jain 10:9275e6f7bf1b 88 float Pi = 3.14159;
t1jain 10:9275e6f7bf1b 89
t1jain 10:9275e6f7bf1b 90 // Calculate the angle of the vector y,x
t1jain 10:9275e6f7bf1b 91 double heading = (atan2(magField[1],magField[0]) * 180) / Pi;
t1jain 10:9275e6f7bf1b 92
t1jain 10:9275e6f7bf1b 93 // Normalize to 0-360
t1jain 10:9275e6f7bf1b 94 if (heading < 0)
t1jain 10:9275e6f7bf1b 95 {
t1jain 10:9275e6f7bf1b 96 heading = 360 + heading;
t1jain 10:9275e6f7bf1b 97 }
t1jain 10:9275e6f7bf1b 98
t1jain 10:9275e6f7bf1b 99 return heading;
t1jain 10:9275e6f7bf1b 100 }
t1jain 10:9275e6f7bf1b 101
t1jain 10:9275e6f7bf1b 102
t1jain 10:9275e6f7bf1b 103
t1jain 10:9275e6f7bf1b 104