Diff: main.cpp

Revision:

3:0920442e3f03

Parent:

2:becb78cfc927

Child:

4:679bf698903e

--- a/main.cpp	Mon Nov 02 11:42:17 2020 +0000
+++ b/main.cpp	Tue Nov 03 12:07:20 2020 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include <vector>
 
 //  Left Motor
 #define PWM_L A6
@@ -53,16 +54,17 @@
     dirR = DIR_DEFAULT_R;
     //  power setting
     float maxPower = 0.9F;
-    
+
     //  serial attach
+    serial.baud(115200);
     serial.attach(control_rx,Serial::RxIrq);
-    
+
     //printf("finish start up! \n\r");
 
-    while(1){
+    while(1) {
         //  prepare power value
         float powerL,powerR;
-        
+
         //float x = joyX.read();  //  go ahead & back
         //float y = joyY.read();  //  left & right
         //  value format
@@ -73,7 +75,7 @@
         //  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;
         double formatY = (2*y) - 1;
@@ -88,47 +90,47 @@
         if(range > 1.0F) range = 1.0F;
         //  all power
         double allPower = maxPower * (range / 1);
-        
+
         //  right left power persent
         double rightPowerPercent,leftPowerPercent;
         int area = 0;
         //1
-        if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)){
+        if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)) {
             rightPowerPercent = ((angle / 45) - 1) / 1;
             leftPowerPercent = 1;
-            
+
             area = 1;
         }
         //2
-        if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)){
+        if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)) {
             rightPowerPercent = 1;
             leftPowerPercent = ((-angle / 45) - 1) / 1;
-            
+
             area = 2;
         }
         //3
-        if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)){
+        if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)) {
             rightPowerPercent = ((-angle / 45) + 1) / 1;
             leftPowerPercent = -1;
-        
+
             area = 3;
         }
         //4
-        if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)){
+        if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)) {
             rightPowerPercent = -1;
             leftPowerPercent = ((angle / 45) + 1) / 1;
-        
+
             area = 4;
         }
-        if(area == 0){
+        if(area == 0) {
             rightPowerPercent = 0;
             leftPowerPercent = 0;
         }
-        
+
         //  convert power persent to real power
         powerL = allPower * leftPowerPercent;
         powerR = allPower * rightPowerPercent;
-        
+
         //  need to change power(power distance)
         double needChangePowerL = powerL - currentPowerL;
         double needChangePowerR = powerR - currentPowerR;
@@ -141,58 +143,68 @@
         //  save current power
         currentPowerL = powerL;
         currentPowerR = powerR;
-        
+
         //printf("before:%lf %lf distance:%lf %lf change:%lf %lf after:%lf %lf \n\r",powerL,powerR,needChangePowerL,needChangePowerR,changePowerL,changePowerR,powerL,powerR);
-        
-        if(powerL >= 0){
-            dirL = DIR_DEFAULT_L;    
-        }else{
+
+        if(powerL >= 0) {
+            dirL = DIR_DEFAULT_L;
+        } else {
             powerL = -powerL;
             dirL = !DIR_DEFAULT_L;
         }
-        if(powerR >= 0){
-            dirR = DIR_DEFAULT_R;    
-        }else{
+        if(powerR >= 0) {
+            dirR = DIR_DEFAULT_R;
+        } else {
             powerR = -powerR;
             dirR = !DIR_DEFAULT_R;
         }
-        
+
         pwmL.write((float)powerL);
         pwmR.write((float)powerR);
-        
+
         wait_ms(50);
         //printf("powerL:%f powerR:%f formatX:%0lf formatY:%0lf angle:%d range:%0lf area: %d Lper:%0lf Rper:%0lf \n\r",powerL,powerR,formatX,formatY,(int) angle,range,area,leftPowerPercent,rightPowerPercent);
     }
 }
 
-void control_rx(){
-    int head = 0;
-    char bytesX[4];
-    char bytesY[4];
-    
-    while(serial.readable()){
+
+
+int head = -1;
+char bytes[5];
+
+void control_rx()
+{
+    printf("rx\n\r");
+
+    while(serial.readable()) {
         char c = serial.getc();
-        
-        if(c == 0x3a){
+        printf("c: %c\n\r",c);
+
+        if(c == 0x3A) {
+            printf("head\n\r");
             head = 0;
-            continue;
         }
-        if(head < 3){
-            bytesX[head] = c;
-        }else{
-            bytesY[head - 3] = c;
+        if(head < 5 && head != -1) {
+            bytes[head] = c;
+            head++;
+        } else {
+            printf("error: failed get data[head:%d]\n\r",head);
+            head = -1;
+            break;
         }
-        if(head == 6){
-            //  printf
-            int32_t intX = (bytesX[0] << 24) + (bytesX[1] << 16) + (bytesX[2] << 8) + bytesX[3];
-            int32_t intY = (bytesY[0] << 24) + (bytesY[1] << 16) + (bytesY[2] << 8) + bytesY[3];            
-            //  cast float to double
-            float formatX = (float) intX;
-            float formatY = (float) intY;
-            //  save
-            x = formatX;
-            y = formatY;
+        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;
+                //  save
+                x = formatX;
+                y = formatY;
+                printf("x:%d y:%d\n\r",uintX,uintY);
+                break;
+            }
         }
-        head++;
     }
 }