OX / Mbed 2 deprecated class_IMU_V1

test

Dependencies:   mbed

main.cpp@0:dd400e4fe461, 2016-12-05 (annotated)

Committer:
siwakon
Date:
Mon Dec 05 14:47:17 2016 +0000
Revision:
0:dd400e4fe461
asdasdasd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
siwakon 0:dd400e4fe461 1 #include "mbed.h"
siwakon 0:dd400e4fe461 2 #include "zmu9250.h"
siwakon 0:dd400e4fe461 3 #include "math.h"
siwakon 0:dd400e4fe461 4
siwakon 0:dd400e4fe461 5 class bmuimu
siwakon 0:dd400e4fe461 6
siwakon 0:dd400e4fe461 7 {
siwakon 0:dd400e4fe461 8 public:
siwakon 0:dd400e4fe461 9 int yaw_z,roll_x,pitch_y,stard_x,stard_y,stard_z;
siwakon 0:dd400e4fe461 10 int bmuimu:: cube(int x, int y, int z){
siwakon 0:dd400e4fe461 11 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)) ){
siwakon 0:dd400e4fe461 12 x++; //R x+
siwakon 0:dd400e4fe461 13
siwakon 0:dd400e4fe461 14 }
siwakon 0:dd400e4fe461 15 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)) ){
siwakon 0:dd400e4fe461 16 y--; //B y+
siwakon 0:dd400e4fe461 17
siwakon 0:dd400e4fe461 18 }
siwakon 0:dd400e4fe461 19 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)) ){
siwakon 0:dd400e4fe461 20 y++; //F y-
siwakon 0:dd400e4fe461 21
siwakon 0:dd400e4fe461 22
siwakon 0:dd400e4fe461 23 }
siwakon 0:dd400e4fe461 24 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)) ){
siwakon 0:dd400e4fe461 25 x--; //L x-
siwakon 0:dd400e4fe461 26
siwakon 0:dd400e4fe461 27
siwakon 0:dd400e4fe461 28 }
siwakon 0:dd400e4fe461 29 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))){
siwakon 0:dd400e4fe461 30 z++; //T z+
siwakon 0:dd400e4fe461 31
siwakon 0:dd400e4fe461 32
siwakon 0:dd400e4fe461 33
siwakon 0:dd400e4fe461 34 }
siwakon 0:dd400e4fe461 35 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))){
siwakon 0:dd400e4fe461 36 z--; //D z-
siwakon 0:dd400e4fe461 37
siwakon 0:dd400e4fe461 38
siwakon 0:dd400e4fe461 39 }
siwakon 0:dd400e4fe461 40
siwakon 0:dd400e4fe461 41 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))){
siwakon 0:dd400e4fe461 42 y++;
siwakon 0:dd400e4fe461 43
siwakon 0:dd400e4fe461 44
siwakon 0:dd400e4fe461 45 }
siwakon 0:dd400e4fe461 46 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)) ){
siwakon 0:dd400e4fe461 47 x++;
siwakon 0:dd400e4fe461 48
siwakon 0:dd400e4fe461 49 }
siwakon 0:dd400e4fe461 50 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))){
siwakon 0:dd400e4fe461 51 y--;
siwakon 0:dd400e4fe461 52
siwakon 0:dd400e4fe461 53 }
siwakon 0:dd400e4fe461 54 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)) ){
siwakon 0:dd400e4fe461 55 x--; //L x-
siwakon 0:dd400e4fe461 56
siwakon 0:dd400e4fe461 57
siwakon 0:dd400e4fe461 58 }
siwakon 0:dd400e4fe461 59 else{
siwakon 0:dd400e4fe461 60 return 0;
siwakon 0:dd400e4fe461 61 }
siwakon 0:dd400e4fe461 62 return x*100+y*10+z ;
siwakon 0:dd400e4fe461 63 }
siwakon 0:dd400e4fe461 64
siwakon 0:dd400e4fe461 65 void bmuimu::update() {
siwakon 0:dd400e4fe461 66 ZMU9250 a;
siwakon 0:dd400e4fe461 67 a.Update();
siwakon 0:dd400e4fe461 68 this->yaw_z = a.Yaw();
siwakon 0:dd400e4fe461 69 this->pitch_y = a.Pitch();
siwakon 0:dd400e4fe461 70 this->roll_x = a.Roll();
siwakon 0:dd400e4fe461 71
siwakon 0:dd400e4fe461 72 }
siwakon 0:dd400e4fe461 73
siwakon 0:dd400e4fe461 74 int bmuimu::rotate(int ok_rotate){
siwakon 0:dd400e4fe461 75
siwakon 0:dd400e4fe461 76 if(this->yaw_z >= 0){
siwakon 0:dd400e4fe461 77 if((this->yaw_z <= 45) && (this->yaw_z >= 0)){
siwakon 0:dd400e4fe461 78 return 0;
siwakon 0:dd400e4fe461 79 }
siwakon 0:dd400e4fe461 80 if((this->yaw_z <= 90) && (this->yaw_z >= 46)){
siwakon 0:dd400e4fe461 81 return 90;
siwakon 0:dd400e4fe461 82 }
siwakon 0:dd400e4fe461 83 if((this->yaw_z <= 135) && (this->yaw_z >= 91)){
siwakon 0:dd400e4fe461 84 return -90;
siwakon 0:dd400e4fe461 85 }
siwakon 0:dd400e4fe461 86 if((this->yaw_z <= 180) && (this->yaw_z >= 136)){
siwakon 0:dd400e4fe461 87 return 180;
siwakon 0:dd400e4fe461 88 }
siwakon 0:dd400e4fe461 89 }
siwakon 0:dd400e4fe461 90 else{
siwakon 0:dd400e4fe461 91 if((this->yaw_z <= 0) && (this->yaw_z >= -45)){
siwakon 0:dd400e4fe461 92 return 180;
siwakon 0:dd400e4fe461 93 }
siwakon 0:dd400e4fe461 94 if((this->yaw_z <= -46) && (this->yaw_z >= -90)){
siwakon 0:dd400e4fe461 95 return -90;
siwakon 0:dd400e4fe461 96 }
siwakon 0:dd400e4fe461 97 if((this->yaw_z <= -91) && (this->yaw_z >= -135)){
siwakon 0:dd400e4fe461 98 return 90;
siwakon 0:dd400e4fe461 99 }
siwakon 0:dd400e4fe461 100 if((this->yaw_z <= -136) && (this->yaw_z >= -180)){
siwakon 0:dd400e4fe461 101 return 0;
siwakon 0:dd400e4fe461 102 }
siwakon 0:dd400e4fe461 103 }
siwakon 0:dd400e4fe461 104 }
siwakon 0:dd400e4fe461 105
siwakon 0:dd400e4fe461 106 void bmuimu:: stard(){
siwakon 0:dd400e4fe461 107 this->stard_x = this->roll_x;
siwakon 0:dd400e4fe461 108 this->stard_y = this->pitch_y;
siwakon 0:dd400e4fe461 109 this->stard_z = this->yaw_z;
siwakon 0:dd400e4fe461 110 }
siwakon 0:dd400e4fe461 111
siwakon 0:dd400e4fe461 112
siwakon 0:dd400e4fe461 113
siwakon 0:dd400e4fe461 114
siwakon 0:dd400e4fe461 115
siwakon 0:dd400e4fe461 116
siwakon 0:dd400e4fe461 117 };//class