Christian Benitez
/
wheelchaircontrol
Wheelchair control
Fork of wheelchaircontrol by
chair_imu.cpp@6:8cd00c26bb47, 2018-07-17 (annotated)
- Committer:
- cpbenite
- Date:
- Tue Jul 17 19:19:26 2018 +0000
- Revision:
- 6:8cd00c26bb47
- Parent:
- 5:e0ccaab3959a
Wheelchair control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 5:e0ccaab3959a | 1 | #include "chair_imu.h" |
cpbenite | 6:8cd00c26bb47 | 2 | |
ryanlin97 | 5:e0ccaab3959a | 3 | Serial pc(USBTX, USBRX); |
ryanlin97 | 5:e0ccaab3959a | 4 | Timer t; |
ryanlin97 | 5:e0ccaab3959a | 5 | |
cpbenite | 6:8cd00c26bb47 | 6 | // Default Initialization |
ryanlin97 | 5:e0ccaab3959a | 7 | chair_imu::chair_imu(){ |
cpbenite | 6:8cd00c26bb47 | 8 | imu = new BNO055(SDA, SCL); |
ryanlin97 | 5:e0ccaab3959a | 9 | } |
ryanlin97 | 5:e0ccaab3959a | 10 | |
cpbenite | 6:8cd00c26bb47 | 11 | // Initialize Pins |
cpbenite | 6:8cd00c26bb47 | 12 | chair_imu::chair_imu(PinName SDA_Pin, PinName SCL_Pin){ |
cpbenite | 6:8cd00c26bb47 | 13 | imu = new BNO055(SDA_Pin, SCL_Pin); |
ryanlin97 | 5:e0ccaab3959a | 14 | } |
cpbenite | 6:8cd00c26bb47 | 15 | |
cpbenite | 6:8cd00c26bb47 | 16 | // Check to make sure BNO055 is properly working |
cpbenite | 6:8cd00c26bb47 | 17 | void chair_imu::setup(){ |
cpbenite | 6:8cd00c26bb47 | 18 | imu->reset(); |
cpbenite | 6:8cd00c26bb47 | 19 | pc.printf("Bosch Sensortec BNO055 test program on \r\n"); |
cpbenite | 6:8cd00c26bb47 | 20 | while (imu->check() == 0) { |
cpbenite | 6:8cd00c26bb47 | 21 | pc.printf("Bosch BNO055 is NOT available!!\r\n"); |
cpbenite | 6:8cd00c26bb47 | 22 | wait(.5); |
cpbenite | 6:8cd00c26bb47 | 23 | } |
cpbenite | 6:8cd00c26bb47 | 24 | pc.printf("Bosch Sensortec BNO055 available \r\n"); |
ryanlin97 | 5:e0ccaab3959a | 25 | |
cpbenite | 6:8cd00c26bb47 | 26 | //Set the SI units & start the timer |
cpbenite | 6:8cd00c26bb47 | 27 | imu->set_accel_units(MPERSPERS); |
cpbenite | 6:8cd00c26bb47 | 28 | imu->setmode(OPERATION_MODE_AMG); |
cpbenite | 6:8cd00c26bb47 | 29 | imu->set_angle_units(DEGREES); |
cpbenite | 6:8cd00c26bb47 | 30 | imu->get_calib(); |
cpbenite | 6:8cd00c26bb47 | 31 | imu->write_calibration_data(); |
cpbenite | 6:8cd00c26bb47 | 32 | imu->setmode(OPERATION_MODE_AMG); //put into while loop |
cpbenite | 6:8cd00c26bb47 | 33 | t.start(); |
cpbenite | 6:8cd00c26bb47 | 34 | } |
cpbenite | 6:8cd00c26bb47 | 35 | |
cpbenite | 6:8cd00c26bb47 | 36 | // Receive acceleration data (in meters per second) |
cpbenite | 6:8cd00c26bb47 | 37 | double chair_imu::accel_x(){ |
cpbenite | 6:8cd00c26bb47 | 38 | imu->get_accel(); |
cpbenite | 6:8cd00c26bb47 | 39 | return (double)imu->accel.x; |
cpbenite | 6:8cd00c26bb47 | 40 | } |
cpbenite | 6:8cd00c26bb47 | 41 | |
cpbenite | 6:8cd00c26bb47 | 42 | double chair_imu::accel_y(){ |
cpbenite | 6:8cd00c26bb47 | 43 | imu->get_accel(); |
cpbenite | 6:8cd00c26bb47 | 44 | return (double)imu->accel.y; |
ryanlin97 | 5:e0ccaab3959a | 45 | } |
cpbenite | 6:8cd00c26bb47 | 46 | |
cpbenite | 6:8cd00c26bb47 | 47 | double chair_imu::accel_z(){ |
cpbenite | 6:8cd00c26bb47 | 48 | imu->get_accel(); |
cpbenite | 6:8cd00c26bb47 | 49 | return (double)imu->accel.z; |
ryanlin97 | 5:e0ccaab3959a | 50 | } |
cpbenite | 6:8cd00c26bb47 | 51 | |
cpbenite | 6:8cd00c26bb47 | 52 | |
cpbenite | 6:8cd00c26bb47 | 53 | // Receive Gyroscope data (in degrees) |
cpbenite | 6:8cd00c26bb47 | 54 | double chair_imu::gyro_x(){ |
cpbenite | 6:8cd00c26bb47 | 55 | imu->get_gyro(); |
cpbenite | 6:8cd00c26bb47 | 56 | return (double)imu->gyro.x; |
cpbenite | 6:8cd00c26bb47 | 57 | } |
cpbenite | 6:8cd00c26bb47 | 58 | |
cpbenite | 6:8cd00c26bb47 | 59 | double chair_imu::gyro_y(){ |
cpbenite | 6:8cd00c26bb47 | 60 | imu->get_gyro(); |
cpbenite | 6:8cd00c26bb47 | 61 | return (double)imu->gyro.y; |
cpbenite | 6:8cd00c26bb47 | 62 | } |
cpbenite | 6:8cd00c26bb47 | 63 | |
cpbenite | 6:8cd00c26bb47 | 64 | double chair_imu::gyro_z(){ |
cpbenite | 6:8cd00c26bb47 | 65 | imu->get_gyro(); |
cpbenite | 6:8cd00c26bb47 | 66 | return (double)imu->gyro.z; |
ryanlin97 | 5:e0ccaab3959a | 67 | } |
ryanlin97 | 5:e0ccaab3959a | 68 | |
ryanlin97 | 5:e0ccaab3959a | 69 | |
cpbenite | 6:8cd00c26bb47 | 70 | // Receive angle relative to North |
cpbenite | 6:8cd00c26bb47 | 71 | double chair_imu::angle_north(){ |
cpbenite | 6:8cd00c26bb47 | 72 | imu->get_mag(); |
cpbenite | 6:8cd00c26bb47 | 73 | float x = imu->mag.x; |
cpbenite | 6:8cd00c26bb47 | 74 | float y = imu->mag.y; |
cpbenite | 6:8cd00c26bb47 | 75 | |
cpbenite | 6:8cd00c26bb47 | 76 | float result = x/y; |
cpbenite | 6:8cd00c26bb47 | 77 | float angleToNorth; |
cpbenite | 6:8cd00c26bb47 | 78 | |
cpbenite | 6:8cd00c26bb47 | 79 | if(imu->mag.y > 0) |
cpbenite | 6:8cd00c26bb47 | 80 | angleToNorth = 90.0 - atan(result) * 180/PI; |
cpbenite | 6:8cd00c26bb47 | 81 | else if(imu->mag.y < 0) |
cpbenite | 6:8cd00c26bb47 | 82 | angleToNorth = 270.0 - atan(result) * 180/PI; |
cpbenite | 6:8cd00c26bb47 | 83 | else if(y == 0 && x <= 0) |
cpbenite | 6:8cd00c26bb47 | 84 | angleToNorth = 180; |
cpbenite | 6:8cd00c26bb47 | 85 | else if(y == 0 && x > 0) |
cpbenite | 6:8cd00c26bb47 | 86 | angleToNorth = 0; |
cpbenite | 6:8cd00c26bb47 | 87 | |
cpbenite | 6:8cd00c26bb47 | 88 | return (double)angleToNorth; |
cpbenite | 6:8cd00c26bb47 | 89 | } |
cpbenite | 6:8cd00c26bb47 | 90 | |
cpbenite | 6:8cd00c26bb47 | 91 | // Rotation about the X-axis |
cpbenite | 6:8cd00c26bb47 | 92 | double chair_imu::roll(){ |
cpbenite | 6:8cd00c26bb47 | 93 | imu->get_accel(); |
cpbenite | 6:8cd00c26bb47 | 94 | imu->get_gyro(); |
cpbenite | 6:8cd00c26bb47 | 95 | |
cpbenite | 6:8cd00c26bb47 | 96 | float roll = atan2 (-imu->accel.x , (sqrt ((imu->accel.y * imu->accel.y) + |
cpbenite | 6:8cd00c26bb47 | 97 | (imu->accel.z * imu->accel.z)) )); |
cpbenite | 6:8cd00c26bb47 | 98 | roll = roll * 57.3; // Scaling factor |
cpbenite | 6:8cd00c26bb47 | 99 | |
cpbenite | 6:8cd00c26bb47 | 100 | t.reset(); |
cpbenite | 6:8cd00c26bb47 | 101 | |
cpbenite | 6:8cd00c26bb47 | 102 | return (double)roll; |
cpbenite | 6:8cd00c26bb47 | 103 | } |
cpbenite | 6:8cd00c26bb47 | 104 | |
cpbenite | 6:8cd00c26bb47 | 105 | // Rotation about the Y-axis |
cpbenite | 6:8cd00c26bb47 | 106 | double chair_imu::pitch(){ |
cpbenite | 6:8cd00c26bb47 | 107 | imu->get_accel(); |
cpbenite | 6:8cd00c26bb47 | 108 | imu->get_gyro(); |
cpbenite | 6:8cd00c26bb47 | 109 | |
cpbenite | 6:8cd00c26bb47 | 110 | float pitch = atan2 (imu->accel.y , (sqrt ((imu->accel.x * imu->accel.x) + |
cpbenite | 6:8cd00c26bb47 | 111 | (imu->accel.z * imu->accel.z)) )); |
cpbenite | 6:8cd00c26bb47 | 112 | pitch = pitch * 57.3; // Scaling Factor |
cpbenite | 6:8cd00c26bb47 | 113 | |
cpbenite | 6:8cd00c26bb47 | 114 | t.reset(); |
cpbenite | 6:8cd00c26bb47 | 115 | |
cpbenite | 6:8cd00c26bb47 | 116 | return (double)pitch; |
cpbenite | 6:8cd00c26bb47 | 117 | } |
cpbenite | 6:8cd00c26bb47 | 118 | |
cpbenite | 6:8cd00c26bb47 | 119 | // Rotation about the Z-axis |
cpbenite | 6:8cd00c26bb47 | 120 | double chair_imu::yaw(){ |
cpbenite | 6:8cd00c26bb47 | 121 | imu->get_gyro(); |
cpbenite | 6:8cd00c26bb47 | 122 | float yaw = (yaw - t.read() * imu->gyro.z); |
cpbenite | 6:8cd00c26bb47 | 123 | |
cpbenite | 6:8cd00c26bb47 | 124 | // Return a yaw value between 0 and 360 degrees |
cpbenite | 6:8cd00c26bb47 | 125 | if(yaw > 360) |
cpbenite | 6:8cd00c26bb47 | 126 | yaw -= 360; |
cpbenite | 6:8cd00c26bb47 | 127 | if(yaw < 0) |
cpbenite | 6:8cd00c26bb47 | 128 | yaw += 360; |
cpbenite | 6:8cd00c26bb47 | 129 | |
cpbenite | 6:8cd00c26bb47 | 130 | t.reset(); |
cpbenite | 6:8cd00c26bb47 | 131 | |
cpbenite | 6:8cd00c26bb47 | 132 | return (double)yaw; |
cpbenite | 6:8cd00c26bb47 | 133 | } |