Diff: microinfinity.cpp

Revision:

3:df0c9883e403

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/microinfinity.cpp	Sat Mar 30 01:47:42 2019 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "microinfinity.h"
+
+Serial device(p13,p14);
+DigitalOut led1(LED1);
+
+int32_t val[15]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+int i=0;
+int sum=0;
+int end=0;
+double t=0;
+char check=0;
+int32_t degree=0;
+int32_t r_degree=0;
+int32_t s_degree=0;
+char counter[4]= {0,0,0,0};
+int round=0;
+double degree0=0;
+double theta0=0;
+//char counter=0;
+
+
+
+void dev_rx()
+{
+    //times.start();
+    val[i]=device.getc();
+    sum+=val[i];
+    i++;
+    if(i>14) {
+        sum=sum%256;
+        check=val[14]*2;
+        if(sum==check) {
+            if(val[4]>0x80)degree=((val[3]&0xFF)|((val[4]<<8)&0xFF00))-0xFFFF;
+            else degree=((val[3]&0xFF)|((val[4]<<8)&0xFF00));
+            if((r_degree>10000)&&(degree<-10000))round++; //1s-8000deg-Error
+            if((r_degree<-10000)&&(degree>10000))round--;
+            s_degree=36000*round+degree;
+            counter[0]=(char)s_degree;
+            counter[1]=(char)(s_degree>>8);
+            counter[2]=(char)(s_degree>>16);
+            counter[3]=(char)(s_degree>>24);
+            //pc.printf("degree:%d %d %d\r\n",s_degree,degree,round);
+
+            r_degree=degree;
+
+            degree0=-degree/100.0-theta0;
+            if(degree0 < -180) {
+                degree0 += 360;
+            } else if(degree0 > 180) {
+                degree0 -= 360;
+            }
+        }
+        i=0;
+    }
+    if((i==1)&&(val[0]!=170))i=0;
+    if((i==1)&&(val[0]==170))sum=0;
+    //times.stop();
+    //printf("%d %f %d\r\n",i,times.read(),val[i]);
+    //times.reset();
+}
+
+
+void check_gyro()
+{
+ /*   led1=1;
+    pc.printf("CruizCore R1370 is self-checking now");
+    wait(0.5);
+    pc.printf(".");
+    wait(0.5);
+    pc.printf(".");
+    wait(0.5);
+    pc.printf(".");
+    wait(0.5);
+    while(1) {
+        if(abs(degree0)<100) {
+            pc.printf("\r\nSelf-checking is passed!!\r\n");
+            break;
+        } else {
+            led1=!led1;
+            pc.printf("\r\nSelf-checking was failed!!");
+            pc.printf(":degree0=%d\r\n",degree0);
+            wait(0.5);
+        }
+    }
+    led1=0;*/
+}