yuto kawamura / Mbed 2 deprecated MR2_n_1_class

l

Dependencies:   mbed

microinfinity.cpp@6:75cfa1a66382, 2019-04-27 (annotated)

Committer:
yuto17320508
Date:
Sat Apr 27 11:22:00 2019 +0000
Revision:
6:75cfa1a66382
Parent:
3:df0c9883e403 
l

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 3:df0c9883e403 1 #include "mbed.h"
eri 3:df0c9883e403 2 #include "microinfinity.h"
eri 3:df0c9883e403 3
eri 3:df0c9883e403 4 Serial device(p13,p14);
eri 3:df0c9883e403 5 DigitalOut led1(LED1);
eri 3:df0c9883e403 6
eri 3:df0c9883e403 7 int32_t val[15]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
eri 3:df0c9883e403 8 int i=0;
eri 3:df0c9883e403 9 int sum=0;
eri 3:df0c9883e403 10 int end=0;
eri 3:df0c9883e403 11 double t=0;
eri 3:df0c9883e403 12 char check=0;
eri 3:df0c9883e403 13 int32_t degree=0;
eri 3:df0c9883e403 14 int32_t r_degree=0;
eri 3:df0c9883e403 15 int32_t s_degree=0;
eri 3:df0c9883e403 16 char counter[4]= {0,0,0,0};
eri 3:df0c9883e403 17 int round=0;
eri 3:df0c9883e403 18 double degree0=0;
eri 3:df0c9883e403 19 double theta0=0;
eri 3:df0c9883e403 20 //char counter=0;
eri 3:df0c9883e403 21
eri 3:df0c9883e403 22
eri 3:df0c9883e403 23
eri 3:df0c9883e403 24 void dev_rx()
eri 3:df0c9883e403 25 {
eri 3:df0c9883e403 26 //times.start();
eri 3:df0c9883e403 27 val[i]=device.getc();
eri 3:df0c9883e403 28 sum+=val[i];
eri 3:df0c9883e403 29 i++;
eri 3:df0c9883e403 30 if(i>14) {
eri 3:df0c9883e403 31 sum=sum%256;
eri 3:df0c9883e403 32 check=val[14]*2;
eri 3:df0c9883e403 33 if(sum==check) {
eri 3:df0c9883e403 34 if(val[4]>0x80)degree=((val[3]&0xFF)|((val[4]<<8)&0xFF00))-0xFFFF;
eri 3:df0c9883e403 35 else degree=((val[3]&0xFF)|((val[4]<<8)&0xFF00));
eri 3:df0c9883e403 36 if((r_degree>10000)&&(degree<-10000))round++; //1s-8000deg-Error
eri 3:df0c9883e403 37 if((r_degree<-10000)&&(degree>10000))round--;
eri 3:df0c9883e403 38 s_degree=36000*round+degree;
eri 3:df0c9883e403 39 counter[0]=(char)s_degree;
eri 3:df0c9883e403 40 counter[1]=(char)(s_degree>>8);
eri 3:df0c9883e403 41 counter[2]=(char)(s_degree>>16);
eri 3:df0c9883e403 42 counter[3]=(char)(s_degree>>24);
eri 3:df0c9883e403 43 //pc.printf("degree:%d %d %d\r\n",s_degree,degree,round);
eri 3:df0c9883e403 44
eri 3:df0c9883e403 45 r_degree=degree;
eri 3:df0c9883e403 46
eri 3:df0c9883e403 47 degree0=-degree/100.0-theta0;
eri 3:df0c9883e403 48 if(degree0 < -180) {
eri 3:df0c9883e403 49 degree0 += 360;
eri 3:df0c9883e403 50 } else if(degree0 > 180) {
eri 3:df0c9883e403 51 degree0 -= 360;
eri 3:df0c9883e403 52 }
eri 3:df0c9883e403 53 }
eri 3:df0c9883e403 54 i=0;
eri 3:df0c9883e403 55 }
eri 3:df0c9883e403 56 if((i==1)&&(val[0]!=170))i=0;
eri 3:df0c9883e403 57 if((i==1)&&(val[0]==170))sum=0;
eri 3:df0c9883e403 58 //times.stop();
eri 3:df0c9883e403 59 //printf("%d %f %d\r\n",i,times.read(),val[i]);
eri 3:df0c9883e403 60 //times.reset();
eri 3:df0c9883e403 61 }
eri 3:df0c9883e403 62
eri 3:df0c9883e403 63
eri 3:df0c9883e403 64 void check_gyro()
eri 3:df0c9883e403 65 {
eri 3:df0c9883e403 66 /* led1=1;
eri 3:df0c9883e403 67 pc.printf("CruizCore R1370 is self-checking now");
eri 3:df0c9883e403 68 wait(0.5);
eri 3:df0c9883e403 69 pc.printf(".");
eri 3:df0c9883e403 70 wait(0.5);
eri 3:df0c9883e403 71 pc.printf(".");
eri 3:df0c9883e403 72 wait(0.5);
eri 3:df0c9883e403 73 pc.printf(".");
eri 3:df0c9883e403 74 wait(0.5);
eri 3:df0c9883e403 75 while(1) {
eri 3:df0c9883e403 76 if(abs(degree0)<100) {
eri 3:df0c9883e403 77 pc.printf("\r\nSelf-checking is passed!!\r\n");
eri 3:df0c9883e403 78 break;
eri 3:df0c9883e403 79 } else {
eri 3:df0c9883e403 80 led1=!led1;
eri 3:df0c9883e403 81 pc.printf("\r\nSelf-checking was failed!!");
eri 3:df0c9883e403 82 pc.printf(":degree0=%d\r\n",degree0);
eri 3:df0c9883e403 83 wait(0.5);
eri 3:df0c9883e403 84 }
eri 3:df0c9883e403 85 }
eri 3:df0c9883e403 86 led1=0;*/
eri 3:df0c9883e403 87 }