Revision 2:f381fc3a8eaf, committed 2020-02-26

Comitter:

wengefa1

Date:

Wed Feb 26 14:20:16 2020 +0000

Parent:

1:8b600c187fe6

Commit message:

Motor Controller added

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Wed Feb 26 14:20:16 2020 +0000
@@ -0,0 +1,135 @@
+/*
+ * Controller.cpp
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f;                    // period of 1 ms
+const float Controller::COUNTS_PER_TURN = 86016.0f;          // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
+const float Controller::KN = 45.0f;                         // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
+const float Controller::KP = 0.12f;                         // speed control parameter
+const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
+const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
+
+/**
+ * Creates and initialises the robot controller.
+ * @param pwmLeft a reference to the pwm output for the left motor.
+ * @param pwmRight a reference to the pwm output for the right motor.
+ * @param counterLeft a reference to the encoder counter of the left motor.
+ * @param counterRight a reference to the encoder counter of the right motor.
+ */
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
+    
+    // initialise pwm outputs
+
+    pwmLeft.period(0.00005f);  // pwm period of 50 us
+    pwmLeft = 0.5f;            // duty-cycle of 50%
+
+    pwmRight.period(0.00005f); // pwm period of 50 us
+    pwmRight = 0.5f;           // duty-cycle of 50%
+
+    // initialise local variables
+
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+
+    speedLeftFilter.setPeriod(PERIOD);
+    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    speedRightFilter.setPeriod(PERIOD);
+    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    // start the periodic task
+
+    ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+/**
+ * Deletes this Controller object.
+ */
+Controller::~Controller() {
+    
+    ticker.detach(); // stop the periodic task
+}
+
+/**
+ * Sets the desired speed of the left motor.
+ * @param desiredSpeedLeft desired speed given in [rpm].
+ */
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
+
+    this->desiredSpeedLeft = desiredSpeedLeft;
+}
+
+/**
+ * Sets the desired speed of the right motor.
+ * @param desiredSpeedRight desired speed given in [rpm].
+ */
+void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
+
+    this->desiredSpeedRight = desiredSpeedRight;
+}
+
+/**
+ * This is an internal method of the controller that is running periodically.
+ */
+void Controller::run() {
+
+    // calculate the actual speed of the motors 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);
+
+    // calculate desired motor voltages Uout   
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+
+    // calculate, limit and set the 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;
+}
+
+float Controller::getSpeedLeft(){
+    return actualSpeedLeft;
+    }
+
+float Controller::getSpeedRight(){
+    return actualSpeedRight;
+    }
+
+float Controller::getDesSpeedLeft(){
+    return desiredSpeedLeft;
+    }
+
+float Controller::getDesSpeedRight(){
+    return desiredSpeedRight;
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Wed Feb 26 14:20:16 2020 +0000
@@ -0,0 +1,61 @@
+/*
+ * Controller.h
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef CONTROLLER_H_
+#define CONTROLLER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+
+/**
+ * This class implements a controller that regulates the
+ * speed of the two motors of the ROME2 mobile robot.
+ */
+class Controller {
+    
+    public:
+        
+                Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight);
+        virtual ~Controller();
+        void    setDesiredSpeedLeft(float desiredSpeedLeft);
+        void    setDesiredSpeedRight(float desiredSpeedRight);
+        float   getSpeedLeft(); 
+        float   getSpeedRight(); 
+        float   getDesSpeedLeft();
+        float   getDesSpeedRight();
+        
+  private:
+        
+        static const float  PERIOD;
+        static const float  COUNTS_PER_TURN;
+        static const float  LOWPASS_FILTER_FREQUENCY;
+        static const float  KN;
+        static const float  KP;
+        static const float  MAX_VOLTAGE;
+        static const float  MIN_DUTY_CYCLE;
+        static const float  MAX_DUTY_CYCLE;
+
+        PwmOut&             pwmLeft;
+        PwmOut&             pwmRight;
+        EncoderCounter&     counterLeft;
+        EncoderCounter&     counterRight;
+        short               previousValueCounterLeft;
+        short               previousValueCounterRight;
+        LowpassFilter       speedLeftFilter;
+        LowpassFilter       speedRightFilter;
+        float               desiredSpeedLeft;
+        float               desiredSpeedRight;
+        float               actualSpeedLeft;
+        float               actualSpeedRight;
+        Ticker              ticker;
+        
+        void    run();
+};
+
+#endif /* CONTROLLER_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Wed Feb 26 14:20:16 2020 +0000
@@ -0,0 +1,182 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initialises the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+    
+    // check pins
+    
+    if ((a == PA_15) && (b == PB_3)) {
+        
+        // pinmap OK for TIM2 CH1 and CH2
+        
+        TIM = TIM2;
+        
+        // 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
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER15;    // reset port A15
+        GPIOA->MODER |= GPIO_MODER_MODER15_1;   // set alternate mode of port A15
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR15;    // reset pull-up/pull-down on port A15
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR15_1;   // set input as pull-down
+        GPIOA->AFR[1] &= ~0xF0000000;           // reset alternate function of port A15
+        GPIOA->AFR[1] |= 1 << 4*7;              // set alternate funtion 1 of port A15
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER3;     // reset port B3
+        GPIOB->MODER |= GPIO_MODER_MODER3_1;    // set alternate mode of port B3
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR3;     // reset pull-up/pull-down on port B3
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR3_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*3);         // reset alternate function of port B3
+        GPIOB->AFR[0] |= 1 << 4*3;              // set alternate funtion 1 of port B3
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM2RST;  //reset TIM2 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM2RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;     // TIM2 clock enable
+        
+    } else if ((a == PB_4) && (b == PC_7)) {
+        
+        // pinmap OK for TIM3 CH1 and CH2
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;    // manually enable port C
+        
+        // configure general purpose I/O registers
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER4;     // reset port B4
+        GPIOB->MODER |= GPIO_MODER_MODER4_1;    // set alternate mode of port B4
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR4;     // reset pull-up/pull-down on port B4
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR4_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*4);         // reset alternate function of port B4
+        GPIOB->AFR[0] |= 2 << 4*4;              // set alternate funtion 2 of port B4
+        
+        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 == PD_12) && (b == PD_13)) {
+        
+        // pinmap OK for TIM4 CH1 and CH2
+        
+        TIM = TIM4;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;    // manually enable port D
+        
+        // configure general purpose I/O registers
+        
+        GPIOD->MODER &= ~GPIO_MODER_MODER12;    // reset port D12
+        GPIOD->MODER |= GPIO_MODER_MODER12_1;   // set alternate mode of port D12
+        GPIOD->PUPDR &= ~GPIO_PUPDR_PUPDR12;    // reset pull-up/pull-down on port D12
+        GPIOD->PUPDR |= GPIO_PUPDR_PUPDR12_1;   // set input as pull-down
+        GPIOD->AFR[1] &= ~(0xF << 4*4);         // reset alternate function of port D12
+        GPIOD->AFR[1] |= 2 << 4*4;              // set alternate funtion 2 of port D12
+        
+        GPIOD->MODER &= ~GPIO_MODER_MODER13;    // reset port D13
+        GPIOD->MODER |= GPIO_MODER_MODER13_1;   // set alternate mode of port D13
+        GPIOD->PUPDR &= ~GPIO_PUPDR_PUPDR13;    // reset pull-up/pull-down on port D13
+        GPIOD->PUPDR |= GPIO_PUPDR_PUPDR13_1;   // set input as pull-down
+        GPIOD->AFR[1] &= ~(0xF << 4*5);         // reset alternate function of port D13
+        GPIOD->AFR[1] |= 2 << 4*5;              // set alternate funtion 2 of port D13
+        
+        // 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 not found for peripheral\n");
+        
+        TIM = NULL;
+    }
+    
+    // configure general purpose timer 2, 3 or 4
+    
+    if (TIM != NULL) {
+        
+        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
+    }
+}
+
+/**
+ * Deletes this EncoderCounter object.
+ */
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+    
+    TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::reset(short offset) {
+    
+    TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+short EncoderCounter::read() {
+    
+    return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator short() {
+    
+    return read();
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h	Wed Feb 26 14:20:16 2020 +0000
@@ -0,0 +1,34 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+    
+    public:
+        
+                    EncoderCounter(PinName a, PinName b);
+        virtual     ~EncoderCounter();
+        void        reset();
+        void        reset(short offset);
+        short       read();
+                    operator short();
+        
+    private:
+        
+        TIM_TypeDef*    TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */
+

--- a/IRSensor.cpp	Wed Feb 26 12:49:12 2020 +0000
+++ b/IRSensor.cpp	Wed Feb 26 14:20:16 2020 +0000
@@ -38,7 +38,7 @@
     bit1 = (number >> 1)&1;
     bit2 = (number >> 2)&1; 
     
-    float dist = -0.58f*(sqrt(distance)+0.58f); 
+    float dist = -0.58f*sqrt(distance)+0.58f; 
     
     return dist; 
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.cpp	Wed Feb 26 14:20:16 2020 +0000
@@ -0,0 +1,112 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "LowpassFilter.h"
+
+using namespace std;
+
+/**
+ * Creates a LowpassFilter object with a default cutoff 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 this 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 periodic task 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 cutoff frequency of this filter.
+ * @param frequency the cutoff 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 cutoff frequency of this filter.
+ * @return the current cutoff 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 Feb 26 14:20:16 2020 +0000
@@ -0,0 +1,39 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef LOWPASS_FILTER_H_
+#define LOWPASS_FILTER_H_
+
+#include <cstdlib>
+
+/**
+ * This class implements a time-discrete 2nd order lowpass 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 /* LOWPASS_FILTER_H_ */
+

--- a/main.cpp	Wed Feb 26 12:49:12 2020 +0000
+++ b/main.cpp	Wed Feb 26 14:20:16 2020 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "IRSensor.h"
+#include "Controller.h"
 
 DigitalOut myled(LED1);
 DigitalIn mybutton(BUTTON1); 
@@ -20,9 +21,10 @@
 DigitalOut en_Motor(PG_0);
 DigitalIn Error_Motor(PD_1);
 DigitalIn Warn_Motor(PD_0); 
-PWMOut MotorLeft(PF_9); 
-PWMOut MotorRight(PF_8); 
-
+PwmOut MotorPWMLeft(PF_9); 
+PwmOut MotorPWMRight(PF_8); 
+EncoderCounter cnt_Left(PD_12,PD_13);
+EncoderCounter cnt_Right(PB_4, PC_7);
 
 IRSensor S_hinten(v_distance, bit0 , bit1, bit2, 0);
 IRSensor S_hinten_links(v_distance, bit0 , bit1, bit2, 1);
@@ -31,57 +33,59 @@
 IRSensor S_vorne_rechts(v_distance, bit0 , bit1, bit2, 4);
 IRSensor S_hinten_rechts(v_distance, bit0 , bit1, bit2, 5);
 
+Controller controller(MotorPWMLeft, MotorPWMRight, cnt_Left, cnt_Right);
 int main() { 
     //Set Motor Settings
-    MotorLeft.period(0.00005); 
-    MotorRight.period(0.00005); 
-    
-    MotorLeft = 0.5; 
-    MotorRight = 0.5;
-
     en_Motor = 1; 
     
-    //Set IR Setting
+    //Set IR Settings
     enable = 1; 
     
 
     while(1) {
-        float distance0 = S_hinten.read();  // gibt die Distanz in [m] zurueck  
-        float distance1 = S_hinten_links.read();   
-        float distance2 = S_vorne_links.read();   
-        float distance3 = S_vorne.read();   
-        float distance4 = S_vorne_rechts.read();   
-        float distance5 = S_hinten_rechts.read();
+        float dist_hinten = S_hinten.read();  // gibt die Distanz in [m] zurueck  
+        float dist_hinten_links = S_hinten_links.read();   
+        float dist_vorne_links= S_vorne_links.read();   
+        float dist_vorne = S_vorne.read();   
+        float dist_vorne_rechts = S_vorne_rechts.read();   
+        float dist_hinten_rechts = S_hinten_rechts.read();
+       
         
-        if(distance0 < 0.2f){
+        if(dist_hinten < 0.2f){
             led0 = 1; 
         }else{
             led0 = 0;    
         }
-        if(distance1 < 0.2f){
+        if(dist_hinten_links < 0.2f){
             led1 = 1; 
         }else{
             led1 = 0;    
         }
-        if(distance2 < 0.2f){
+        if(dist_vorne_links < 0.2f){
             led2 = 1; 
         }else{
             led2 = 0;    
         }
-        if(distance3 < 0.2f){
+        if(dist_vorne < 0.2f){
             led3 = 1; 
         }else{
             led3 = 0;    
         }
-        if(distance4 < 0.2f){
+        if(dist_vorne_rechts < 0.2f){
             led4 = 1; 
         }else{
             led4 = 0;    
         }
-        if(distance5 < 0.2f){
+        if(dist_hinten_rechts < 0.2f){
             led5 = 1; 
         }else{
             led5 = 0;    
         }
+        
+        controller.setDesiredSpeedLeft(50.0); 
+        controller.setDesiredSpeedRight(-50.0); 
+        
+        printf("Soll Links:%f    Ist Links%f    Soll Rechts:%f     Ist Rechts:%f \n\r",controller.getDesSpeedLeft(),controller.getSpeedLeft(),controller.getDesSpeedRight(),controller.getSpeedRight());
+        
     }
 }