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?
User | Revision | Line number | New 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 |