Diff: main.cpp

Revision:

6:17e3a28338dc

Parent:

5:aef6f39b9683

Child:

7:0bc2bc07f2fe

--- a/main.cpp	Tue Nov 17 08:29:41 2020 +0000
+++ b/main.cpp	Sat Nov 28 07:48:16 2020 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 #include <vector>
+//  Buffer Serial
+#include "BufferSerial.h"
 
 //  Left Motor
 #define PWM_L A6
@@ -29,7 +31,10 @@
 DigitalOut dirR(DIR_R);
 
 //  serial
-Serial serial(D5,D4);
+//Serial serial(D5,D4);
+//  bufferSerial
+BufferSerial _buffer_serial(D5,D4,115200,2000);
+
 //  time out led
 DigitalOut connectTimeoutLED(CONNECT_TIMEOUT_LED);
 
@@ -51,6 +56,8 @@
 Timer connectTimer;
 int connectLimit = 2000;
 
+void load_data();
+
 int main()
 {
     //  period
@@ -60,21 +67,26 @@
     dirL = DIR_DEFAULT_L;
     dirR = DIR_DEFAULT_R;
     //  power setting
-    float maxPower = 0.97F;
+    float maxPower = 0.95F;
 
+    /*
     //  serial attach
     serial.baud(115200);
     serial.attach(control_rx,Serial::RxIrq);
+    */
     //  connect timer
     connectTimer.reset();
     connectTimer.start();
-    
+
     while(1) {
         //  prepare power value
         float powerL,powerR;
 
+        //  loat data
+        load_data();
+
         //  last connect checker
-        if(connectTimer.read_ms() > connectLimit){
+        if(connectTimer.read_ms() > connectLimit) {
             x = 0.5;
             y = 0.5;
             //  LED
@@ -86,9 +98,6 @@
         if(x < 0) x = 0.0;
         if(y > 1) y = 1.0;
         if(y < 0) y = 0.0;
-        //  off set
-        //if(0.48F < x && x < 0.52F)x = 0.5F;
-        //if(0.48F < y && y < 0.52F)y = 0.5F;
 
         //  format x and y
         double formatX = (2*x) - 1;
@@ -182,13 +191,13 @@
 }
 
 
-
+/*
 int head;
 char bytes[5];
 
 void control_rx()
 {
-    
+
     while(serial.readable()) {
         char c = serial.getc();
         if(c == 0x3A) {
@@ -212,14 +221,50 @@
                 //  save
                 x = formatX;
                 y = formatY;
-                
+
                 //  reset connect timer
                 connectTimer.reset();
                 break;
             }else{
                 head = -1;
-                break;   
+                break;
             }
         }
     }
+}*/
+
+void load_data()
+{
+    /*
+    //  buffer check
+    int buffer_length = sizeof(_buffer_serial._rx_buffer) / sizeof(_buffer_serial._rx_buffer[0]);
+    for(int i = 0; i <= buffer_length; i++) {
+        printf("%x ",_buffer_serial._rx_buffer[i]);
+    }
+    printf("\n\r");
+    */
+    char bytes[6];
+    size_t index = _buffer_serial.readBytesUntil('\n',(char*)bytes,6);
+    if(index != 5){
+        //printf("e:index %d\n\r",index);
+        return;
+    }
+    if(bytes[0] == 0x3A) {
+        //  cast float to double
+        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("%lf %lf\n\r",formatX,formatY);
+        printf("%x %x %x %x %x %x %lf %lf\n\r",bytes[0],bytes[1],bytes[2],bytes[3],bytes[4],bytes[5],formatX,formatY);
+        //  reset connect timer
+        connectTimer.reset();
+
+    } else {
+        //printf("failed load_data()\n\r");
+    }
 }