yuto kawamura / Mbed 2 deprecated MR2_2n_1_saku

sls

Dependencies:   mbed

microinfinity.cpp@0:c1476d342c13, 2019-04-26 (annotated)

Committer:
eri
Date:
Fri Apr 26 11:34:13 2019 +0000
Revision:
0:c1476d342c13
a

Who changed what in which revision?

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