Diff: main.cpp

Revision:

4:679bf698903e

Parent:

3:0920442e3f03

Child:

5:aef6f39b9683

--- a/main.cpp	Tue Nov 03 12:07:20 2020 +0000
+++ b/main.cpp	Mon Nov 16 09:08:56 2020 +0000
@@ -53,7 +53,7 @@
     dirL = DIR_DEFAULT_L;
     dirR = DIR_DEFAULT_R;
     //  power setting
-    float maxPower = 0.9F;
+    float maxPower = 0.97F;
 
     //  serial attach
     serial.baud(115200);
@@ -169,19 +169,15 @@
 
 
 
-int head = -1;
+int head;
 char bytes[5];
 
 void control_rx()
 {
-    printf("rx\n\r");
-
+    
     while(serial.readable()) {
         char c = serial.getc();
-        printf("c: %c\n\r",c);
-
         if(c == 0x3A) {
-            printf("head\n\r");
             head = 0;
         }
         if(head < 5 && head != -1) {
@@ -195,15 +191,18 @@
         if(head == 5) {
             if(bytes[0] == 0x3A) {
                 //  cast float to double
-                uint16_t uintX = ((bytes[1] << 8 | bytes[2])>>4);
-                uint16_t uintY = ((bytes[3] << 8 | bytes[4])>>4);
-                float formatX = uintX / 65536;
-                float formatY = uintY / 65536;
+                uint16_t uintX = ((bytes[1] << 8 | bytes[2]) & 0xffff);
+                uint16_t uintY = ((bytes[3] << 8 | bytes[4]) & 0xffff);
+                double formatX = (double)uintX / 65536;
+                double formatY = (double)uintY / 65536;
                 //  save
                 x = formatX;
                 y = formatY;
-                printf("x:%d y:%d\n\r",uintX,uintY);
+                //printf("x:%hu y:%hu byte[%x %x %x %x]\n\r",uintX,uintY,bytes[1],bytes[2],bytes[3],bytes[4]);
                 break;
+            }else{
+                head = -1;
+                break;   
             }
         }
     }