Diff: main.cpp

Revision:

9:8e3392380915

Parent:

7:cb6947e1f1d3

--- a/main.cpp	Thu Feb 05 12:39:34 2015 +0000
+++ b/main.cpp	Mon Feb 09 09:20:58 2015 +0000
@@ -9,10 +9,12 @@
 #include "mbed.h"
 #include "config.h"
 
-bool Start();
+//bool Start(int speed);
+bool Start(float speed);
 void ShutOff();
 void Move();
 bool FrontIsClear();
+bool FrontIsClearU(); // with ultra sonic sensor
 bool MoveMeasure(int time, int& left, int& right);
 bool MoveTicks(int ticks);
 void TurnBack();
@@ -24,39 +26,32 @@
 void MoveMore(int anzahl);
 void Back();
 bool WaitUntilButtonPressed();
+int BottomIsBlack();    // with line sensor
 
 // Eigene Funktionsdefinitionen hier
-bool FrontIsClearU()
+
+void LedAround()
 {
-    int dist = 0;
-        usensor.start();
-        wait_ms(10);
-        dist=usensor.get_dist_cm();
-    if(dist < 5)
-        return false;
-    else
-        return true;
-        
-//        pc.printf("\r\nCount =%d",count);
-//        pc.printf("Distance: %d",dist );
-}    
+    int i=10;
+    int led = 0;
+
+    led = LED_FL1;  // 0x01
+
+    while(i-- > 0) {
+        TurnLedOn(led);
+        wait(1);
+        led *= 2;
+        if(led > 0x80)
+            led = LED_FL1;  // 0x01
+    }
+}
+
 // *************  Hauptprogramm ************
 int main()
 {
-    int dist = 0;
-    unsigned char count=0;
-/*    while(count < 20) {
-        usensor.start();
-        wait_ms(500);
-        dist=usensor.get_dist_cm();
-        pc.printf("\r\nCount =%d",count);
-        pc.printf("Distance: %d",dist );
-        count++;
-
-    }
-*/    // while(!Start()) {}   // wait to start
-    wait_ms(200);
-    while(FrontIsClearU())
+    Start(0.3);     // speed between 0.2 (low) and 1.0 (normal)
+    //Move();
+    while(!BottomIsBlack())
     {
         MoveTicks(40);
     }
@@ -66,23 +61,34 @@
         MoveTicks(40);
     }
 
-    // TurnLeftTicks(2000);    // Anzahl der MotorSensorwerte
-
+    TurnLeftTicks(200);    // Anzahl der Motorsensorwerte
+    Move();
     ShutOff();
     return 0;
 }
 
+// Line sensor at bottom of Bertl
+int BottomIsBlack()
+{
+    int detect;
+    
+    detect = linesensor;
+    wait_ms(5);
+    return detect;
+}
 
-bool Start()
+bool Start(float speed)
 {
+    pc.baud(9600);
+    pc.printf("Hello");
+
     DEBUG_PRINT("Debug level: %d", (int) DEBUG);
     i2c.frequency(40000);       // I2C Frequenz 40kHz
-
-    while(WaitUntilButtonPressed()) {    }
+    mg1 = mg2 = speed;
+//    while(WaitUntilButtonPressed()) {    }
     return true;
 }
 
-
 void ShutOff()
 {
     MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
@@ -105,7 +111,7 @@
         count++;
         wait_ms(1);                         // wait for 1 ms
     }
-    //DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
+    DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
 
     MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
     MotorR_EN=MotorL_EN=0;
@@ -133,7 +139,19 @@
     DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
     return wert;
 }
-
+// with ultra sonic sensor 
+bool FrontIsClearU()
+{
+    int dist = 0;
+    usensor.start();
+    wait_ms(10);
+    dist=usensor.get_dist_cm();
+    if(dist < 5)
+        return false;
+    else
+        return true;
+    DEBUG_PRINT("Distance: %d", dist);
+}
 /*
 use in main():
     int left=0, right=0;
@@ -242,11 +260,13 @@
 {
     MotorR_EN=1;            // motor right ENABLE
 
-    MotorR_FORWARD = 1;     // motor right forward ON
+    MotorR_FORWARD = MotorL_REVERSE = 1;
+    //MotorR_FORWARD = 1;     // motor right forward ON
     wait_ms(500);           // wait 500 ms (90°)
-    MotorR_FORWARD = 0;     // motor right forward OFF
-    MotorR_EN=0;
-
+    //MotorR_FORWARD = 0;     // motor right forward OFF
+    //MotorR_EN=0;
+    MotorL_REVERSE = MotorR_FORWARD = 0;
+    MotorL_EN=MotorR_EN=0;
     wait_ms(250);           // only to step the robot
 }
 
@@ -292,6 +312,22 @@
     wait(0.5);
 }
 
+void ManuelDistancMeasure()
+{
+    int dist = 0;
+    unsigned char count=0;
+
+    while(count < 20) {
+        usensor.start();
+        wait_ms(500);
+        dist=usensor.get_dist_cm();
+        pc.printf("\r\nCount =%d",count);
+        pc.printf("Distance: %d",dist );
+        count++;
+
+    }
+}
+
 /*
 void TurnAllLedOff()
 {