Diff: main.cpp

Revision:

3:c7e5419fa980

Parent:

2:43547160ab56

Child:

4:a975caedface

--- a/main.cpp	Sat Jan 03 14:18:50 2015 +0000
+++ b/main.cpp	Sat Jan 03 17:32:26 2015 +0000
@@ -17,45 +17,62 @@
 const int BTN_BL = 0x10;
 const int BTN_BM = 0x02;
 const int BTN_BR = 0x20;
-
-void move()
-{  
-    DigitalOut MotorL_EN(p34);
+bool frontIsClear();
+void shutOff()
+{
+ /*   DigitalOut MotorL_EN(p34);
     DigitalOut MotorL_FORWARD(P1_1);
-    //DigitalOut MotorL_REVERSE(P1_0);
-
     DigitalOut MotorR_EN(p36);
     DigitalOut MotorR_FORWARD(P1_3);
-    //DigitalOut MotorR_REVERSE(P1_4);
+*/    PwmOut mg1(p34);             //PWM Ausgang zum Motor
+    PwmOut mg2(p36);
+    mg1=mg2=0.0;
+
+ //    MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
+//    MotorR_EN=MotorL_EN=0;   
+}
+bool move()
+{
+    DigitalOut MotorL_EN(p34);
+    DigitalOut MotorL_FORWARD(P1_0);
+    //DigitalOut MotorL_REVERSE(P1_1);
+
+    DigitalOut MotorR_EN(p36);
+    DigitalOut MotorR_FORWARD(P1_4);
+    //DigitalOut MotorR_REVERSE(P1_3);
     PwmOut mg1(p34);             //PWM Ausgang zum Motor
     PwmOut mg2(p36);
-    mg1=mg2=1.0;
-
-    MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
+    if(!frontIsClear()) {
+        mg1=mg2=0.0;
+        return false;
+    } else {
+        mg1=mg2=1.0;
+        MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
 
-    MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
-    wait_ms(250);                           // warte 0,25 Sekunde
-    MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
-    MotorR_EN=MotorL_EN=0;
-    //mg1=mg2=0.0;
-    wait_ms(250);            
+        MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
+        wait_ms(250);                           // warte 0,25 Sekunde
+        MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
+        MotorR_EN=MotorL_EN=0;
+        //mg1=mg2=0.0;
+        wait_ms(250);
+        return true;
+    }
 }
 
 void moveSpeed(float speedy)
 {  
     DigitalOut MotorL_EN(p34);
-    DigitalOut MotorL_FORWARD(P1_1);
-    //DigitalOut MotorL_REVERSE(P1_0);
+    DigitalOut MotorL_FORWARD(P1_0);
+    //DigitalOut MotorL_REVERSE(P1_1);
 
     DigitalOut MotorR_EN(p36);
-    DigitalOut MotorR_FORWARD(P1_3);
-    //DigitalOut MotorR_REVERSE(P1_4);
+    DigitalOut MotorR_FORWARD(P1_4);
+    //DigitalOut MotorR_REVERSE(P1_3);
     PwmOut mg1(p34);             //PWM Ausgang zum Motor
     PwmOut mg2(p36);
     mg1=mg2=speedy;
     
     MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
-
     MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
     wait_ms(2000);                           // warte 0,25 Sekunde
     MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
@@ -69,6 +86,27 @@
         move();
     
 }
+void turnBack()
+{
+//    DigitalOut MotorL_EN(p34);
+//    DigitalOut MotorL_FORWARD(P1_1);
+//    DigitalOut MotorL_REVERSE(P1_0);
+
+    DigitalOut MotorR_EN(p36);
+//    DigitalOut MotorR_FORWARD(P1_4);
+    DigitalOut MotorR_REVERSE(P1_3);
+//  MotorL_EN=1;
+    PwmOut mg2(p36);
+    mg2=1.0;
+    MotorR_EN=1;      // rechten Motor ENABLE
+
+    MotorR_REVERSE = 1;  // rechter Motor vorwärts EIN 
+    wait_ms(750);        // warte (90°)
+    MotorR_REVERSE = 0;  // Motoren AUS
+    MotorR_EN=0;
+    
+    wait_ms(250);        // warte (90°)
+}
 
 void turnLeft()
 {
@@ -77,15 +115,15 @@
 //    DigitalOut MotorL_REVERSE(P1_0);
 
     DigitalOut MotorR_EN(p36);
-    DigitalOut MotorR_FORWARD(P1_3);
-//    DigitalOut MotorR_REVERSE(P1_4);
+    DigitalOut MotorR_FORWARD(P1_4);
+//    DigitalOut MotorR_REVERSE(P1_3);
 //  MotorL_EN=1;
     PwmOut mg2(p36);
     mg2=1.0;
     MotorR_EN=1;      // rechten Motor ENABLE
 
     MotorR_FORWARD = 1;  // rechter Motor vorwärts EIN 
-    wait_ms(750);        // warte (90°)
+    wait_ms(500);        // warte (90°)
     MotorR_FORWARD = 0;  // Motoren AUS
     MotorR_EN=0;
     
@@ -107,7 +145,7 @@
     i2c.write(addr, cmd, 1);
     i2c.read(addr|1, cmd, 1);
     btns = cmd[0];
-    if( btns == BTN_FM )
+    if( btns & (BTN_FL|BTN_FM|BTN_FR))
         return false;
     else
         return true;
@@ -126,10 +164,16 @@
     DigitalOut LED_D12(P1_10);    // LED D10 und D13 definieren
     DigitalOut LED_D13(P1_11);   //
 */
-    //while(frontIsClear()) {}
+    while(frontIsClear()) {}
 
+    move();
     turnLeft();
-    move();
+    if(move())
+        turnBack();
+    if(move())
+        turnBack();
+    if(move())
+        turnBack();
     wait(1);
 /*    moveSpeed(0.25);
     LED_D10 = 1;
@@ -153,8 +197,12 @@
     PwmOut mg2(p36);
     mg1=mg2=0.0;
     wait(2);
-*/    turnLeft();
+*/    
+    move(5);
+    turnLeft();
     move();
+    turnLeft();
+    shutOff();
     //turnLeft();
     //move(5);