Diff: main.cpp

Revision:

4:84f836aaf390

Parent:

3:1a391dd91a35

--- a/main.cpp	Fri Jun 24 13:43:08 2016 +0000
+++ b/main.cpp	Tue Jun 28 12:52:35 2016 +0000
@@ -5,11 +5,12 @@
  
 //*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
 Serial stm(PA_2, PA_3);
-Servo cam_poziom(PB_6);
-Servo cam_pion(PC_7);
+Servo cam_poziom = Servo(PB_6);
+Servo cam_pion = Servo(PC_7);
 Engine eng_left = Engine(PB_13, PB_4, PB_10);
 Engine eng_right = Engine(PB_14, PB_5, PB_3);
 
+
 //*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
 HCSR04 sensor(PB_9, PB_8, 11770);
 const int BufferSize=10;
@@ -23,9 +24,10 @@
 int p=0;
 int po=0;
 int pi=0;
-Timer t, t_sonar;
+Timer t, t_sonar, czas;
 long distance;
- 
+int counter=0, wypisz_czas;
+
 //*************** Funkcja czyszczaca bufor *****************
 void cleanBuffer(char *buffor)
  {
@@ -157,14 +159,25 @@
             if(akt_pion>90)
                 akt_pion=90;
             else if (akt_pion<-90)
-                akt_poziom=-90;                
+                akt_poziom=-90;                      
 }
 
 
 void move_camera()
 {               
-            cam_poziom.position(akt_poziom);
-            cam_pion.position(akt_pion);    
+            cam_pion.SetPosition(akt_pion*5+1500);  
+            cam_poziom.SetPosition(akt_poziom*5+1500);
+             /*while(1) {
+                  for (int pos = 1000; pos < 2100; pos += 10) {
+                     cam_pion.SetPosition(pos);  
+                     stm.printf("position %d\n", pos);
+                     wait_ms(10);
+                  }
+                  for (int pos = 2100; pos > 1000; pos -= 10) {
+                      cam_pion.SetPosition(pos); 
+                      wait_ms(10); 
+                  }
+              }*/
 }
 
 
@@ -175,7 +188,11 @@
     stm.baud(115200);
     t_sonar.start();
     distance = 0;
+    
+    cam_poziom.Enable(1450,20000);
+    cam_pion.Enable(1450,20000);
     move_camera();
+    czas.start();
     
     while(true)
     {
@@ -184,6 +201,7 @@
             eng_left.move(0);
             eng_right.move(0);
             }
+            
         if(t_sonar.read_ms()>1200){
             distance = sensor.distance(); 
             t_sonar.stop();
@@ -212,7 +230,15 @@
         {   
             t.stop();
             t.reset();
-            t.start();              
+            t.start();   
+            counter++;
+            if(czas.read_ms()>1000){
+                stm.printf("counter %d\n", counter);
+                counter=0;
+                czas.stop();
+                czas.reset();
+                czas.start();   
+            }           
         }
         if(bufor[9]=='$'){
             actualize_speed();
@@ -229,9 +255,17 @@
                 }
             }
         }
-        else if(bufor[9]=='&'){
+        else if(bufor[9]=='&'){            
             actualize_servo_values();
             move_camera();
+           /* counter++;
+            if(czas.read_ms()>1000){
+               // stm.printf("counter %d\n", counter);
+                counter=0;
+                czas.stop();
+                czas.reset();
+                czas.start();   
+            }*/
         }
         i=0;
         //send2rpi('s', distance);