With pid left, right, and foward
Dependents: wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol wheelchaircontrol2 ... more
Fork of chair_BNO055 by
chair_BNO055.cpp
- Committer:
- ryanlin97
- Date:
- 2018-07-20
- Revision:
- 0:ad7a811c859f
- Child:
- 1:3258d62af038
File content as of revision 0:ad7a811c859f:
#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; }