Diff: Roboter.cpp

Revision:

1:4e84271a70c6

Parent:

0:306a2438de17

Child:

2:953263712480

diff -r 306a2438de17 -r 4e84271a70c6 Roboter.cpp
--- a/Roboter.cpp	Tue Mar 21 12:42:01 2017 +0000
+++ b/Roboter.cpp	Tue Apr 04 11:55:54 2017 +0000
@@ -1,50 +1,60 @@
-
 #include "Roboter.h"
 
 
+Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR)
+{
 
-//timer object for ledshow and distance sensor
-Ticker t1;
+    sensors[0].init( distance, bit0, bit1, bit2, 0);
+    sensors[1].init( distance, bit0, bit1, bit2, 1);
+    sensors[2].init( distance, bit0, bit1, bit2, 2);
+    sensors[3].init( distance, bit0, bit1, bit2, 3);
+    sensors[4].init( distance, bit0, bit1, bit2, 4);
+    sensors[5].init( distance, bit0, bit1, bit2, 5);
 
-//lights up the led according to the sensor signal
-void ledDistance()
+    this->pwmR = pwmR;
+    this->pwmL = pwmL;
+
+    pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmR->period(0.00005f);
+
+}
+
+float Roboter::readSensor1()
 {
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0;
+    return sensors[0].read();
+
 }
 
-//blinks at startup and starts ledDistance task
-void ledShow()
+
+void Roboter::bandeAusweichen()
 {
-    static int timer = 0;
-    for( int ii = 0; ii<6; ++ii)
-        leds[ii] = !leds[ii];
+    float offsetDir;
+    float offsetLin;
+
+    float x=0.13f;       // Distanz ab welcher sensoren reagieren sollen
+
+
+    offsetDir = 0;
+    offsetLin = 0.1;
+
 
-    //quit ticker and start led distance show
-    if( ++timer > 10) {
-        t1.detach();
-        t1.attach( &ledDistance, 0.01f );
+    if(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
+        offsetDir = -0.05;
+        offsetLin = 0;
+    }
+    if(sensors[1] < x) {     // sensor rechts, roboter dreht nach links
+        offsetDir = -0.05;
+        offsetLin = 0;
     }
+    if(sensors[5] < x && sensors[1]>(x+0.02f)) {     // sensor links, roboter dreht nach rechts
+        offsetDir = 0.05;
+        offsetLin = 0;
+    }
+
+    *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
+    *pwmR = 0.5f+offsetDir-offsetLin;
+
+    wait(0.1);
+
 }
 
-int IRSensor()
-{
-    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
-    pwmR.period(0.00005f);
-
-    pwmL = 0.5f; // Setzt die Duty-Cycle auf 50%
-    pwmR = 0.5f;
-    enableMotorDriver = 0;
-
-    t1.attach( &ledShow, 0.05f );
-
-    //init distance sensors
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
-
-    enable = 1;
-
-    while(1) {
-        wait( 0.2f );
-    }
-}