OX / Mbed 2 deprecated O_V1

OX V1

Dependencies:   mbed

bmu.h@2:84a84429750a, 2016-12-05 (annotated)

Committer:
arthicha
Date:
Mon Dec 05 16:11:19 2016 +0000
Revision:
2:84a84429750a
Parent:
0:6182212860fb 
hhh;

Who changed what in which revision?

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