Revision 2:d1dec651ff13, committed 2016-07-19

Comitter:

jguzik

Date:

Tue Jul 19 13:10:16 2016 +0000

Parent:

1:1e01b12063cd

Commit message:

Changed in this revision

diff -r 1e01b12063cd -r d1dec651ff13 main.cpp
--- a/main.cpp	Thu Jul 14 13:31:01 2016 +0000
+++ b/main.cpp	Tue Jul 19 13:10:16 2016 +0000
@@ -16,9 +16,10 @@
 
 //*****deklaracja zmiennych*****
 
-const int BufferSize=15;
+const int BufferSize=18;
 char buffer1[BufferSize];
 char buffer2[BufferSize];
+char buffer3[BufferSize];
 char camove[2];
 char tablicaf[4];
 int wall=30;        // odleglosc od sciany w cm
@@ -42,14 +43,23 @@
 }
 
 int what_command(char* buffer1){             // rozpoznajaca komende
-    if ((buffer1[0]=='f') && (distance>30))
-        return 1;
-    else if ((buffer1[0]=='t') && (distance>30))
-        if (buffer1[2]=='l')
-            return 2;
-        else
-            return 3;
-    else if (buffer1[0]=='b')
+
+    /*forward 1  
+    t_leftt 2  
+    t_right 3  
+    backkkk 4  
+    STOPPPP 5 
+    leftttt 6  
+    righttt 7 
+    v_m_lef 8  
+    h_m_lef 9  
+    v_m_rig 10 
+    h_m_rig 11 
+    _m_zero 12 
+    unknown command*/
+
+    
+    if (buffer1[0]=='b')
         return 4;
     else if (buffer1[0]=='S')
         return 5;
@@ -57,18 +67,37 @@
         return 6;
     else if (buffer1[0]=='r')
         return 7;
-    else if (buffer1[0]=='v')
+    else if (buffer1[0]=='v'){
         if (buffer1[4]=='l')
             return 8;
         else
-            return 10;
-    else if (buffer1[0]=='h')
+            return 10;}
+    else if (buffer1[0]=='h'){
         if (buffer1[4]=='l')
             return 9;
         else
-            return 11;
+            return 11;}
     else if (buffer1[0]=='_')
         return 12;
+    else if (distance<30){
+         if (buffer1[0]=='f')         
+            return 5;
+         else if(buffer1[0]=='b')
+            return 4;
+         else if (buffer1[0]=='l')
+            return 6;
+         else if (buffer1[0]=='r')
+            return 7;
+         //return 5;
+         }
+         
+    else if ((buffer1[0]=='f'))
+        return 1;
+    else if ((buffer1[0]=='t')){ 
+        if (buffer1[2]=='l')
+            return 2;
+        else
+            return 3;}
     else
         return 0;
 }
@@ -145,7 +174,30 @@
         sscanf(buffer2, "%3d", &val);
 }
 
+void go_slower(){
+        if(distance<50)
+            val=0.5*val;
+        if(distance<30)
+            val=0.2*val;
+    }
+    
+void distance2array(int dist, char * dist_array){
+    if(dist<10){
+        dist_array[0]='0';
+        dist_array[1]='0';
+        dist_array[2]=dist+48;}
+    else if(dist>=10 && dist<100){
+          dist_array[0]='0';
+          dist_array[1]=dist/10+48;
+          dist_array[2]=dist%10+48;}
+    else {
+        dist_array[0]=dist/100+48;
+        dist_array[1]=dist%100/10+48;
+        dist_array[2]=dist%10+48;}      
+}
+    
 int main(){
+    char dist_array[3];
     stm.baud(115200);
     cleanBuffer1();
     cleanBuffer2(); 
@@ -156,12 +208,12 @@
     
     while(true){
         //stm.printf("Poczatek petli nr %d \n", i);
-        if(t.read_ms()>500)         //jesli przez ponad 500ms nie ma nowej ramki, zatrzymujemy robota
+        if(t.read_ms()>200)         //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
             { 
             eng_left.move(0);
             eng_right.move(0);
             }
-        if(t_sonar.read_ms()>1000)          //odczyt odleglosci co 1,2s
+        if(t_sonar.read_ms()>100)          //odczyt odleglosci co 0,4s
         {
             distance = sensor.distance();
             //stm.printf("distance z czujnika: %d\n",distance);
@@ -175,10 +227,10 @@
             /*stm.gets(buffer1, 13);
             stm.puts(buffer1);
             break;}*/
-
+        
             for (int x=0; x<12; x++){
                 if(t_fidu.read_ms()>1000){
-                    stm.printf("urywam;");
+                    //stm.printf("urywam;");
                     t_fidu.stop();
                     t_fidu.reset();
                     break;}
@@ -189,29 +241,54 @@
                 t_fidu.start();
                     break;}
             }
-            stm.puts(buffer1);
+            distance2array(distance, dist_array);
+            distance2array(distance, buffer3);
+            for(int i=0; i<3; i++){
+                buffer3[i]=dist_array[i];}
+            for(int i=0; i<12; i++){
+                buffer3[i+3]=buffer1[i];}
+
+           
+            stm.puts(buffer3);
             break;}
         
         set_val();
-        
         t.stop();
         t.reset();
         t.start();
         switch (what_command(buffer1))
         {
+      
+                /*forward 1  
+                t_leftt 2  
+                t_right 3  
+                backkkk 4  
+                STOPPPP 5 
+                leftttt 6  
+                righttt 7 
+                v_m_lef 8  
+                h_m_lef 9  
+                v_m_rig 10 
+                h_m_rig 11 
+                _m_zero 12 
+                unknown command*/    
+                
             case 1:
+                go_slower();
                 eng_left.move(val);
                 eng_right.move(val); 
                 set_camera_zero();
                 speed_car = val;
                 break;
             case 2:
+                go_slower();
                 eng_right.move(speed_car);
                 eng_left.move(speed_car - map(val, 0, 100, 0, speed_car));
                 cam_horizontal.position(7);
                 cam_vertical.position(-20);
                 break;
             case 3:
+                go_slower();
                 eng_left.move(speed_car);
                 eng_right.move(speed_car - map(val, 0, 100, 0, speed_car));
                 cam_horizontal.position(-23);