OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

bmu.h@0:a291977ec0b1, 2016-12-05 (annotated)

Committer:
arthicha
Date:
Mon Dec 05 18:31:43 2016 +0000
Revision:
0:a291977ec0b1
Child:
1:d8ce226c8c2e
Publich

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arthicha 0:a291977ec0b1 1 #include "mbed.h"
arthicha 0:a291977ec0b1 2 #include "zmu9250.h"
arthicha 0:a291977ec0b1 3 #include "math.h"
arthicha 0:a291977ec0b1 4
arthicha 0:a291977ec0b1 5
arthicha 0:a291977ec0b1 6 ZMU9250 a;
arthicha 0:a291977ec0b1 7 class bmuimu
arthicha 0:a291977ec0b1 8
arthicha 0:a291977ec0b1 9 {
arthicha 0:a291977ec0b1 10
arthicha 0:a291977ec0b1 11
arthicha 0:a291977ec0b1 12
arthicha 0:a291977ec0b1 13 public:
arthicha 0:a291977ec0b1 14
arthicha 0:a291977ec0b1 15 int yaw_z,roll_x,pitch_y,stard_x,stard_y,stard_z;
arthicha 0:a291977ec0b1 16 int X_pos , Y_pos , Z_pos ;
arthicha 0:a291977ec0b1 17 bool bmuimu:: Getposition(int x, int y, int z){
arthicha 0:a291977ec0b1 18 if( ((this->yaw_z > -40) && (this->yaw_z < 0)) && ((this->roll_x > -30) && (this->roll_x < 30)) && ((this->pitch_y > -20) && (this->pitch_y < 30)) ){
arthicha 0:a291977ec0b1 19 x++; //R x+
arthicha 0:a291977ec0b1 20
arthicha 0:a291977ec0b1 21 }
arthicha 0:a291977ec0b1 22 else if( ((this->yaw_z > -105) && (this->yaw_z < -75))&& ((this->roll_x > -20) && (this->roll_x < 35)) && ((this->pitch_y > -20) && (this->pitch_y < 30)) ){
arthicha 0:a291977ec0b1 23 y--; //B y+
arthicha 0:a291977ec0b1 24
arthicha 0:a291977ec0b1 25 }
arthicha 0:a291977ec0b1 26 else if( ((this->yaw_z > 140) && (this->yaw_z < 175)) && ((this->roll_x > -40) && (this->roll_x < 25)) && ((this->pitch_y > -35) && (this->pitch_y < 25)) ){
arthicha 0:a291977ec0b1 27 y++; //F y-
arthicha 0:a291977ec0b1 28
arthicha 0:a291977ec0b1 29
arthicha 0:a291977ec0b1 30 }
arthicha 0:a291977ec0b1 31 else if( ((this->yaw_z > -155) && (this->yaw_z < -130)) && ((this->roll_x > -25) && (this->roll_x < 10)) && ((this->pitch_y > -20) && (this->pitch_y < 15)) ){
arthicha 0:a291977ec0b1 32 x--; //L x-
arthicha 0:a291977ec0b1 33
arthicha 0:a291977ec0b1 34
arthicha 0:a291977ec0b1 35 }
arthicha 0:a291977ec0b1 36 else if( ((this->yaw_z > -150) && (this->yaw_z < -88)) && ((this->roll_x > 70) && (this->roll_x < 110)) && ((this->pitch_y > -20) && (this->pitch_y < 35))){
arthicha 0:a291977ec0b1 37 z++; //T z+
arthicha 0:a291977ec0b1 38
arthicha 0:a291977ec0b1 39
arthicha 0:a291977ec0b1 40
arthicha 0:a291977ec0b1 41 }
arthicha 0:a291977ec0b1 42 else if( ((this->yaw_z > -125) && (this->yaw_z < -100)) && ((this->roll_x > -100) && (this->roll_x < -75)) && ((this->pitch_y > -25) && (this->pitch_y < 35))){
arthicha 0:a291977ec0b1 43 z--; //D z-
arthicha 0:a291977ec0b1 44
arthicha 0:a291977ec0b1 45
arthicha 0:a291977ec0b1 46 }
arthicha 0:a291977ec0b1 47
arthicha 0:a291977ec0b1 48 else if( ((this->yaw_z > -80) && (this->yaw_z < -40)) && ((this->roll_x > 160) || (this->roll_x < -160)) && ((this->pitch_y > -15) && (this->pitch_y < 15))){
arthicha 0:a291977ec0b1 49 y++;
arthicha 0:a291977ec0b1 50
arthicha 0:a291977ec0b1 51
arthicha 0:a291977ec0b1 52 }
arthicha 0:a291977ec0b1 53 else if( ((this->yaw_z > 160) || (this->yaw_z < -165)) && ((this->roll_x > 165) || (this->roll_x < -165)) && ((this->pitch_y > -10) && (this->pitch_y < 10)) ){
arthicha 0:a291977ec0b1 54 x++;
arthicha 0:a291977ec0b1 55
arthicha 0:a291977ec0b1 56 }
arthicha 0:a291977ec0b1 57 else if( ((this->yaw_z > 100) && (this->yaw_z < 130)) && ((this->roll_x > 165) || (this->roll_x < -165)) && ((this->pitch_y > -20) && (this->pitch_y < 15))){
arthicha 0:a291977ec0b1 58 y--;
arthicha 0:a291977ec0b1 59
arthicha 0:a291977ec0b1 60 }
arthicha 0:a291977ec0b1 61 else if( ((this->yaw_z > 45) && (this->yaw_z < 65)) && ((this->roll_x > 165) || (this->roll_x < -165)) && ((this->pitch_y > -15) && (this->pitch_y < 15)) ){
arthicha 0:a291977ec0b1 62 x--; //L x-
arthicha 0:a291977ec0b1 63
arthicha 0:a291977ec0b1 64
arthicha 0:a291977ec0b1 65 }
arthicha 0:a291977ec0b1 66 else
arthicha 0:a291977ec0b1 67 {
arthicha 0:a291977ec0b1 68 return false;
arthicha 0:a291977ec0b1 69 }
arthicha 0:a291977ec0b1 70 this->X_pos = x;
arthicha 0:a291977ec0b1 71 this->Y_pos = y;
arthicha 0:a291977ec0b1 72 this->Z_pos = z;
arthicha 0:a291977ec0b1 73 return true ;
arthicha 0:a291977ec0b1 74 }
arthicha 0:a291977ec0b1 75
arthicha 0:a291977ec0b1 76 void bmuimu::update() {
arthicha 0:a291977ec0b1 77
arthicha 0:a291977ec0b1 78 a.Update();
arthicha 0:a291977ec0b1 79 this->yaw_z = a.Yaw();
arthicha 0:a291977ec0b1 80 this->pitch_y = a.Pitch();
arthicha 0:a291977ec0b1 81 this->roll_x = a.Roll();
arthicha 0:a291977ec0b1 82
arthicha 0:a291977ec0b1 83 }
arthicha 0:a291977ec0b1 84
arthicha 0:a291977ec0b1 85
arthicha 0:a291977ec0b1 86 int bmuimu::GetX()
arthicha 0:a291977ec0b1 87 {
arthicha 0:a291977ec0b1 88 return X_pos;
arthicha 0:a291977ec0b1 89 }
arthicha 0:a291977ec0b1 90
arthicha 0:a291977ec0b1 91 int bmuimu::GetY()
arthicha 0:a291977ec0b1 92 {
arthicha 0:a291977ec0b1 93 return Y_pos;
arthicha 0:a291977ec0b1 94 }
arthicha 0:a291977ec0b1 95
arthicha 0:a291977ec0b1 96 int bmuimu::GetZ()
arthicha 0:a291977ec0b1 97 {
arthicha 0:a291977ec0b1 98 return Z_pos;
arthicha 0:a291977ec0b1 99 }
arthicha 0:a291977ec0b1 100
arthicha 0:a291977ec0b1 101 int bmuimu::Rotate(){
arthicha 0:a291977ec0b1 102
arthicha 0:a291977ec0b1 103 if(this->yaw_z >= 0){
arthicha 0:a291977ec0b1 104 if((this->yaw_z <= 45) && (this->yaw_z >= 0)){
arthicha 0:a291977ec0b1 105 return 0;
arthicha 0:a291977ec0b1 106 }
arthicha 0:a291977ec0b1 107 if((this->yaw_z <= 90) && (this->yaw_z >= 46)){
arthicha 0:a291977ec0b1 108 return 90;
arthicha 0:a291977ec0b1 109 }
arthicha 0:a291977ec0b1 110 if((this->yaw_z <= 135) && (this->yaw_z >= 91)){
arthicha 0:a291977ec0b1 111 return -90;
arthicha 0:a291977ec0b1 112 }
arthicha 0:a291977ec0b1 113 if((this->yaw_z <= 180) && (this->yaw_z >= 136)){
arthicha 0:a291977ec0b1 114 return 180;
arthicha 0:a291977ec0b1 115 }
arthicha 0:a291977ec0b1 116 }
arthicha 0:a291977ec0b1 117 else{
arthicha 0:a291977ec0b1 118 if((this->yaw_z <= 0) && (this->yaw_z >= -45)){
arthicha 0:a291977ec0b1 119 return 180;
arthicha 0:a291977ec0b1 120 }
arthicha 0:a291977ec0b1 121 if((this->yaw_z <= -46) && (this->yaw_z >= -90)){
arthicha 0:a291977ec0b1 122 return -90;
arthicha 0:a291977ec0b1 123 }
arthicha 0:a291977ec0b1 124 if((this->yaw_z <= -91) && (this->yaw_z >= -135)){
arthicha 0:a291977ec0b1 125 return 90;
arthicha 0:a291977ec0b1 126 }
arthicha 0:a291977ec0b1 127 if((this->yaw_z <= -136) && (this->yaw_z >= -180)){
arthicha 0:a291977ec0b1 128 return 0;
arthicha 0:a291977ec0b1 129 }
arthicha 0:a291977ec0b1 130 }
arthicha 0:a291977ec0b1 131 }
arthicha 0:a291977ec0b1 132
arthicha 0:a291977ec0b1 133 void bmuimu:: stard(){
arthicha 0:a291977ec0b1 134 this->stard_x = this->roll_x;
arthicha 0:a291977ec0b1 135 this->stard_y = this->pitch_y;
arthicha 0:a291977ec0b1 136 this->stard_z = this->yaw_z;
arthicha 0:a291977ec0b1 137 }
arthicha 0:a291977ec0b1 138
arthicha 0:a291977ec0b1 139
arthicha 0:a291977ec0b1 140
arthicha 0:a291977ec0b1 141
arthicha 0:a291977ec0b1 142
arthicha 0:a291977ec0b1 143
arthicha 0:a291977ec0b1 144 };//class