microinfinity.cpp

00001 #include "mbed.h"
00002 #include "microinfinity.h"
00003 
00004 Serial device(p13,p14);
00005 DigitalOut led1(LED1);
00006 DigitalOut led2(LED2);
00007 DigitalOut led3(LED3);
00008 
00009 int32_t val[15]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
00010 int i=0;
00011 int sum=0;
00012 int end=0;
00013 double t=0;
00014 char check=0;
00015 int32_t degree=0;
00016 int32_t r_degree=0;
00017 int32_t s_degree=0;
00018 char counter[4]= {0,0,0,0};
00019 int round=0;
00020 double degree0=0;
00021 double theta0=0;
00022 //char counter=0;
00023 
00024 
00025 
00026 void dev_rx()
00027 {
00028     //times.start();
00029     val[i]=device.getc();
00030     sum+=val[i];
00031     i++;
00032     if(i>14) {
00033         sum=sum%256;
00034         check=val[14]*2;
00035         if(sum==check) {
00036             if(val[4]>0x80)degree=((val[3]&0xFF)|((val[4]<<8)&0xFF00))-0xFFFF;
00037             else degree=((val[3]&0xFF)|((val[4]<<8)&0xFF00));
00038             if((r_degree>10000)&&(degree<-10000))round++; //1s-8000deg-Error
00039             if((r_degree<-10000)&&(degree>10000))round--;
00040             s_degree=36000*round+degree;
00041             counter[0]=(char)s_degree;
00042             counter[1]=(char)(s_degree>>8);
00043             counter[2]=(char)(s_degree>>16);
00044             counter[3]=(char)(s_degree>>24);
00045             //pc.printf("degree:%d %d %d\r\n",s_degree,degree,round);
00046 
00047             r_degree=degree;
00048 
00049             degree0=-degree/100.0-theta0;
00050             if(degree0 < -180) {
00051                 degree0 += 360;
00052             } else if(degree0 > 180) {
00053                 degree0 -= 360;
00054             }
00055         }
00056         i=0;
00057     }
00058     if((i==1)&&(val[0]!=170))i=0;
00059     if((i==1)&&(val[0]==170))sum=0;
00060     //times.stop();
00061     //printf("%d %f %d\r\n",i,times.read(),val[i]);
00062     //times.reset();
00063 }
00064 
00065 
00066 void check_gyro()
00067 {
00068     led1=1;
00069     printf("CruizCore R1370 is self-checking now");
00070     wait(0.5);
00071     printf(".");
00072     wait(0.5);
00073     printf(".");
00074     wait(0.5);
00075     printf(".");
00076     wait(0.5);
00077     while(1) {
00078         if(abs(degree0)<100) {
00079             printf("\r\nSelf-checking is passed!!\r\n");
00080             break;
00081         } else {
00082             led1=!led1;
00083             printf("\r\nSelf-checking was failed!!");
00084             printf(":degree0=%f\r\n",degree0);
00085             wait(0.5);
00086         }
00087     }
00088     led1=0;
00089 }