With pid left, right, and foward
Dependents: wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol wheelchaircontrol2 ... more
Fork of chair_BNO055 by
Diff: chair_BNO055.cpp
- Revision:
- 0:ad7a811c859f
- Child:
- 1:3258d62af038
diff -r 000000000000 -r ad7a811c859f chair_BNO055.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/chair_BNO055.cpp Fri Jul 20 17:54:09 2018 +0000 @@ -0,0 +1,118 @@ +#include "chair_BNO055.h" + +Timer t; + + +chair_BNO055::chair_BNO055(){ + imu = new BNO055(SDA, SCL); +} + +chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin){ + imu = new BNO055(sda_pin, scl_pin); +} + +void chair_BNO055::setup(){ + imu->reset(); + while (imu->check() == 0) { + wait(.5); + } + imu->set_accel_units(MPERSPERS); + imu->setmode(OPERATION_MODE_AMG); + imu->get_calib(); + imu->write_calibration_data(); + imu->set_angle_units(DEGREES); + imu->setmode(OPERATION_MODE_AMG); //put into while loop + t.start(); +} + +double chair_BNO055::accel_x(){ + imu->get_accel(); + return (double)imu->accel.x; +} + +double chair_BNO055::accel_y(){ + imu->get_accel(); + return (double)imu->accel.y; +} + +double chair_BNO055::accel_z(){ + imu->get_accel(); + return (double)imu->accel.z; +} + +double chair_BNO055::gyro_x(){ + imu->get_gyro(); + return (double)imu->gyro.x; +} + +double chair_BNO055::gyro_y(){ + imu->get_gyro(); + return (double)imu->gyro.y; +} + +double chair_BNO055::gyro_z(){ + imu->get_gyro(); + return (double)imu->gyro.z; +} + +double chair_BNO055::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; +} + +double chair_BNO055::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; + + t.reset(); + + return (double)roll; +} + +double chair_BNO055::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; + + t.reset(); + + return (double)pitch; +} + +double chair_BNO055::yaw(){ + imu->get_gyro(); + float yaw = (yaw - t.read()*imu->gyro.z); + + if(yaw > 360) + yaw -= 360; + if(yaw < 0) + yaw += 360; + + t.reset(); + + return (double)yaw; +} + +