Christian Benitez / Mbed 2 deprecated wheelchaircontrol

Wheelchair control

Dependencies:   BNO055 mbed

Fork of wheelchaircontrol by ryan lin

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?

UserRevisionLine numberNew 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 }