With pid left, right, and foward
Dependents: wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol wheelchaircontrol2 ... more
Fork of chair_BNO055 by
chair_BNO055.cpp@2:7d6467fa1977, 2018-08-09 (annotated)
- Committer:
- ryanlin97
- Date:
- Thu Aug 09 16:36:24 2018 +0000
- Revision:
- 2:7d6467fa1977
- Parent:
- 1:3258d62af038
- Child:
- 4:a6452220ec66
most recent update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:ad7a811c859f | 1 | #include "chair_BNO055.h" |
ryanlin97 | 0:ad7a811c859f | 2 | |
ryanlin97 | 1:3258d62af038 | 3 | static float total_yaw = 0; |
ryanlin97 | 1:3258d62af038 | 4 | static float minGyroZ; |
ryanlin97 | 1:3258d62af038 | 5 | chair_BNO055::chair_BNO055(Serial* out, Timer* time) |
ryanlin97 | 1:3258d62af038 | 6 | { |
ryanlin97 | 1:3258d62af038 | 7 | imu = new BNO055(SDA, SCL); |
ryanlin97 | 1:3258d62af038 | 8 | usb = out; |
ryanlin97 | 1:3258d62af038 | 9 | t = time; |
ryanlin97 | 1:3258d62af038 | 10 | start = false; |
ryanlin97 | 0:ad7a811c859f | 11 | } |
ryanlin97 | 0:ad7a811c859f | 12 | |
ryanlin97 | 1:3258d62af038 | 13 | chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time) |
ryanlin97 | 1:3258d62af038 | 14 | { |
ryanlin97 | 1:3258d62af038 | 15 | imu = new BNO055(sda_pin, scl_pin); |
ryanlin97 | 1:3258d62af038 | 16 | usb = out; |
ryanlin97 | 1:3258d62af038 | 17 | t = time; |
ryanlin97 | 1:3258d62af038 | 18 | start = false; |
ryanlin97 | 0:ad7a811c859f | 19 | } |
ryanlin97 | 0:ad7a811c859f | 20 | |
ryanlin97 | 1:3258d62af038 | 21 | void chair_BNO055::setup() |
ryanlin97 | 1:3258d62af038 | 22 | { |
ryanlin97 | 1:3258d62af038 | 23 | imu->reset(); |
ryanlin97 | 1:3258d62af038 | 24 | usb->printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n"); |
ryanlin97 | 1:3258d62af038 | 25 | while (imu->check() == 0) { |
ryanlin97 | 1:3258d62af038 | 26 | usb->printf("Bosch BNO055 is NOT available!!\r\n"); |
ryanlin97 | 1:3258d62af038 | 27 | wait(.5); |
ryanlin97 | 1:3258d62af038 | 28 | } |
ryanlin97 | 1:3258d62af038 | 29 | usb->printf("BNO055 found\r\n\r\n"); |
ryanlin97 | 1:3258d62af038 | 30 | usb->printf("Chip ID: %0z\r\n",imu->ID.id); |
ryanlin97 | 1:3258d62af038 | 31 | usb->printf("Accelerometer ID: %0z\r\n",imu->ID.accel); |
ryanlin97 | 1:3258d62af038 | 32 | usb->printf("Gyroscope ID: %0z\r\n",imu->ID.gyro); |
ryanlin97 | 1:3258d62af038 | 33 | usb->printf("Magnetometer ID: %0z\r\n\r\n",imu->ID.mag); |
ryanlin97 | 1:3258d62af038 | 34 | usb->printf("Firmware version v%d.%0d\r\n",imu->ID.sw[0],imu->ID.sw[1]); |
ryanlin97 | 1:3258d62af038 | 35 | usb->printf("Bootloader version v%d\r\n\r\n",imu->ID.bootload); |
ryanlin97 | 1:3258d62af038 | 36 | imu->set_accel_units(MPERSPERS); |
ryanlin97 | 1:3258d62af038 | 37 | imu->setmode(OPERATION_MODE_AMG); |
ryanlin97 | 1:3258d62af038 | 38 | imu->read_calibration_data(); |
ryanlin97 | 1:3258d62af038 | 39 | imu->write_calibration_data(); |
ryanlin97 | 1:3258d62af038 | 40 | imu->set_angle_units(DEGREES); |
ryanlin97 | 0:ad7a811c859f | 41 | |
ryanlin97 | 1:3258d62af038 | 42 | t->start(); |
ryanlin97 | 1:3258d62af038 | 43 | |
ryanlin97 | 0:ad7a811c859f | 44 | } |
ryanlin97 | 0:ad7a811c859f | 45 | |
ryanlin97 | 1:3258d62af038 | 46 | void chair_BNO055::calibrate_yaw() { |
ryanlin97 | 1:3258d62af038 | 47 | float start = t->read(); |
ryanlin97 | 1:3258d62af038 | 48 | |
ryanlin97 | 1:3258d62af038 | 49 | while( t->read()- start <= CAL_TIME ){ |
ryanlin97 | 1:3258d62af038 | 50 | total_yaw = boxcar(chair_BNO055::angle_north()); |
ryanlin97 | 1:3258d62af038 | 51 | usb->printf("total_yaw %f\n", total_yaw); |
ryanlin97 | 1:3258d62af038 | 52 | } |
ryanlin97 | 1:3258d62af038 | 53 | } |
ryanlin97 | 1:3258d62af038 | 54 | |
ryanlin97 | 1:3258d62af038 | 55 | |
ryanlin97 | 1:3258d62af038 | 56 | double chair_BNO055::accel_x() |
ryanlin97 | 1:3258d62af038 | 57 | { |
ryanlin97 | 1:3258d62af038 | 58 | imu->get_accel(); |
ryanlin97 | 1:3258d62af038 | 59 | return (double)imu->accel.x; |
ryanlin97 | 0:ad7a811c859f | 60 | } |
ryanlin97 | 0:ad7a811c859f | 61 | |
ryanlin97 | 1:3258d62af038 | 62 | double chair_BNO055::accel_y() |
ryanlin97 | 1:3258d62af038 | 63 | { |
ryanlin97 | 1:3258d62af038 | 64 | imu->get_accel(); |
ryanlin97 | 1:3258d62af038 | 65 | return (double)imu->accel.y; |
ryanlin97 | 0:ad7a811c859f | 66 | } |
ryanlin97 | 0:ad7a811c859f | 67 | |
ryanlin97 | 1:3258d62af038 | 68 | double chair_BNO055::accel_z() |
ryanlin97 | 1:3258d62af038 | 69 | { |
ryanlin97 | 1:3258d62af038 | 70 | imu->get_accel(); |
ryanlin97 | 1:3258d62af038 | 71 | return (double)imu->accel.z; |
ryanlin97 | 0:ad7a811c859f | 72 | } |
ryanlin97 | 0:ad7a811c859f | 73 | |
ryanlin97 | 1:3258d62af038 | 74 | double chair_BNO055::gyro_x() |
ryanlin97 | 1:3258d62af038 | 75 | { |
ryanlin97 | 1:3258d62af038 | 76 | imu->get_gyro(); |
ryanlin97 | 1:3258d62af038 | 77 | return (double)imu->gyro.x; |
ryanlin97 | 0:ad7a811c859f | 78 | } |
ryanlin97 | 0:ad7a811c859f | 79 | |
ryanlin97 | 1:3258d62af038 | 80 | double chair_BNO055::gyro_y() |
ryanlin97 | 1:3258d62af038 | 81 | { |
ryanlin97 | 1:3258d62af038 | 82 | imu->get_gyro(); |
ryanlin97 | 1:3258d62af038 | 83 | return (double)imu->gyro.y; |
ryanlin97 | 1:3258d62af038 | 84 | } |
ryanlin97 | 1:3258d62af038 | 85 | |
ryanlin97 | 1:3258d62af038 | 86 | double chair_BNO055::gyro_z() |
ryanlin97 | 1:3258d62af038 | 87 | { |
ryanlin97 | 1:3258d62af038 | 88 | imu->get_gyro(); |
ryanlin97 | 1:3258d62af038 | 89 | return lowPass(imu->gyro.z); |
ryanlin97 | 1:3258d62af038 | 90 | } |
ryanlin97 | 1:3258d62af038 | 91 | |
ryanlin97 | 1:3258d62af038 | 92 | double chair_BNO055::angle_north() |
ryanlin97 | 1:3258d62af038 | 93 | { |
ryanlin97 | 1:3258d62af038 | 94 | imu->get_mag(); |
ryanlin97 | 1:3258d62af038 | 95 | float x = imu->mag.x - 520; |
ryanlin97 | 1:3258d62af038 | 96 | float y = imu->mag.y; |
ryanlin97 | 1:3258d62af038 | 97 | |
ryanlin97 | 1:3258d62af038 | 98 | float result = x/y; |
ryanlin97 | 1:3258d62af038 | 99 | |
ryanlin97 | 1:3258d62af038 | 100 | float angleToNorth; |
ryanlin97 | 1:3258d62af038 | 101 | if(imu->mag.y>0) |
ryanlin97 | 1:3258d62af038 | 102 | angleToNorth = 90.0 - atan(result)*180/PI; |
ryanlin97 | 1:3258d62af038 | 103 | else if(imu->mag.y<0) |
ryanlin97 | 1:3258d62af038 | 104 | angleToNorth = 270.0 - atan(result)*180/PI; |
ryanlin97 | 1:3258d62af038 | 105 | else if(y == 0 && x <= 0) |
ryanlin97 | 1:3258d62af038 | 106 | angleToNorth = 180; |
ryanlin97 | 1:3258d62af038 | 107 | else if(y == 0 && x > 0) |
ryanlin97 | 1:3258d62af038 | 108 | angleToNorth = 0; |
ryanlin97 | 1:3258d62af038 | 109 | |
ryanlin97 | 1:3258d62af038 | 110 | return (double)angleToNorth; |
ryanlin97 | 0:ad7a811c859f | 111 | } |
ryanlin97 | 0:ad7a811c859f | 112 | |
ryanlin97 | 1:3258d62af038 | 113 | double chair_BNO055::roll() |
ryanlin97 | 1:3258d62af038 | 114 | { |
ryanlin97 | 1:3258d62af038 | 115 | imu->get_accel(); |
ryanlin97 | 1:3258d62af038 | 116 | imu->get_gyro(); |
ryanlin97 | 0:ad7a811c859f | 117 | |
ryanlin97 | 1:3258d62af038 | 118 | float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) + |
ryanlin97 | 1:3258d62af038 | 119 | (imu->accel.z * imu->accel.z)))); |
ryanlin97 | 1:3258d62af038 | 120 | roll = roll*57.3; |
ryanlin97 | 1:3258d62af038 | 121 | |
ryanlin97 | 1:3258d62af038 | 122 | t->reset(); |
ryanlin97 | 1:3258d62af038 | 123 | |
ryanlin97 | 1:3258d62af038 | 124 | return (double)roll; |
ryanlin97 | 0:ad7a811c859f | 125 | } |
ryanlin97 | 0:ad7a811c859f | 126 | |
ryanlin97 | 1:3258d62af038 | 127 | double chair_BNO055::pitch() |
ryanlin97 | 1:3258d62af038 | 128 | { |
ryanlin97 | 1:3258d62af038 | 129 | imu->get_accel(); |
ryanlin97 | 1:3258d62af038 | 130 | imu->get_gyro(); |
ryanlin97 | 0:ad7a811c859f | 131 | |
ryanlin97 | 1:3258d62af038 | 132 | float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) + |
ryanlin97 | 1:3258d62af038 | 133 | (imu->accel.z * imu->accel.z)))); |
ryanlin97 | 1:3258d62af038 | 134 | pitch = pitch*57.3; |
ryanlin97 | 1:3258d62af038 | 135 | |
ryanlin97 | 1:3258d62af038 | 136 | t->reset(); |
ryanlin97 | 1:3258d62af038 | 137 | |
ryanlin97 | 1:3258d62af038 | 138 | return (double)pitch; |
ryanlin97 | 0:ad7a811c859f | 139 | } |
ryanlin97 | 0:ad7a811c859f | 140 | |
ryanlin97 | 1:3258d62af038 | 141 | double chair_BNO055::yaw() |
ryanlin97 | 1:3258d62af038 | 142 | { |
ryanlin97 | 1:3258d62af038 | 143 | float gyroZ = chair_BNO055::gyro_z(); |
ryanlin97 | 0:ad7a811c859f | 144 | |
ryanlin97 | 1:3258d62af038 | 145 | if(abs(gyroZ) >= .3) { |
ryanlin97 | 1:3258d62af038 | 146 | total_yaw = (total_yaw - t->read()*gyroZ); |
ryanlin97 | 1:3258d62af038 | 147 | //usb->printf("gyroZ %f\n", gyroZ); |
ryanlin97 | 1:3258d62af038 | 148 | } |
ryanlin97 | 1:3258d62af038 | 149 | t->reset(); |
ryanlin97 | 1:3258d62af038 | 150 | |
ryanlin97 | 1:3258d62af038 | 151 | if(total_yaw > 360) |
ryanlin97 | 1:3258d62af038 | 152 | total_yaw -= 360; |
ryanlin97 | 1:3258d62af038 | 153 | if(total_yaw < 0) |
ryanlin97 | 1:3258d62af038 | 154 | total_yaw += 360; |
ryanlin97 | 1:3258d62af038 | 155 | |
ryanlin97 | 1:3258d62af038 | 156 | //usb->printf("yaw %f\n", total_yaw); |
ryanlin97 | 1:3258d62af038 | 157 | return (double)total_yaw; |
ryanlin97 | 0:ad7a811c859f | 158 | } |