OX / Mbed 2 deprecated IMU_oo

new

Dependencies:   mbed

main.cpp@1:6bc2c5d68446, 2016-12-04 (annotated)

Committer:
siwakon
Date:
Sun Dec 04 09:19:53 2016 +0000
Revision:
1:6bc2c5d68446
Parent:
0:77a7d1a1c6db 
weewfd

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