Revision 8:27d6afa7cb7c, committed 2019-09-19

Comitter:

Haeun

Date:

Thu Sep 19 14:33:33 2019 +0000

Parent:

6:beddcb25ad4e

Commit message:

0919

Changed in this revision

diff -r beddcb25ad4e -r 27d6afa7cb7c main.cpp
--- a/main.cpp	Mon Sep 16 12:06:13 2019 +0000
+++ b/main.cpp	Thu Sep 19 14:33:33 2019 +0000
@@ -80,17 +80,23 @@
 #endif
 
 #ifdef JOYSTICK_MODE
-int X_position = 0;
-int Y_position = 0;
+int x_position = 0;
+int y_position = 0;
 
 void rxData(){
-    char temp[20];
-    temp = bt1.gets();
-    bt1.printf(0);            //convention
-//    if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
-//        rxChar = temp;
-//        rxFlag = 1;
-//        }
+    char temp[5];
+//    int x_temp=0,y_temp=0;
+    if('('==bt1.getc()){
+            for(int i=1;i<6&&temp[i-2]!=',';i++){
+                temp[i-1]=bt1.getc();
+            }
+            x_position=atoi(temp);
+            for(int i=1;i<6&&temp[i-2]!=')';i++){
+                temp[i-1]=bt1.getc();
+            }
+            y_position=atoi(temp);
+//            bt2.printf("X:%d,Y:%d\n",x_temp,y_temp);
+    }
 }
 #endif
 int main()
@@ -135,6 +141,8 @@
         #endif
                //return INTEGER
     float speed = MIN_SPEED;
+    bool forward=1;
+    float speed_joystick =0,direction=0;   
     while(1) {
  
     #ifdef ToF_MODE     
@@ -161,16 +169,48 @@
         //bt1.putc(0);
        
         #endif
-        bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
-        
-        #ifdef JOYSTICK_MODE        
         
         
+        #ifdef JOYSTICK_MODE     
+            bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
+            
+
+            speed_joystick = (float)y_position/250;
+            direction = (float)x_position/2000;
+            //bt2.printf("X:%d,Y:%d\n",x_position,y_position);
+            //bt2.printf("speed:%1.3f,dir:%1.3f\n",speed,direction);
+            if(speed_joystick>0&&forward==0){
+                forward=1;
+                throttleR=0;
+                throttleL=0;
+                wait(TIME_DELAY_FOR_REVERSE);
+                reverseR = 0;
+                reverseL = 0;
+                wait(1.0);
+                }
+            else if(speed_joystick<0&&forward==1){
+                throttleR=0;
+                throttleL=0;                
+                forward=0;
+                wait(TIME_DELAY_FOR_REVERSE);
+                reverseR = 1;
+                reverseL = 1;                
+                wait(1.0);
+                }
+            if(direction<0){//left
+                throttleR = abs(speed_joystick)+SPEED_CORRECTION+0.01f;
+                throttleL = abs(speed_joystick)-SPEED_CORRECTION+direction;
+            }
+            else{//right
+                throttleR = speed_joystick-direction;
+                throttleL = speed_joystick+SPEED_CORRECTION;               
+                }
+                
         
         #endif
         
         #ifdef BUTTON_MODE
-
+        bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
         if(rxFlag)
         {
             rxFlag=0;