![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Wheelchair control
Fork of wheelchaircontrol by
chair_imu.cpp@5:e0ccaab3959a, 2018-07-17 (annotated)
- Committer:
- ryanlin97
- Date:
- Tue Jul 17 07:19:04 2018 +0000
- Revision:
- 5:e0ccaab3959a
- Child:
- 6:8cd00c26bb47
imu added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 5:e0ccaab3959a | 1 | #include "chair_imu.h" |
ryanlin97 | 5:e0ccaab3959a | 2 | |
ryanlin97 | 5:e0ccaab3959a | 3 | Serial pc(USBTX, USBRX); |
ryanlin97 | 5:e0ccaab3959a | 4 | Timer t; |
ryanlin97 | 5:e0ccaab3959a | 5 | |
ryanlin97 | 5:e0ccaab3959a | 6 | chair_imu::chair_imu(){ |
ryanlin97 | 5:e0ccaab3959a | 7 | imu = new BNO055(SDA, SCL); |
ryanlin97 | 5:e0ccaab3959a | 8 | } |
ryanlin97 | 5:e0ccaab3959a | 9 | |
ryanlin97 | 5:e0ccaab3959a | 10 | chair_imu::chair_imu(PinName sda_pin, PinName scl_pin){ |
ryanlin97 | 5:e0ccaab3959a | 11 | imu = new BNO055(sda_pin, scl_pin); |
ryanlin97 | 5:e0ccaab3959a | 12 | } |
ryanlin97 | 5:e0ccaab3959a | 13 | |
ryanlin97 | 5:e0ccaab3959a | 14 | void chair_imu::setup(){ |
ryanlin97 | 5:e0ccaab3959a | 15 | imu->reset(); |
ryanlin97 | 5:e0ccaab3959a | 16 | pc.printf("Bosch Sensortec BNO055 test program on \r\n"); |
ryanlin97 | 5:e0ccaab3959a | 17 | while (imu->check() == 0) { |
ryanlin97 | 5:e0ccaab3959a | 18 | pc.printf("Bosch BNO055 is NOT available!!\r\n"); |
ryanlin97 | 5:e0ccaab3959a | 19 | wait(.5); |
ryanlin97 | 5:e0ccaab3959a | 20 | } |
ryanlin97 | 5:e0ccaab3959a | 21 | pc.printf("Bosch Sensortec BNO055 available \r\n"); |
ryanlin97 | 5:e0ccaab3959a | 22 | imu->set_accel_units(MPERSPERS); |
ryanlin97 | 5:e0ccaab3959a | 23 | imu->setmode(OPERATION_MODE_AMG); |
ryanlin97 | 5:e0ccaab3959a | 24 | imu->get_calib(); |
ryanlin97 | 5:e0ccaab3959a | 25 | imu->write_calibration_data(); |
ryanlin97 | 5:e0ccaab3959a | 26 | imu->set_angle_units(DEGREES); |
ryanlin97 | 5:e0ccaab3959a | 27 | imu->setmode(OPERATION_MODE_AMG); //put into while loop |
ryanlin97 | 5:e0ccaab3959a | 28 | t.start(); |
ryanlin97 | 5:e0ccaab3959a | 29 | } |
ryanlin97 | 5:e0ccaab3959a | 30 | |
ryanlin97 | 5:e0ccaab3959a | 31 | double chair_imu::accel_x(){ |
ryanlin97 | 5:e0ccaab3959a | 32 | imu->get_accel(); |
ryanlin97 | 5:e0ccaab3959a | 33 | return (double)imu->accel.x; |
ryanlin97 | 5:e0ccaab3959a | 34 | } |
ryanlin97 | 5:e0ccaab3959a | 35 | |
ryanlin97 | 5:e0ccaab3959a | 36 | double chair_imu::accel_y(){ |
ryanlin97 | 5:e0ccaab3959a | 37 | imu->get_accel(); |
ryanlin97 | 5:e0ccaab3959a | 38 | return (double)imu->accel.y; |
ryanlin97 | 5:e0ccaab3959a | 39 | } |
ryanlin97 | 5:e0ccaab3959a | 40 | |
ryanlin97 | 5:e0ccaab3959a | 41 | double chair_imu::accel_z(){ |
ryanlin97 | 5:e0ccaab3959a | 42 | imu->get_accel(); |
ryanlin97 | 5:e0ccaab3959a | 43 | return (double)imu->accel.z; |
ryanlin97 | 5:e0ccaab3959a | 44 | } |
ryanlin97 | 5:e0ccaab3959a | 45 | |
ryanlin97 | 5:e0ccaab3959a | 46 | double chair_imu::gyro_x(){ |
ryanlin97 | 5:e0ccaab3959a | 47 | imu->get_gyro(); |
ryanlin97 | 5:e0ccaab3959a | 48 | return (double)imu->gyro.x; |
ryanlin97 | 5:e0ccaab3959a | 49 | } |
ryanlin97 | 5:e0ccaab3959a | 50 | |
ryanlin97 | 5:e0ccaab3959a | 51 | double chair_imu::gyro_y(){ |
ryanlin97 | 5:e0ccaab3959a | 52 | imu->get_gyro(); |
ryanlin97 | 5:e0ccaab3959a | 53 | return (double)imu->gyro.y; |
ryanlin97 | 5:e0ccaab3959a | 54 | } |
ryanlin97 | 5:e0ccaab3959a | 55 | |
ryanlin97 | 5:e0ccaab3959a | 56 | double chair_imu::gyro_z(){ |
ryanlin97 | 5:e0ccaab3959a | 57 | imu->get_gyro(); |
ryanlin97 | 5:e0ccaab3959a | 58 | return (double)imu->gyro.z; |
ryanlin97 | 5:e0ccaab3959a | 59 | } |
ryanlin97 | 5:e0ccaab3959a | 60 | |
ryanlin97 | 5:e0ccaab3959a | 61 | double chair_imu::angle_north(){ |
ryanlin97 | 5:e0ccaab3959a | 62 | imu->get_mag(); |
ryanlin97 | 5:e0ccaab3959a | 63 | float x = imu->mag.x; |
ryanlin97 | 5:e0ccaab3959a | 64 | float y = imu->mag.y; |
ryanlin97 | 5:e0ccaab3959a | 65 | |
ryanlin97 | 5:e0ccaab3959a | 66 | float result = x/y; |
ryanlin97 | 5:e0ccaab3959a | 67 | |
ryanlin97 | 5:e0ccaab3959a | 68 | float angleToNorth; |
ryanlin97 | 5:e0ccaab3959a | 69 | if(imu->mag.y>0) |
ryanlin97 | 5:e0ccaab3959a | 70 | angleToNorth = 90.0 - atan(result)*180/PI; |
ryanlin97 | 5:e0ccaab3959a | 71 | else if(imu->mag.y<0) |
ryanlin97 | 5:e0ccaab3959a | 72 | angleToNorth = 270.0 - atan(result)*180/PI; |
ryanlin97 | 5:e0ccaab3959a | 73 | else if(y == 0 && x <= 0) |
ryanlin97 | 5:e0ccaab3959a | 74 | angleToNorth = 180; |
ryanlin97 | 5:e0ccaab3959a | 75 | else if(y == 0 && x > 0) |
ryanlin97 | 5:e0ccaab3959a | 76 | angleToNorth = 0; |
ryanlin97 | 5:e0ccaab3959a | 77 | |
ryanlin97 | 5:e0ccaab3959a | 78 | return (double)angleToNorth; |
ryanlin97 | 5:e0ccaab3959a | 79 | } |
ryanlin97 | 5:e0ccaab3959a | 80 | |
ryanlin97 | 5:e0ccaab3959a | 81 | double chair_imu::roll(){ |
ryanlin97 | 5:e0ccaab3959a | 82 | imu->get_accel(); |
ryanlin97 | 5:e0ccaab3959a | 83 | imu->get_gyro(); |
ryanlin97 | 5:e0ccaab3959a | 84 | |
ryanlin97 | 5:e0ccaab3959a | 85 | float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) + |
ryanlin97 | 5:e0ccaab3959a | 86 | (imu->accel.z * imu->accel.z)))); |
ryanlin97 | 5:e0ccaab3959a | 87 | roll = roll*57.3; |
ryanlin97 | 5:e0ccaab3959a | 88 | |
ryanlin97 | 5:e0ccaab3959a | 89 | t.reset(); |
ryanlin97 | 5:e0ccaab3959a | 90 | |
ryanlin97 | 5:e0ccaab3959a | 91 | return (double)roll; |
ryanlin97 | 5:e0ccaab3959a | 92 | } |
ryanlin97 | 5:e0ccaab3959a | 93 | |
ryanlin97 | 5:e0ccaab3959a | 94 | double chair_imu::pitch(){ |
ryanlin97 | 5:e0ccaab3959a | 95 | imu->get_accel(); |
ryanlin97 | 5:e0ccaab3959a | 96 | imu->get_gyro(); |
ryanlin97 | 5:e0ccaab3959a | 97 | |
ryanlin97 | 5:e0ccaab3959a | 98 | float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) + |
ryanlin97 | 5:e0ccaab3959a | 99 | (imu->accel.z * imu->accel.z)))); |
ryanlin97 | 5:e0ccaab3959a | 100 | pitch = pitch*57.3; |
ryanlin97 | 5:e0ccaab3959a | 101 | |
ryanlin97 | 5:e0ccaab3959a | 102 | t.reset(); |
ryanlin97 | 5:e0ccaab3959a | 103 | |
ryanlin97 | 5:e0ccaab3959a | 104 | return (double)pitch; |
ryanlin97 | 5:e0ccaab3959a | 105 | } |
ryanlin97 | 5:e0ccaab3959a | 106 | |
ryanlin97 | 5:e0ccaab3959a | 107 | double chair_imu::yaw(){ |
ryanlin97 | 5:e0ccaab3959a | 108 | imu->get_gyro(); |
ryanlin97 | 5:e0ccaab3959a | 109 | float yaw = (yaw - t.read()*imu->gyro.z); |
ryanlin97 | 5:e0ccaab3959a | 110 | |
ryanlin97 | 5:e0ccaab3959a | 111 | if(yaw > 360) |
ryanlin97 | 5:e0ccaab3959a | 112 | yaw -= 360; |
ryanlin97 | 5:e0ccaab3959a | 113 | if(yaw < 0) |
ryanlin97 | 5:e0ccaab3959a | 114 | yaw += 360; |
ryanlin97 | 5:e0ccaab3959a | 115 | |
ryanlin97 | 5:e0ccaab3959a | 116 | t.reset(); |
ryanlin97 | 5:e0ccaab3959a | 117 | |
ryanlin97 | 5:e0ccaab3959a | 118 | return (double)yaw; |
ryanlin97 | 5:e0ccaab3959a | 119 | } |
ryanlin97 | 5:e0ccaab3959a | 120 | |
ryanlin97 | 5:e0ccaab3959a | 121 |