Christian Benitez
/
wheelchaircontrol
Wheelchair control
Fork of wheelchaircontrol by
chair_imu.cpp
- Committer:
- cpbenite
- Date:
- 2018-07-17
- Revision:
- 6:8cd00c26bb47
- Parent:
- 5:e0ccaab3959a
File content as of revision 6:8cd00c26bb47:
#include "chair_imu.h" Serial pc(USBTX, USBRX); Timer t; // Default Initialization chair_imu::chair_imu(){ imu = new BNO055(SDA, SCL); } // Initialize Pins chair_imu::chair_imu(PinName SDA_Pin, PinName SCL_Pin){ imu = new BNO055(SDA_Pin, SCL_Pin); } // Check to make sure BNO055 is properly working void chair_imu::setup(){ imu->reset(); pc.printf("Bosch Sensortec BNO055 test program on \r\n"); while (imu->check() == 0) { pc.printf("Bosch BNO055 is NOT available!!\r\n"); wait(.5); } pc.printf("Bosch Sensortec BNO055 available \r\n"); //Set the SI units & start the timer imu->set_accel_units(MPERSPERS); imu->setmode(OPERATION_MODE_AMG); imu->set_angle_units(DEGREES); imu->get_calib(); imu->write_calibration_data(); imu->setmode(OPERATION_MODE_AMG); //put into while loop t.start(); } // Receive acceleration data (in meters per second) double chair_imu::accel_x(){ imu->get_accel(); return (double)imu->accel.x; } double chair_imu::accel_y(){ imu->get_accel(); return (double)imu->accel.y; } double chair_imu::accel_z(){ imu->get_accel(); return (double)imu->accel.z; } // Receive Gyroscope data (in degrees) double chair_imu::gyro_x(){ imu->get_gyro(); return (double)imu->gyro.x; } double chair_imu::gyro_y(){ imu->get_gyro(); return (double)imu->gyro.y; } double chair_imu::gyro_z(){ imu->get_gyro(); return (double)imu->gyro.z; } // Receive angle relative to North double chair_imu::angle_north(){ imu->get_mag(); float x = imu->mag.x; float y = imu->mag.y; float result = x/y; float angleToNorth; if(imu->mag.y > 0) angleToNorth = 90.0 - atan(result) * 180/PI; else if(imu->mag.y < 0) angleToNorth = 270.0 - atan(result) * 180/PI; else if(y == 0 && x <= 0) angleToNorth = 180; else if(y == 0 && x > 0) angleToNorth = 0; return (double)angleToNorth; } // Rotation about the X-axis double chair_imu::roll(){ imu->get_accel(); imu->get_gyro(); float roll = atan2 (-imu->accel.x , (sqrt ((imu->accel.y * imu->accel.y) + (imu->accel.z * imu->accel.z)) )); roll = roll * 57.3; // Scaling factor t.reset(); return (double)roll; } // Rotation about the Y-axis double chair_imu::pitch(){ imu->get_accel(); imu->get_gyro(); float pitch = atan2 (imu->accel.y , (sqrt ((imu->accel.x * imu->accel.x) + (imu->accel.z * imu->accel.z)) )); pitch = pitch * 57.3; // Scaling Factor t.reset(); return (double)pitch; } // Rotation about the Z-axis double chair_imu::yaw(){ imu->get_gyro(); float yaw = (yaw - t.read() * imu->gyro.z); // Return a yaw value between 0 and 360 degrees if(yaw > 360) yaw -= 360; if(yaw < 0) yaw += 360; t.reset(); return (double)yaw; }