OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

bmu.h@2:ce3ee4bc8cf7, 2016-12-06 (annotated)

Committer:
siwakon
Date:
Tue Dec 06 12:19:49 2016 +0000
Revision:
2:ce3ee4bc8cf7
Parent:
1:d8ce226c8c2e 
ddd

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