Revision 1:d40ff07e2fe0, committed 2017-05-10

Comitter:

EHess

Date:

Wed May 10 09:14:12 2017 +0000

Parent:

0:96f88638114b

Commit message:

Encoder und Sensorwerte

Changed in this revision

--- a/IRSensor.cpp	Tue Mar 21 14:57:54 2017 +0000
+++ b/IRSensor.cpp	Wed May 10 09:14:12 2017 +0000
@@ -1,67 +1,43 @@
-/*
- * IRSensor.cpp
- * Copyright (c) 2016, ZHAW
- * All rights reserved.
- */
+#include "IRSensor.h"
+#include <cmath>
 
-#include <cmath>
-#include "IRSensor.h"
-
+//E. Hess
+//IRSensor.cpp
 
-/**
- * Creates an IRSensor object.
- * @param distance an analog input object to read the voltage of the sensor.
- * @param bit0 a digital output to set the first bit of the multiplexer.
- * @param bit1 a digital output to set the second bit of the multiplexer.
- * @param bit2 a digital output to set the third bit of the multiplexer.
- * @param number the number of the sensor, either 0, 1, 2, 3, 4 or 5.
- */
-IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
-{
-    init(distance, bit0, bit1, bit2, number);
-}
+//Konstruktor -> Erstellt ein IRSensor-Objekt
+//AnalogIn* distance -> Liest die Voltanzahl des Sensors aus
+//DigitalOut* bit0, bit1, bit2 -> Binär-Outputs wählen die 6 Sensoren an
+//Int number -> Sensornummer als int (0-5)
 
-
-IRSensor::IRSensor()
-{
-}
+IRSensor::IRSensor() {}
 
-void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
-{
-
-    this->distance = distance;  // set local references to objects
+IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) {
+    this->distance = distance;  //Weist den Objektvariablen, die eingegebenen Werte zu
     this->bit0 = bit0;
     this->bit1 = bit1;
     this->bit2 = bit2;
-
     this->number = number;
 }
 
-
-/**
- * Deletes the IRSensor object.
- */
+//Destruktor -> Löscht das IRSensor-Objekt
 IRSensor::~IRSensor() {}
 
-/**
- * Gets the distance measured with the IR sensor in [m].
- * @return the distance, given in [m].
- */
+//Initialisiert nachträglich
+void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) {
+    this->distance = distance;  //Weist den Objektvariablen, die eingegebenen Werte zu
+    this->bit0 = bit0;
+    this->bit1 = bit1;
+    this->bit2 = bit2;
+    this->number = number;
+}
+
+//Distanzrechner
 float IRSensor::read()
 {
-    *bit0 = (number >> 0) & 1;
-    *bit1 = (number >> 1) & 1;
-    *bit2 = (number >> 2) & 1;
-
-    float d = -0.38f*sqrt(distance->read())+0.38f;  // calculate the distance in [m]
-    return d;
-}
+    *bit0 = (number >> 0) & 1;  //Vergleicht das Least-Significant-Bit von number mit 1 und setzt *bit0 (z.B. 5: 0000'0101 & 0000'0001 == 0000'0001)
+    *bit1 = (number >> 1) & 1;  //Vergleicht das zweite Bit von rechts
+    *bit2 = (number >> 2) & 1;  //Vergleicht das dritte Bit von rechts
 
-/**
- * The empty operator is a shorthand notation of the <code>read()</code> method.
- */
-IRSensor::operator float()
-{
-
-    return read();
-}
+    float d = distance->read();
+    return d;
+}
\ No newline at end of file

--- a/IRSensor.h	Tue Mar 21 14:57:54 2017 +0000
+++ b/IRSensor.h	Wed May 10 09:14:12 2017 +0000
@@ -1,40 +1,25 @@
-/*
- * IRSensor.h
- * Copyright (c) 2016, ZHAW
- * All rights reserved.
- */
-
-#ifndef IR_SENSOR_H_
-#define IR_SENSOR_H_
+#ifndef IR_SENSOR_H
+#define IR_SENSOR_H
 
-#include <cstdlib>
-#include <mbed.h>
+#include "mbed.h"
 
-/**
- * This is a device driver class to read the distance measured with a Sharp IR sensor.
- */
-class IRSensor
-{
+//E. Hess
+//IRSensor.h
 
-public:
-
-    IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
-    IRSensor();
-    
-    void        init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
-    virtual     ~IRSensor();
-    float       read();
+class IRSensor {
+    public:
+        IRSensor();
+        IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
+        ~IRSensor();
+        void init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
+        float read();
 
-    operator float();
-
-private:
-
-    AnalogIn*       distance;
-    DigitalOut*     bit0;
-    DigitalOut*     bit1;
-    DigitalOut*     bit2;
-
-    int             number;
+    private:
+        AnalogIn*       distance;
+        DigitalOut*     bit0;
+        DigitalOut*     bit1;
+        DigitalOut*     bit2;
+        int             number;
 };
 
-#endif /* IR_SENSOR_H_ */
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.cpp	Wed May 10 09:14:12 2017 +0000
@@ -0,0 +1,107 @@
+#include "LowpassFilter.h"
+
+//E. Hess
+//LowpassFilter.cpp
+
+using namespace std;
+
+/**
+ * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s].
+ */
+LowpassFilter::LowpassFilter() {
+    
+    period = 1.0f;
+    frequency = 1000.0f;
+    
+    a11 = (1.0f+frequency*period)*exp(-frequency*period);
+    a12 = period*exp(-frequency*period);
+    a21 = -frequency*frequency*period*exp(-frequency*period);
+    a22 = (1.0f-frequency*period)*exp(-frequency*period);
+    b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+    b2 = period*exp(-frequency*period);
+    
+    x1 = 0.0f;
+    x2 = 0.0f;
+}
+
+/**
+ * Deletes the LowpassFilter object.
+ */
+LowpassFilter::~LowpassFilter() {}
+
+/**
+ * Resets the filtered value to zero.
+ */
+void LowpassFilter::reset() {
+    
+    x1 = 0.0f;
+    x2 = 0.0f;
+}
+
+/**
+ * Resets the filtered value to a given value.
+ * @param value the value to reset the filter to.
+ */
+void LowpassFilter::reset(float value) {
+    
+    x1 = value/frequency/frequency;
+    x2 = (x1-a11*x1-b1*value)/a12;
+}
+
+/**
+ * Sets the sampling period of the filter.
+ * This is typically the sampling period of the realtime thread of a controller that uses this filter.
+ * @param the sampling period, given in [s].
+ */
+void LowpassFilter::setPeriod(float period) {
+    
+    this->period = period;
+    
+    a11 = (1.0f+frequency*period)*exp(-frequency*period);
+    a12 = period*exp(-frequency*period);
+    a21 = -frequency*frequency*period*exp(-frequency*period);
+    a22 = (1.0f-frequency*period)*exp(-frequency*period);
+    b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+    b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Sets the corner frequency of this filter.
+ * @param frequency the corner frequency of the filter in [rad/s].
+ */
+void LowpassFilter::setFrequency(float frequency) {
+    
+    this->frequency = frequency;
+    
+    a11 = (1.0f+frequency*period)*exp(-frequency*period);
+    a12 = period*exp(-frequency*period);
+    a21 = -frequency*frequency*period*exp(-frequency*period);
+    a22 = (1.0f-frequency*period)*exp(-frequency*period);
+    b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+    b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Gets the current corner frequency of this filter.
+ * @return the current corner frequency in [rad/s].
+ */
+float LowpassFilter::getFrequency() {
+    
+    return frequency;
+}
+
+/**
+ * Filters a value.
+ * @param value the original unfiltered value.
+ * @return the filtered value.
+ */
+float LowpassFilter::filter(float value) {
+
+    float x1old = x1;
+    float x2old = x2;
+    
+    x1 = a11*x1old+a12*x2old+b1*value;
+    x2 = a21*x1old+a22*x2old+b2*value;
+    
+    return frequency*frequency*x1;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.h	Wed May 10 09:14:12 2017 +0000
@@ -0,0 +1,36 @@
+#ifndef LOWPASS_FILTER_H_
+#define LOWPASS_FILTER_H_
+
+#include <cstdlib>
+#include <cmath>
+
+//E. Hess
+//LowpassFilter.cpp
+
+/**
+ * This class implements a time-discrete 2nd order low-pass filter for a series of data values.
+ * This filter can typically be used within a periodic task that takes measurements that need
+ * to be filtered, like speed or position values.
+ */
+class LowpassFilter {
+    
+    public:
+    
+                LowpassFilter();
+        virtual ~LowpassFilter();
+        void    reset();
+        void    reset(float value);
+        void    setPeriod(float period);
+        void    setFrequency(float frequency);
+        float   getFrequency();
+        float   filter(float value);
+        
+    private:
+        
+        float   period;
+        float   frequency;
+        float   a11, a12, a21, a22, b1, b2;
+        float   x1, x2;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorEncoder.cpp	Wed May 10 09:14:12 2017 +0000
@@ -0,0 +1,115 @@
+#include "MotorEncoder.h"
+
+//E. Hess
+//MotorEncoder.h
+
+using namespace std;
+
+// Erstellt einen Treiber um den Encoder zu lesen und initialisiert ihn
+// PinName a ist der Input für Kanal A
+// PinName b ist der Input für Kanal B
+
+MotorEncoder::MotorEncoder(PinName a, PinName b) {
+    // Überprüft die Inputs
+    if ((a == PA_6) && (b == PC_7)) {   // Rechter Motor
+        // pinmap OK for TIM3 CH1 and CH2
+        TIM = TIM3;
+        
+        // Kunfiguration vom Reset und der Taktsteuerung
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;    // manually enable port C (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
+        GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
+        GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
+        
+        GPIOC->MODER &= ~GPIO_MODER_MODER7;     // reset port C7
+        GPIOC->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port C7
+        GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port C7
+        GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOC->AFR[0] &= ~0xF0000000;           // reset alternate function of port C7
+        GPIOC->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port C7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
+        
+    } else if ((a == PB_6) && (b == PB_7)) {    // Links Motor
+        
+        // pinmap OK for TIM4 CH1 and CH2
+        
+        TIM = TIM4;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER6;     // reset port B6
+        GPIOB->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port B6
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port B6
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port B6
+        GPIOB->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port B6
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER7;     // reset port B7
+        GPIOB->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port B7
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port B7
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B7
+        GPIOB->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port B7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST;  //reset TIM4 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;     // TIM4 clock enable
+        
+    } else {
+        printf("Pinmap konnte nicht gefunden werden!\n");
+    }
+    
+    // configure general purpose timer 3 or 4
+    
+    TIM->CR1 = 0x0000;          // counter disable
+    TIM->CR2 = 0x0000;          // reset master mode selection
+    TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+    TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+    TIM->CCMR2 = 0x0000;        // reset capture mode register 2
+    TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+    TIM->CNT = 0x0000;          // reset counter value
+    TIM->ARR = 0xFFFF;          // auto reload register
+    TIM->CR1 = TIM_CR1_CEN;     // counter enable
+}
+
+MotorEncoder::~MotorEncoder() {}
+
+// Setzt den Zähler auf NULL
+void MotorEncoder::reset() {
+    
+    TIM->CNT = 0x0000;
+}
+
+// Setzt den Zähler auf NULL zurück
+void MotorEncoder::reset(short offset) {
+    TIM->CNT = -offset;
+}
+
+// Liest den Zähler
+short MotorEncoder::read() {
+    return (short)(-TIM->CNT);
+}
+
+// Kurznotation für read()
+MotorEncoder::operator short() {
+    return read();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorEncoder.h	Wed May 10 09:14:12 2017 +0000
@@ -0,0 +1,23 @@
+#ifndef MOTOR_ENCODER_H
+#define MOTOR_ENCODER_H
+
+#include <cstdlib>
+#include <mbed.h>
+
+//E. Hess
+//MotorEncoder.h
+
+class MotorEncoder {
+    public:
+        MotorEncoder(PinName a, PinName b);
+        virtual ~MotorEncoder();
+        void    reset();
+        void    reset(short offset);
+        short   read();
+        operator short();
+        
+    private:
+        TIM_TypeDef*    TIM;
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Tue Mar 21 14:57:54 2017 +0000
+++ b/main.cpp	Wed May 10 09:14:12 2017 +0000
@@ -1,75 +1,144 @@
 #include "mbed.h"
 #include "IRSensor.h"
-
-//E. Hess
-//Robotterbewegungen
+#include "MotorEncoder.h"
+#include "LowpassFilter.h"
 
-DigitalOut led(LED1);
+DigitalOut led(LED1);           //Zustands-LED: Grüne LED für Benutzer
 
-//Erstellt In- / Outputs
-AnalogIn distance(PB_1); 
-DigitalOut enable(PC_1);
-DigitalOut bit0(PH_1);
+AnalogIn distance(PB_1);        //Input der Distanzsensoren
+DigitalOut enableSensor(PC_1);  //Aktivierung der IRSensoren
+DigitalOut bit0(PH_1);          //Ansteuerung der Sensoren 0-5 mit 3 Bits
 DigitalOut bit1(PC_2);
 DigitalOut bit2(PC_3);
-IRSensor sensors[6];        //-> Was
+IRSensor sensors[6];            //Erstellt 6 IRSensor-Objekte als Array
 
-//LED Indikatoren rund um Roboter
-DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
-
-//Timer-Objekt für LED- und Distanzsensor
-Ticker t1;
+DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };    //LED-Outputs der Sensoren
 
-//Motoren
-DigitalOut enableMotorDriver(PB_2); //Erstellt das Objekt
-PwmOut pwmL(PA_8);
-PwmOut pwmR(PA_9);
 
-//DistanzLEDs ->Was
-void ledDistance(){
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0;
+//Titel printf()
+void title() {
+    printf("\f  <    \t\t   -   \t\t    >\n\r"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite
 }
 
-//Blinkt beim Start und startet die DistanzLEDs
-void ledShow(){
-    static int timer = 0;
-    for( int ii = 0; ii<6; ++ii)
-        leds[ii] = !leds[ii];
+
+const float PERIOD = 0.002f;                    // period of control task, given in [s]
+const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
+const float LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
+const float KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
+const float KP = 0.2f;                          // speed controller gain, given in [V/rpm]
+const float MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
+const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
+const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
+
+MotorEncoder counterLeft(PB_6, PB_7);
+MotorEncoder counterRight(PA_6, PC_7);
+
+LowpassFilter speedLeftFilter;
+LowpassFilter speedRightFilter;
+
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmLeft(PA_8);
+PwmOut pwmRight(PA_9);
+
+DigitalOut my_led(LED1);
+
+short previousValueCounterRight = 0;
+short previousValueCounterLeft = 0;
+
+float desiredSpeedLeft;
+float desiredSpeedRight;
 
-    //Beendet den Ticker und startet die DistanzLED-Show
-    if( ++timer > 10) {
-        t1.detach();
-        t1.attach( &ledDistance, 0.01f );
-    }
+float actualSpeedLeft;
+float actualSpeedRight;
+
+void speedCtrl()
+{
+    // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
+    short valueCounterLeft = counterLeft.read();
+    short valueCounterRight = counterRight.read();
+    short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+    short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+    previousValueCounterLeft = valueCounterLeft;
+    previousValueCounterRight = valueCounterRight;
+    actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
+    actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
+
+    // Berechnen der Motorspannungen Uout
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+
+    // Berechnen, Limitieren und Setzen der Duty-Cycle
+    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+    if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
+    else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
+
+    pwmLeft = dutyCycleLeft;
+    float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
+    if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
+    else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
+
+    pwmRight = dutyCycleRight;
 }
 
-int main()
-{
-    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs -> f speichert float anstatt double (schneller)
-    pwmR.period(0.00005f);
-    pwmL = 0.5f; // Setzt die Duty-Cycle auf 50%
-    pwmR = 0.5f;
-    enableMotorDriver = 1;
+int main(){
+    
+    // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmRight.period(0.00005f);
+    pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
+    pwmRight = 0.5f; // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+    speedLeftFilter.setPeriod(PERIOD);
+    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+    speedRightFilter.setPeriod(PERIOD);
+    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
 
-    t1.attach( &ledShow, 0.05f ); //->Was
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    Ticker t1;
+    t1.attach( &speedCtrl, PERIOD);
 
-    //Initialisiert Distanzsensoren
-    for( int ii = 0; ii<6; ++ii)
-        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
+    desiredSpeedLeft = 15.0f; //50 RPM
+    desiredSpeedRight = 15.0f; //50 RPM
+    enableMotorDriver = 1;
+    
+    float sensorMittelwert[6];   //Array der 6 Sensorenwerte
+    float sensorTiefbass[6];
+    int zaehler = 0;
 
-    enable = 1;
+    //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL
+    for( int i = 0; i < 6; i++) {
+        sensors[i].init(&distance, &bit0, &bit1, &bit2, i);
+        sensorMittelwert[i] = 0.0f;
+        sensorTiefbass[i] = 0.0f;
+    }
+    enableSensor = 1;   //Aktiviert die IRSensoren
 
     while(1) {
-        wait( 0.2f );
-        if(sensors[0].read() < 0.2f){
-            pwmL = 0.55f;
-            pwmR = 0.55f;
+        for(int j = 0; j < 25; j++){                 //Zählt 25 Sensorwerten pro Sensor zusammen
+            for(int i = 0; i < 6; i++){
+                sensorMittelwert[i] += sensors[i].read();
+            }
+            wait( 0.001f );
+        }
+        for(int i = 0; i < 6; i++){
+            sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f;    //Verrechnet den neuen Wert mit dem alten
+            sensorMittelwert[i] = 0.0f;                                                 //Setzt die Sensorwerte auf NULL
         }
-        else{
-            pwmL = 0.6f;
-            pwmR = 0.4f;
+        
+        printf("%f\t%f\t%f\n\r", sensorTiefbass[5], sensorTiefbass[0], sensorTiefbass[1]);   //Plottet die unteren Sensoren + Mitte-Oben
+        
+        zaehler++;
+        if(zaehler % 120 == 0){
+            wait(3.5f);
         }
-//float x = sensor[0];
+        if(zaehler % 40 == 0) title();  //Erstellt nach 40 Zeilen eine neue Seite
     }
-}
+}
\ No newline at end of file