Christian Benitez / Mbed 2 deprecated wheelchaircontrol

Wheelchair control

Dependencies:   BNO055 mbed

Fork of wheelchaircontrol by ryan lin

chair_imu.cpp@5:e0ccaab3959a, 2018-07-17 (annotated)

Committer:
ryanlin97
Date:
Tue Jul 17 07:19:04 2018 +0000
Revision:
5:e0ccaab3959a
Child:
6:8cd00c26bb47
imu added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 5:e0ccaab3959a 1 #include "chair_imu.h"
ryanlin97 5:e0ccaab3959a 2
ryanlin97 5:e0ccaab3959a 3 Serial pc(USBTX, USBRX);
ryanlin97 5:e0ccaab3959a 4 Timer t;
ryanlin97 5:e0ccaab3959a 5
ryanlin97 5:e0ccaab3959a 6 chair_imu::chair_imu(){
ryanlin97 5:e0ccaab3959a 7 imu = new BNO055(SDA, SCL);
ryanlin97 5:e0ccaab3959a 8 }
ryanlin97 5:e0ccaab3959a 9
ryanlin97 5:e0ccaab3959a 10 chair_imu::chair_imu(PinName sda_pin, PinName scl_pin){
ryanlin97 5:e0ccaab3959a 11 imu = new BNO055(sda_pin, scl_pin);
ryanlin97 5:e0ccaab3959a 12 }
ryanlin97 5:e0ccaab3959a 13
ryanlin97 5:e0ccaab3959a 14 void chair_imu::setup(){
ryanlin97 5:e0ccaab3959a 15 imu->reset();
ryanlin97 5:e0ccaab3959a 16 pc.printf("Bosch Sensortec BNO055 test program on \r\n");
ryanlin97 5:e0ccaab3959a 17 while (imu->check() == 0) {
ryanlin97 5:e0ccaab3959a 18 pc.printf("Bosch BNO055 is NOT available!!\r\n");
ryanlin97 5:e0ccaab3959a 19 wait(.5);
ryanlin97 5:e0ccaab3959a 20 }
ryanlin97 5:e0ccaab3959a 21 pc.printf("Bosch Sensortec BNO055 available \r\n");
ryanlin97 5:e0ccaab3959a 22 imu->set_accel_units(MPERSPERS);
ryanlin97 5:e0ccaab3959a 23 imu->setmode(OPERATION_MODE_AMG);
ryanlin97 5:e0ccaab3959a 24 imu->get_calib();
ryanlin97 5:e0ccaab3959a 25 imu->write_calibration_data();
ryanlin97 5:e0ccaab3959a 26 imu->set_angle_units(DEGREES);
ryanlin97 5:e0ccaab3959a 27 imu->setmode(OPERATION_MODE_AMG); //put into while loop
ryanlin97 5:e0ccaab3959a 28 t.start();
ryanlin97 5:e0ccaab3959a 29 }
ryanlin97 5:e0ccaab3959a 30
ryanlin97 5:e0ccaab3959a 31 double chair_imu::accel_x(){
ryanlin97 5:e0ccaab3959a 32 imu->get_accel();
ryanlin97 5:e0ccaab3959a 33 return (double)imu->accel.x;
ryanlin97 5:e0ccaab3959a 34 }
ryanlin97 5:e0ccaab3959a 35
ryanlin97 5:e0ccaab3959a 36 double chair_imu::accel_y(){
ryanlin97 5:e0ccaab3959a 37 imu->get_accel();
ryanlin97 5:e0ccaab3959a 38 return (double)imu->accel.y;
ryanlin97 5:e0ccaab3959a 39 }
ryanlin97 5:e0ccaab3959a 40
ryanlin97 5:e0ccaab3959a 41 double chair_imu::accel_z(){
ryanlin97 5:e0ccaab3959a 42 imu->get_accel();
ryanlin97 5:e0ccaab3959a 43 return (double)imu->accel.z;
ryanlin97 5:e0ccaab3959a 44 }
ryanlin97 5:e0ccaab3959a 45
ryanlin97 5:e0ccaab3959a 46 double chair_imu::gyro_x(){
ryanlin97 5:e0ccaab3959a 47 imu->get_gyro();
ryanlin97 5:e0ccaab3959a 48 return (double)imu->gyro.x;
ryanlin97 5:e0ccaab3959a 49 }
ryanlin97 5:e0ccaab3959a 50
ryanlin97 5:e0ccaab3959a 51 double chair_imu::gyro_y(){
ryanlin97 5:e0ccaab3959a 52 imu->get_gyro();
ryanlin97 5:e0ccaab3959a 53 return (double)imu->gyro.y;
ryanlin97 5:e0ccaab3959a 54 }
ryanlin97 5:e0ccaab3959a 55
ryanlin97 5:e0ccaab3959a 56 double chair_imu::gyro_z(){
ryanlin97 5:e0ccaab3959a 57 imu->get_gyro();
ryanlin97 5:e0ccaab3959a 58 return (double)imu->gyro.z;
ryanlin97 5:e0ccaab3959a 59 }
ryanlin97 5:e0ccaab3959a 60
ryanlin97 5:e0ccaab3959a 61 double chair_imu::angle_north(){
ryanlin97 5:e0ccaab3959a 62 imu->get_mag();
ryanlin97 5:e0ccaab3959a 63 float x = imu->mag.x;
ryanlin97 5:e0ccaab3959a 64 float y = imu->mag.y;
ryanlin97 5:e0ccaab3959a 65
ryanlin97 5:e0ccaab3959a 66 float result = x/y;
ryanlin97 5:e0ccaab3959a 67
ryanlin97 5:e0ccaab3959a 68 float angleToNorth;
ryanlin97 5:e0ccaab3959a 69 if(imu->mag.y>0)
ryanlin97 5:e0ccaab3959a 70 angleToNorth = 90.0 - atan(result)*180/PI;
ryanlin97 5:e0ccaab3959a 71 else if(imu->mag.y<0)
ryanlin97 5:e0ccaab3959a 72 angleToNorth = 270.0 - atan(result)*180/PI;
ryanlin97 5:e0ccaab3959a 73 else if(y == 0 && x <= 0)
ryanlin97 5:e0ccaab3959a 74 angleToNorth = 180;
ryanlin97 5:e0ccaab3959a 75 else if(y == 0 && x > 0)
ryanlin97 5:e0ccaab3959a 76 angleToNorth = 0;
ryanlin97 5:e0ccaab3959a 77
ryanlin97 5:e0ccaab3959a 78 return (double)angleToNorth;
ryanlin97 5:e0ccaab3959a 79 }
ryanlin97 5:e0ccaab3959a 80
ryanlin97 5:e0ccaab3959a 81 double chair_imu::roll(){
ryanlin97 5:e0ccaab3959a 82 imu->get_accel();
ryanlin97 5:e0ccaab3959a 83 imu->get_gyro();
ryanlin97 5:e0ccaab3959a 84
ryanlin97 5:e0ccaab3959a 85 float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) +
ryanlin97 5:e0ccaab3959a 86 (imu->accel.z * imu->accel.z))));
ryanlin97 5:e0ccaab3959a 87 roll = roll*57.3;
ryanlin97 5:e0ccaab3959a 88
ryanlin97 5:e0ccaab3959a 89 t.reset();
ryanlin97 5:e0ccaab3959a 90
ryanlin97 5:e0ccaab3959a 91 return (double)roll;
ryanlin97 5:e0ccaab3959a 92 }
ryanlin97 5:e0ccaab3959a 93
ryanlin97 5:e0ccaab3959a 94 double chair_imu::pitch(){
ryanlin97 5:e0ccaab3959a 95 imu->get_accel();
ryanlin97 5:e0ccaab3959a 96 imu->get_gyro();
ryanlin97 5:e0ccaab3959a 97
ryanlin97 5:e0ccaab3959a 98 float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) +
ryanlin97 5:e0ccaab3959a 99 (imu->accel.z * imu->accel.z))));
ryanlin97 5:e0ccaab3959a 100 pitch = pitch*57.3;
ryanlin97 5:e0ccaab3959a 101
ryanlin97 5:e0ccaab3959a 102 t.reset();
ryanlin97 5:e0ccaab3959a 103
ryanlin97 5:e0ccaab3959a 104 return (double)pitch;
ryanlin97 5:e0ccaab3959a 105 }
ryanlin97 5:e0ccaab3959a 106
ryanlin97 5:e0ccaab3959a 107 double chair_imu::yaw(){
ryanlin97 5:e0ccaab3959a 108 imu->get_gyro();
ryanlin97 5:e0ccaab3959a 109 float yaw = (yaw - t.read()*imu->gyro.z);
ryanlin97 5:e0ccaab3959a 110
ryanlin97 5:e0ccaab3959a 111 if(yaw > 360)
ryanlin97 5:e0ccaab3959a 112 yaw -= 360;
ryanlin97 5:e0ccaab3959a 113 if(yaw < 0)
ryanlin97 5:e0ccaab3959a 114 yaw += 360;
ryanlin97 5:e0ccaab3959a 115
ryanlin97 5:e0ccaab3959a 116 t.reset();
ryanlin97 5:e0ccaab3959a 117
ryanlin97 5:e0ccaab3959a 118 return (double)yaw;
ryanlin97 5:e0ccaab3959a 119 }
ryanlin97 5:e0ccaab3959a 120
ryanlin97 5:e0ccaab3959a 121