OX / Mbed 2 deprecated testIMUmain

dsafa

Dependencies:   mbed

Fork of IMU_oo by OX

main.cpp@0:77a7d1a1c6db, 2016-12-03 (annotated)

Committer:
siwakon
Date:
Sat Dec 03 18:21:47 2016 +0000
Revision:
0:77a7d1a1c6db
Child:
1:29b8881ac539
ooo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
siwakon 0:77a7d1a1c6db 1 #include "mbed.h"
siwakon 0:77a7d1a1c6db 2 #include "zmu9250.h"
siwakon 0:77a7d1a1c6db 3 #include "math.h"
siwakon 0:77a7d1a1c6db 4
siwakon 0:77a7d1a1c6db 5 ZMU9250 a;
siwakon 0:77a7d1a1c6db 6 Serial pc(D1, D0);
siwakon 0:77a7d1a1c6db 7
siwakon 0:77a7d1a1c6db 8 float yaw_z, pitch_y, roll_x;
siwakon 0:77a7d1a1c6db 9 int ir,ip,iy;
siwakon 0:77a7d1a1c6db 10 int l;
siwakon 0:77a7d1a1c6db 11 int xlr,yfb,ztd;
siwakon 0:77a7d1a1c6db 12 int sata;
siwakon 0:77a7d1a1c6db 13 char rt;
siwakon 0:77a7d1a1c6db 14
siwakon 0:77a7d1a1c6db 15 int cube1(int x, int y, int z);
siwakon 0:77a7d1a1c6db 16 int seccon(float roll ,float pitch, float yaw);
siwakon 0:77a7d1a1c6db 17 int round(int rt_x, int rt_y, int rt_z);
siwakon 0:77a7d1a1c6db 18
siwakon 0:77a7d1a1c6db 19
siwakon 0:77a7d1a1c6db 20 DigitalIn op_roll(USER_BUTTON);
siwakon 0:77a7d1a1c6db 21
siwakon 0:77a7d1a1c6db 22
siwakon 0:77a7d1a1c6db 23 int main(){
siwakon 0:77a7d1a1c6db 24
siwakon 0:77a7d1a1c6db 25 while(1){
siwakon 0:77a7d1a1c6db 26 wait(0.1);
siwakon 0:77a7d1a1c6db 27 a.Update();
siwakon 0:77a7d1a1c6db 28 yaw_z = a.Yaw();
siwakon 0:77a7d1a1c6db 29 pitch_y = a.Pitch();
siwakon 0:77a7d1a1c6db 30 roll_x = a.Roll();
siwakon 0:77a7d1a1c6db 31
siwakon 0:77a7d1a1c6db 32 // pc.printf("Yaw_z %f\t Pitch_y %f\t Roll_x %f\n", yaw_z, pitch_y, roll_x);
siwakon 0:77a7d1a1c6db 33
siwakon 0:77a7d1a1c6db 34 //l = cube1(3,3,3);
siwakon 0:77a7d1a1c6db 35
siwakon 0:77a7d1a1c6db 36 //pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\n",l , yaw_z, pitch_y, roll_x); //add
siwakon 0:77a7d1a1c6db 37
siwakon 0:77a7d1a1c6db 38 pc.printf("%d\t Yaw_z %f\t Pitch_y %f\t Roll_x %f\t %d\t %c\t %d\t \r\n ",round(5,5,5), yaw_z, pitch_y, roll_x, sata, rt, l); //round
siwakon 0:77a7d1a1c6db 39
siwakon 0:77a7d1a1c6db 40 // pc.printf( "%d\n" ,seccon(roll_x, pitch_y, yaw_z));
siwakon 0:77a7d1a1c6db 41 // pc.printf("========\n"); //it
siwakon 0:77a7d1a1c6db 42 if(op_roll == 0){
siwakon 0:77a7d1a1c6db 43
siwakon 0:77a7d1a1c6db 44 l = cube1(3,3,3);
siwakon 0:77a7d1a1c6db 45 }
siwakon 0:77a7d1a1c6db 46
siwakon 0:77a7d1a1c6db 47
siwakon 0:77a7d1a1c6db 48 }
siwakon 0:77a7d1a1c6db 49 }
siwakon 0:77a7d1a1c6db 50
siwakon 0:77a7d1a1c6db 51 int cube1(int x, int y, int z){
siwakon 0:77a7d1a1c6db 52
siwakon 0:77a7d1a1c6db 53 if( ((pitch_y > -30) && (pitch_y < 30)) ){ // vCC L leg D
siwakon 0:77a7d1a1c6db 54
siwakon 0:77a7d1a1c6db 55 if( ((yaw_z > 36) && (yaw_z < 113)) && ((roll_x > -40) && (roll_x < 40)) ){
siwakon 0:77a7d1a1c6db 56 x++; //R x+
siwakon 0:77a7d1a1c6db 57 xlr = 1;
siwakon 0:77a7d1a1c6db 58
siwakon 0:77a7d1a1c6db 59
siwakon 0:77a7d1a1c6db 60 }
siwakon 0:77a7d1a1c6db 61 if( ((yaw_z > -101) && (yaw_z < 35)) && ((roll_x > -40) && (roll_x < 40)) ){
siwakon 0:77a7d1a1c6db 62 y--; //B y+
siwakon 0:77a7d1a1c6db 63 yfb = -1;
siwakon 0:77a7d1a1c6db 64 }
siwakon 0:77a7d1a1c6db 65 if( ((yaw_z > 110) && (yaw_z < 179)) && ((roll_x > -40) && (roll_x < 40)) ){
siwakon 0:77a7d1a1c6db 66 y++; //F y-
siwakon 0:77a7d1a1c6db 67 yfb = 1;
siwakon 0:77a7d1a1c6db 68
siwakon 0:77a7d1a1c6db 69 }
siwakon 0:77a7d1a1c6db 70 if( ((yaw_z > -180) && (yaw_z < -100)) && ((roll_x > -40) && (roll_x < 40)) ){
siwakon 0:77a7d1a1c6db 71 x--; //L x-
siwakon 0:77a7d1a1c6db 72 xlr = -1;
siwakon 0:77a7d1a1c6db 73
siwakon 0:77a7d1a1c6db 74 }
siwakon 0:77a7d1a1c6db 75 if((roll_x > 41) && (roll_x < 130) ){
siwakon 0:77a7d1a1c6db 76 z++; //T z+
siwakon 0:77a7d1a1c6db 77 ztd = 1;
siwakon 0:77a7d1a1c6db 78
siwakon 0:77a7d1a1c6db 79 }
siwakon 0:77a7d1a1c6db 80 if( (roll_x > -130) && (roll_x < -41)){
siwakon 0:77a7d1a1c6db 81 z--; //D z-
siwakon 0:77a7d1a1c6db 82 ztd = -1;
siwakon 0:77a7d1a1c6db 83
siwakon 0:77a7d1a1c6db 84 }
siwakon 0:77a7d1a1c6db 85
siwakon 0:77a7d1a1c6db 86 }
siwakon 0:77a7d1a1c6db 87
siwakon 0:77a7d1a1c6db 88
siwakon 0:77a7d1a1c6db 89
siwakon 0:77a7d1a1c6db 90 return (x*100+y*10+z);
siwakon 0:77a7d1a1c6db 91 }
siwakon 0:77a7d1a1c6db 92
siwakon 0:77a7d1a1c6db 93
siwakon 0:77a7d1a1c6db 94
siwakon 0:77a7d1a1c6db 95
siwakon 0:77a7d1a1c6db 96 int seccon(float roll ,float pitch, float yaw){
siwakon 0:77a7d1a1c6db 97 if( ( ((yaw > -50) && (yaw < -30)) || ((yaw > -145) && (yaw < -125)) || ((yaw > 140) && (yaw < 160))||((yaw > 0) && (yaw < -20)) )
siwakon 0:77a7d1a1c6db 98 && ((roll_x > -10 )&& (roll_x < 10))
siwakon 0:77a7d1a1c6db 99 && (( pitch > -10 )&&(pitch < 10)) //L T R D
siwakon 0:77a7d1a1c6db 100 ){
siwakon 0:77a7d1a1c6db 101 ir = roll;
siwakon 0:77a7d1a1c6db 102 ip = pitch;
siwakon 0:77a7d1a1c6db 103 iy = yaw;
siwakon 0:77a7d1a1c6db 104 return 1;
siwakon 0:77a7d1a1c6db 105 }
siwakon 0:77a7d1a1c6db 106 else{
siwakon 0:77a7d1a1c6db 107 return 0;
siwakon 0:77a7d1a1c6db 108 }
siwakon 0:77a7d1a1c6db 109 }
siwakon 0:77a7d1a1c6db 110
siwakon 0:77a7d1a1c6db 111
siwakon 0:77a7d1a1c6db 112 int round(int rt_x, int rt_y, int rt_z){
siwakon 0:77a7d1a1c6db 113
siwakon 0:77a7d1a1c6db 114 if( (pitch_y > -30) && (pitch_y < 30) ){ // vCC L leg D
siwakon 0:77a7d1a1c6db 115
siwakon 0:77a7d1a1c6db 116 if( ((yaw_z > 36) && (yaw_z < 113)) && ( ((roll_x > -40) && (roll_x < 40)) ) ){
siwakon 0:77a7d1a1c6db 117 //R x+
siwakon 0:77a7d1a1c6db 118 if(xlr == 1){
siwakon 0:77a7d1a1c6db 119 sata = 0;
siwakon 0:77a7d1a1c6db 120 rt = 'z';
siwakon 0:77a7d1a1c6db 121 }
siwakon 0:77a7d1a1c6db 122 else if(yfb == -1){
siwakon 0:77a7d1a1c6db 123 sata = 90;
siwakon 0:77a7d1a1c6db 124 rt = 'z';
siwakon 0:77a7d1a1c6db 125 }
siwakon 0:77a7d1a1c6db 126 else if(yfb == 1){
siwakon 0:77a7d1a1c6db 127 sata = 270;
siwakon 0:77a7d1a1c6db 128 rt = 'z';
siwakon 0:77a7d1a1c6db 129 }
siwakon 0:77a7d1a1c6db 130 else if(xlr == -1){
siwakon 0:77a7d1a1c6db 131 sata = 180;
siwakon 0:77a7d1a1c6db 132 rt = 'z';
siwakon 0:77a7d1a1c6db 133 }
siwakon 0:77a7d1a1c6db 134 else if(ztd == 1){
siwakon 0:77a7d1a1c6db 135 sata = 270; //x
siwakon 0:77a7d1a1c6db 136 rt = 'x';
siwakon 0:77a7d1a1c6db 137 }
siwakon 0:77a7d1a1c6db 138 else if(ztd == -1){
siwakon 0:77a7d1a1c6db 139 sata = 90; //x
siwakon 0:77a7d1a1c6db 140 rt ='x';
siwakon 0:77a7d1a1c6db 141 }
siwakon 0:77a7d1a1c6db 142 }
siwakon 0:77a7d1a1c6db 143 if( ((yaw_z > -74) && (yaw_z < 35)) && ( ((roll_x > -40) && (roll_x < 40)) ) ){
siwakon 0:77a7d1a1c6db 144 //B y+
siwakon 0:77a7d1a1c6db 145 if(xlr == 1){
siwakon 0:77a7d1a1c6db 146 sata = 270;
siwakon 0:77a7d1a1c6db 147 rt = 'z';
siwakon 0:77a7d1a1c6db 148 }
siwakon 0:77a7d1a1c6db 149 else if(yfb == -1){
siwakon 0:77a7d1a1c6db 150 sata = 0;
siwakon 0:77a7d1a1c6db 151 rt = 'z';
siwakon 0:77a7d1a1c6db 152 }
siwakon 0:77a7d1a1c6db 153 else if(yfb == 1){
siwakon 0:77a7d1a1c6db 154 sata = 180;
siwakon 0:77a7d1a1c6db 155 rt = 'z';
siwakon 0:77a7d1a1c6db 156 }
siwakon 0:77a7d1a1c6db 157 else if(xlr == -1){
siwakon 0:77a7d1a1c6db 158 sata = 90;
siwakon 0:77a7d1a1c6db 159 rt = 'z';
siwakon 0:77a7d1a1c6db 160 }
siwakon 0:77a7d1a1c6db 161 else if(ztd == 1){
siwakon 0:77a7d1a1c6db 162 sata = 270; //x
siwakon 0:77a7d1a1c6db 163 rt = 'x';
siwakon 0:77a7d1a1c6db 164 }
siwakon 0:77a7d1a1c6db 165 else if(ztd == -1){
siwakon 0:77a7d1a1c6db 166 sata = 90; //x
siwakon 0:77a7d1a1c6db 167 rt = 'x';
siwakon 0:77a7d1a1c6db 168 }
siwakon 0:77a7d1a1c6db 169 }
siwakon 0:77a7d1a1c6db 170 if( ((yaw_z > 110) && (yaw_z < 179)) &&( ((roll_x > -40) && (roll_x < 40)) ) ){
siwakon 0:77a7d1a1c6db 171 //F y-
siwakon 0:77a7d1a1c6db 172 if(xlr == 1){
siwakon 0:77a7d1a1c6db 173 sata = 90;
siwakon 0:77a7d1a1c6db 174 rt = 'z';
siwakon 0:77a7d1a1c6db 175 }
siwakon 0:77a7d1a1c6db 176 else if(yfb == -1){
siwakon 0:77a7d1a1c6db 177 sata = 180;
siwakon 0:77a7d1a1c6db 178 rt = 'z';
siwakon 0:77a7d1a1c6db 179 }
siwakon 0:77a7d1a1c6db 180 else if(yfb == 1){
siwakon 0:77a7d1a1c6db 181 sata = 0;
siwakon 0:77a7d1a1c6db 182 rt = 'z';
siwakon 0:77a7d1a1c6db 183 }
siwakon 0:77a7d1a1c6db 184 else if(xlr == -1){
siwakon 0:77a7d1a1c6db 185 sata = 270;
siwakon 0:77a7d1a1c6db 186 rt = 'z';
siwakon 0:77a7d1a1c6db 187 }
siwakon 0:77a7d1a1c6db 188 else if(ztd == 1){
siwakon 0:77a7d1a1c6db 189 sata = 90;
siwakon 0:77a7d1a1c6db 190 rt = 'x';
siwakon 0:77a7d1a1c6db 191 }
siwakon 0:77a7d1a1c6db 192 else if(ztd == -1){
siwakon 0:77a7d1a1c6db 193 sata = 270;
siwakon 0:77a7d1a1c6db 194 rt = 'x';
siwakon 0:77a7d1a1c6db 195 }
siwakon 0:77a7d1a1c6db 196 }
siwakon 0:77a7d1a1c6db 197 if( ((yaw_z > -180) && (yaw_z < -75)) &&( ((roll_x > -40) && (roll_x < 40)) ) ){
siwakon 0:77a7d1a1c6db 198 //L x-
siwakon 0:77a7d1a1c6db 199 if(xlr == 1){
siwakon 0:77a7d1a1c6db 200 sata = 180;
siwakon 0:77a7d1a1c6db 201 rt = 'z';
siwakon 0:77a7d1a1c6db 202 }
siwakon 0:77a7d1a1c6db 203 else if(yfb == -1){
siwakon 0:77a7d1a1c6db 204 sata = 270;
siwakon 0:77a7d1a1c6db 205 rt = 'z';
siwakon 0:77a7d1a1c6db 206 }
siwakon 0:77a7d1a1c6db 207 else if(yfb == 1){
siwakon 0:77a7d1a1c6db 208 sata = 90;
siwakon 0:77a7d1a1c6db 209 rt = 'z';
siwakon 0:77a7d1a1c6db 210 }
siwakon 0:77a7d1a1c6db 211 else if(xlr == -1){
siwakon 0:77a7d1a1c6db 212 sata = 0;
siwakon 0:77a7d1a1c6db 213 rt = 'z';
siwakon 0:77a7d1a1c6db 214 }
siwakon 0:77a7d1a1c6db 215 else if(ztd == 1){
siwakon 0:77a7d1a1c6db 216 sata = 90;
siwakon 0:77a7d1a1c6db 217 rt = 'x';
siwakon 0:77a7d1a1c6db 218 }
siwakon 0:77a7d1a1c6db 219 else if(ztd == -1){
siwakon 0:77a7d1a1c6db 220 sata = 270;
siwakon 0:77a7d1a1c6db 221 rt = 'x';
siwakon 0:77a7d1a1c6db 222 }
siwakon 0:77a7d1a1c6db 223 }
siwakon 0:77a7d1a1c6db 224 if((roll_x > 41) && (roll_x < 130) ){
siwakon 0:77a7d1a1c6db 225 //T z+
siwakon 0:77a7d1a1c6db 226 if(xlr == 1){
siwakon 0:77a7d1a1c6db 227 sata = 90; //x
siwakon 0:77a7d1a1c6db 228 rt = 'x';
siwakon 0:77a7d1a1c6db 229 }
siwakon 0:77a7d1a1c6db 230 else if(yfb == -1){
siwakon 0:77a7d1a1c6db 231 sata = 90; //y
siwakon 0:77a7d1a1c6db 232 rt = 'y';
siwakon 0:77a7d1a1c6db 233 }
siwakon 0:77a7d1a1c6db 234 else if(yfb == 1){
siwakon 0:77a7d1a1c6db 235 sata = 270;
siwakon 0:77a7d1a1c6db 236 rt = 'y';
siwakon 0:77a7d1a1c6db 237 }
siwakon 0:77a7d1a1c6db 238 else if(xlr == -1){
siwakon 0:77a7d1a1c6db 239 sata = 270;
siwakon 0:77a7d1a1c6db 240 rt = 'x';
siwakon 0:77a7d1a1c6db 241 }
siwakon 0:77a7d1a1c6db 242 else if(ztd == 1){
siwakon 0:77a7d1a1c6db 243 sata = 0;
siwakon 0:77a7d1a1c6db 244 rt = 'x';
siwakon 0:77a7d1a1c6db 245 }
siwakon 0:77a7d1a1c6db 246 else if(ztd == -1){
siwakon 0:77a7d1a1c6db 247 sata = 180; //x
siwakon 0:77a7d1a1c6db 248 rt = 'x';
siwakon 0:77a7d1a1c6db 249 }
siwakon 0:77a7d1a1c6db 250 }
siwakon 0:77a7d1a1c6db 251 if( (roll_x > -130) && (roll_x < -41)){
siwakon 0:77a7d1a1c6db 252 //D z-
siwakon 0:77a7d1a1c6db 253 if(xlr == 1){
siwakon 0:77a7d1a1c6db 254 sata = 270;
siwakon 0:77a7d1a1c6db 255 rt = 'x';
siwakon 0:77a7d1a1c6db 256 }
siwakon 0:77a7d1a1c6db 257 else if(yfb == -1){
siwakon 0:77a7d1a1c6db 258 sata = 270;
siwakon 0:77a7d1a1c6db 259 rt = 'y';
siwakon 0:77a7d1a1c6db 260 }
siwakon 0:77a7d1a1c6db 261 else if(yfb == 1){
siwakon 0:77a7d1a1c6db 262 sata = 90;
siwakon 0:77a7d1a1c6db 263 rt = 'y';
siwakon 0:77a7d1a1c6db 264 }
siwakon 0:77a7d1a1c6db 265 else if(xlr == -1){
siwakon 0:77a7d1a1c6db 266 sata = 90;
siwakon 0:77a7d1a1c6db 267 rt = 'x';
siwakon 0:77a7d1a1c6db 268 }
siwakon 0:77a7d1a1c6db 269 else if(ztd == 1){
siwakon 0:77a7d1a1c6db 270 sata = 180;
siwakon 0:77a7d1a1c6db 271 rt = 'x';
siwakon 0:77a7d1a1c6db 272 }
siwakon 0:77a7d1a1c6db 273 else if(ztd == -1){
siwakon 0:77a7d1a1c6db 274 sata = 0 ; //x
siwakon 0:77a7d1a1c6db 275 rt = 'x';
siwakon 0:77a7d1a1c6db 276 }
siwakon 0:77a7d1a1c6db 277
siwakon 0:77a7d1a1c6db 278 }
siwakon 0:77a7d1a1c6db 279 }
siwakon 0:77a7d1a1c6db 280
siwakon 0:77a7d1a1c6db 281 rt_x = rt_x - 3;
siwakon 0:77a7d1a1c6db 282 rt_y = rt_y - 3;
siwakon 0:77a7d1a1c6db 283 rt_z = rt_z - 3;
siwakon 0:77a7d1a1c6db 284 if(rt == 'x'){
siwakon 0:77a7d1a1c6db 285 rt_x = rt_x + 3; //x
siwakon 0:77a7d1a1c6db 286 rt_y = (rt_y * cos(sata*(PI/180))+ rt_z * sin(sata*(PI/180)) )+ 3;
siwakon 0:77a7d1a1c6db 287 rt_z = (-rt_y * sin(sata*(PI/180))+ rt_z * cos(sata*(PI/180)) ) + 3;
siwakon 0:77a7d1a1c6db 288 }
siwakon 0:77a7d1a1c6db 289 if(rt == 'y'){
siwakon 0:77a7d1a1c6db 290 rt_x = (rt_x * cos(sata*(PI/180)) - rt_z * sin(sata*(PI/180)) )+ 3; // y
siwakon 0:77a7d1a1c6db 291 rt_y = rt_y + 3;
siwakon 0:77a7d1a1c6db 292 rt_z = (rt_x * sin(sata*(PI/180)) + rt_z * cos(sata*(PI/180)) )+ 3;
siwakon 0:77a7d1a1c6db 293 }
siwakon 0:77a7d1a1c6db 294 if(rt == 'z'){
siwakon 0:77a7d1a1c6db 295 rt_x = (rt_x * cos(sata*(PI/180))+ rt_y * sin(sata*(PI/180)) )+ 3; //z
siwakon 0:77a7d1a1c6db 296 rt_y = (rt_y * cos(sata*(PI/180))- rt_x * sin(sata*(PI/180)) )+ 3;
siwakon 0:77a7d1a1c6db 297 rt_z = rt_z + 3;
siwakon 0:77a7d1a1c6db 298 }
siwakon 0:77a7d1a1c6db 299 pc.printf("rt_x %d\t", rt_x);
siwakon 0:77a7d1a1c6db 300 pc.printf("rt_y %d\t", rt_y);
siwakon 0:77a7d1a1c6db 301 pc.printf("rt_z %d \n\n\n\n\r", rt_z);
siwakon 0:77a7d1a1c6db 302
siwakon 0:77a7d1a1c6db 303 return (rt_x*100 + rt_y*10 + rt_z);
siwakon 0:77a7d1a1c6db 304 }
siwakon 0:77a7d1a1c6db 305