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