Diff: main.cpp

Revision:

1:2e48f1a47d07

Parent:

0:2271f1e56f17

diff -r 2271f1e56f17 -r 2e48f1a47d07 main.cpp
--- a/main.cpp	Tue Apr 14 15:54:47 2015 +0000
+++ b/main.cpp	Tue Oct 13 16:46:33 2015 +0000
@@ -5,21 +5,15 @@
  
 SerialBLK pc(USBTX, USBRX);
 SvProtocol ua0(&pc);
-AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17);
 
 void CommandHandler();
-void followmaster();
+void DriveUntilObstacle();
+void Turn180();
 void blinkvorne();
 void blinkhinten();
-void messen();
-void rechts();
-void links();
-void gerade();
-
 Timer t1;
 unsigned int led=1;
 
-
 int main(void)
 {
   InitBertl();
@@ -32,11 +26,18 @@
   Timer stw; stw.start();
   while(1)
   { 
+    t1.start();
+    pex.ReadButtons(); 
+    if( pex.IsButton(BTN_FLL) )
+        {
+        DriveUntilObstacle(); 
+        }
+
     
-        
-        messen(); 
-
-     CommandHandler();
+       
+    
+    
+    CommandHandler();
     if( (stw.read_ms()>10) ) // 100Hz
     { // dieser Teil wird mit 100Hz aufgerufen
       stw.reset();
@@ -77,40 +78,90 @@
 }
 
 
-
-
+void DriveUntilObstacle()
+{
+   
+     //pex.SetLeds(LED_FL1|LED_FL2); 
+     mL.SetPow(0.5);
+     mR.SetPow(-0.5);  
+       
+     while(1)
+     {
+        us.StartMeas();
+        wait_ms(10);
+        if(led==0)
+        blinkvorne();
+        else
+        blinkhinten();
+        if ((us.distCM) < 5)
+        {
+         
+         Turn180();
+        wait_ms(10);
+        } 
+         
+         
+     }
+     
 
 
-void messen()
-{
-ls1.Read();
-ls2.Read();
-ls3.Read();
-ls4.Read();
-
-if (ls1.Read() > 800)
-rechts();
-
-else if (ls4.Read()> 800)
-links();
-
-else
-gerade();
 }
 
 
-void links()
+void Turn180()
+
 {
-    mL.SetPow(0.3);
-    mR.SetPow(0.0); 
+         if(led==0)
+         led=1;
+         else
+         led=0;
+         mL.SetPow(0.5);
+         mR.SetPow(0.5);
+         mL.encCnt=0;
+         mR.encCnt=0;
+        
+         while(1)
+    {
+        if (mL.encCnt>45)
+        mL.SetPow(0);
+        if (mR.encCnt>45)
+        mR.SetPow(0);
+        
+        if (mR.encCnt>48 && mL.encCnt>48 )
+        {
+            
+            DriveUntilObstacle();
+        }
+    }          
 }
-void rechts()
+
+void blinkvorne()
 {
-    mL.SetPow(0.0);
-    mR.SetPow(0.3); 
-}
-void gerade()
+    if(t1.read_ms() > 200)
+    {   pex.ClearLeds();
+        pex.SetLeds(LED_ALL_FRONT);
+        
+        }
+        
+         if(t1.read_ms() > 200)
+    {
+       pex.ClearLeds();
+        t1.reset();
+        }
+        
+    }
+    
+void blinkhinten()
 {
-    mL.SetPow(0.3);
-    mR.SetPow(0.3); 
-}
\ No newline at end of file
+     if(t1.read_ms() > 200)
+    {   pex.ClearLeds();
+        pex.SetLeds(LED_ALL_BACK);
+        
+        }
+        
+         if(t1.read_ms() > 200)
+    {
+       pex.ClearLeds();
+        t1.reset();
+        }
+    }
\ No newline at end of file