Revision 0:6d0671ae4648, committed 2018-04-23

Comitter:

Jacqueline

Date:

Mon Apr 23 11:28:11 2018 +0000

Child:

1:a95ba1510438

Commit message:

kann fahren und drehen

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,147 @@
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f; // Periode von 1 ms
+const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
+const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V]
+const float Controller::KP = 0.25f; // KP Regler-Parameter
+const float Controller::KI = 4.0f; // KI Regler-Parameter
+const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung
+const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle
+const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle
+
+int ii =0;
+
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
+                        EncoderCounter& counterLeft, EncoderCounter& counterRight) :
+                        pwmLeft(pwmLeft), pwmRight(pwmRight),
+                        counterLeft(counterLeft), counterRight(counterRight) {
+
+    // Initialisieren der PWM Ausgaenge
+    
+    pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    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);
+
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    // Starten des periodischen Tasks
+    ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+Controller::~Controller()
+{
+    ticker.detach(); // Stoppt den periodischen Task
+}
+
+
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
+{
+    this->desiredSpeedLeft = desiredSpeedLeft;
+}
+
+void Controller::setDesiredSpeedRight(float desiredSpeedRight)
+{
+    this->desiredSpeedRight = desiredSpeedRight;
+}
+
+float Controller::getSpeedLeft()
+{
+    return actualSpeedLeft;
+}
+
+float Controller::getSpeedRight()
+{
+    return actualSpeedRight;
+}
+
+float Controller::getIntegralLeft()
+{
+    return iSumLeft;
+}
+
+float Controller::getIntegralRight()
+{
+    return iSumRight;
+}
+
+float Controller::getProportionalLeft()
+{
+    return (desiredSpeedLeft-actualSpeedLeft);
+}
+
+float Controller::getProportionalRight()
+{
+    return (desiredSpeedRight-actualSpeedRight);
+}
+
+void Controller::run() {
+
+    // 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);
+
+
+    //Berechnung I - Anteil
+
+    
+    iSumLeft += (desiredSpeedLeft-actualSpeedLeft); 
+    if (iSumLeft > I_MAX) iSumLeft = I_MAX;  //Max Saettigung I - Anteil       
+    if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil
+
+    iSumRight += (desiredSpeedRight-actualSpeedRight); 
+    if (iSumRight > I_MAX) iSumRight = I_MAX;  //Max Saettigung I - Anteil       
+    if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil
+       
+    // Berechnen der Motorspannungen Uout
+        
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD
+                        +desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD
+                         +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;
+    
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,66 @@
+#ifndef CONTROLLER_H_
+#define CONTROLLER_H_
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+
+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 getIntegralLeft();
+    float getIntegralRight();
+    float getProportionalLeft();
+    float getProportionalRight();
+
+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 KI;
+    static const float I_MAX;    
+    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;
+    float              iSumLeft;
+    float              iSumRight;
+    Ticker             ticker;
+    
+    void               run();
+
+};
+
+#endif /* CONTROLLER_H_ */
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,170 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes 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_0) && (b == PA_1)) {
+        
+        // pinmap OK for TIM2 CH1 and CH2
+        
+        TIM = TIM2;
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER0;     // reset port A0
+        GPIOA->MODER |= GPIO_MODER_MODER0_1;    // set alternate mode of port A0
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR0;     // reset pull-up/pull-down on port A0
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*0);         // reset alternate function of port A0
+        GPIOA->AFR[0] |= 1 << 4*0;              // set alternate funtion 1 of port A0
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER1;     // reset port A1
+        GPIOA->MODER |= GPIO_MODER_MODER1_1;    // set alternate mode of port A1
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR1;     // reset pull-up/pull-down on port A1
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR1_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*1);         // reset alternate function of port A1
+        GPIOA->AFR[0] |= 1 << 4*1;              // set alternate funtion 1 of port A1
+        
+        // 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 == PA_6) && (b == PC_7)) {
+        
+        // pinmap OK for TIM3 CH1 and CH2
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        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)) {
+        
+        // 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 not found for peripheral\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
+}
+
+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	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,35 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2018, 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_ */
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Fahren.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,214 @@
+#include "Fahren.h"
+#include <mbed.h>
+
+
+//Konstruktor
+Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight):
+    controller(controller), counterLeft(counterLeft), counterRight(counterRight)
+    
+{}
+//Dekonstruktor
+Fahren::~Fahren() {}
+
+
+
+//Implementation der Methode geradeaus fahren
+void Fahren::geradeaus(){
+    
+    //einlesen des aktuellen Encoder standes
+    initialClicksRight = counterRight.read();   
+    initialClicksLeft = counterLeft.read();
+    
+    //Anzahl Clicks die der Encoder zählen soll
+    wegRechts = 1767;
+    wegLinks = 1767;
+        
+    //Geschwindigkeit
+    speedRight = 50;
+    speedLeft = 50;
+    
+    //Zustand, dass der Roboter nicht gestoppt hat
+    stopRight = false;
+    stopLeft = false;
+    
+    //Drehrichtung der Motoren
+    controller.setDesiredSpeedRight(speedRight);
+    controller.setDesiredSpeedLeft(-(speedLeft));
+    
+    
+    while(1){
+       
+       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
+       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
+        
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
+            controller.setDesiredSpeedRight(0);
+            stopRight = true;
+            }
+            
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
+        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+            controller.setDesiredSpeedLeft(0);
+            stopLeft = true;
+            }
+     
+        if(stopRight == true && stopLeft == true){
+            return;
+            }  
+         
+        }
+        
+    }
+    
+    
+    
+//Implementation der Methode 90 Grad Rechtsdrehen
+void Fahren::rechts90() {
+        
+    //einlesen des aktuellen Encoder standes
+    initialClicksRight = counterRight.read();   
+    initialClicksLeft = counterLeft.read();
+    
+    //Anzahl Clicks die der Encoder zählen soll
+    wegRechts = 881;
+    wegLinks = 881;
+    
+    //Geschwindigkeit
+    speedRight = 50;
+    speedLeft = 50;
+    
+    //Zustand, dass der Roboter nicht gestoppt hat
+    stopRight = false;
+    stopLeft = false;
+    
+    //Drehrichtung der Motoren
+    controller.setDesiredSpeedRight((speedRight));
+    controller.setDesiredSpeedLeft((speedLeft));
+    
+    while(1){
+       
+       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
+       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
+        
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
+            controller.setDesiredSpeedRight(0);
+            stopRight = true;
+            }
+            
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
+        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+            controller.setDesiredSpeedLeft(0);
+            stopLeft = true;
+            }
+     
+        if(stopRight == true && stopLeft == true){
+            return;
+            }  
+         
+        }
+        
+    }
+    
+    
+    
+//Implementation der Methode 180 Grad Rechtsdrehen
+void Fahren::rechts180() {
+        
+    //einlesen des aktuellen Encoder standes
+    initialClicksRight = counterRight.read();   
+    initialClicksLeft = counterLeft.read();
+    
+    //Anzahl Clicks die der Encoder zählen soll
+    wegRechts = 1755;
+    wegLinks = 1755;
+    
+    //Geschwindigkeit
+    speedRight = 50;
+    speedLeft = 50;
+    
+    //Zustand, dass der Roboter nicht gestoppt hat
+    stopRight = false;
+    stopLeft = false;
+    
+    //Drehrichtung der Motoren
+    controller.setDesiredSpeedRight((speedRight));
+    controller.setDesiredSpeedLeft((speedLeft));
+    
+    while(1){
+       
+       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
+       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
+        
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
+            controller.setDesiredSpeedRight(0);
+            stopRight = true;
+            }
+            
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
+        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+            controller.setDesiredSpeedLeft(0);
+            stopLeft = true;
+            }
+     
+        if(stopRight == true && stopLeft == true){
+            return;
+            }  
+         
+        }
+        
+    }
+    
+
+//Implementation der Methode 180 Grad Rechtsdrehen
+void Fahren::rechts270() {
+        
+    //einlesen des aktuellen Encoder standes
+    initialClicksRight = counterRight.read();   
+    initialClicksLeft = counterLeft.read();
+    
+    //Anzahl Clicks die der Encoder zählen soll
+    wegRechts = 2632;
+    wegLinks = 2632;
+    
+    //Geschwindigkeit
+    speedRight = 50;
+    speedLeft = 50;
+    
+    //Zustand, dass der Roboter nicht gestoppt hat
+    stopRight = false;
+    stopLeft = false;
+    
+    //Drehrichtung der Motoren
+    controller.setDesiredSpeedRight((speedRight));
+    controller.setDesiredSpeedLeft((speedLeft));
+    
+    while(1){
+       
+       // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
+       // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
+        
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
+            controller.setDesiredSpeedRight(0);
+            stopRight = true;
+            }
+            
+        //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
+        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+            controller.setDesiredSpeedLeft(0);
+            stopLeft = true;
+            }
+     
+        if(stopRight == true && stopLeft == true){
+            return;
+            }  
+         
+        }
+        
+    }
+    
+    
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Fahren.h	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,38 @@
+#ifndef FAHREN_H_
+#define FAHREN_H_
+
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "Controller.h"
+
+class Fahren{
+    
+    public:    
+    Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight); //Konstruktor
+    
+    virtual ~Fahren();
+    
+    void geradeaus();
+    void rechts90();
+    void rechts180();
+    void rechts270();                                              
+    
+    
+    private:
+    Controller& controller;
+    EncoderCounter& counterLeft;
+    EncoderCounter& counterRight;
+    
+    //Variablen die in der Klasse Fahren verwendet werden
+    double speedRight;
+    double speedLeft;
+    short initialClicksLeft;
+    short initialClicksRight;
+    short wegLinks;
+    short wegRechts;
+    short stopRight;
+    short stopLeft;
+    
+
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,88 @@
+
+//Implementation IR Sensoren
+// V04.18
+// V. Ahlers
+
+#include <cmath>
+#include "IRSensor.h"
+
+using namespace std;
+
+    const float IRSensor :: PR1 = 3.4734*0.000000001;    //Koeffizienten
+    const float IRSensor :: PR2 = -7.1846*0.000001;
+    const float IRSensor :: PR3 = 0.0055;
+    const float IRSensor :: PR4 = -1.9304;
+    const float IRSensor :: PR5 = 301.2428;
+    const float IRSensor :: PL1 = 3.4734*0.000000001;
+    const float IRSensor :: PL2 = -7.1846*0.000001;
+    const float IRSensor :: PL3 = 0.0055;
+    const float IRSensor :: PL4 = -1.9304;
+    const float IRSensor :: PL5 = 301.2428;
+    const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10);
+    const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6);
+    const float IRSensor :: PF3 = 0.0024f;
+    const float IRSensor :: PF4 = -1.3299f;
+    const float IRSensor :: PF5 = 351.1557f;
+    const int IRSensor :: minIrR = 100;   //Noch definieren
+    const int IRSensor :: minIrL = 100;
+    const int IRSensor :: minIrF = 100;
+
+
+
+// IR Sensor Distanz in mm
+IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F) : 
+IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F){}
+                     
+IRSensor::~IRSensor(){}
+
+float IRSensor::readR() {
+    
+        measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        measR = measR * 1000; // Change the value to be in the 0 to 1000 range
+        disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR)
+        
+        return disR;    
+}
+
+float IRSensor::readL(){
+    
+        measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        measL = measL * 1000; // Change the value to be in the 0 to 1000 range
+        disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL)
+        
+        return disL;    
+}
+
+float IRSensor::readF(){
+    
+        measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        measF = measF * 1000; // Change the value to be in the 0 to 1000 range
+        disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF)
+        
+        return disF;    
+}
+
+// IR Sensor Zusatand
+int IRSensor::codeR(){
+    
+        if(disR < minIrR) {
+            IrR = 1;
+            } else { IrR = 0; }
+        return IrR;
+}
+
+int IRSensor ::codeL(){
+    
+        if(disL < minIrL) {
+            IrL = 1;
+            } else { IrL = 0; }
+        return IrL;     
+}
+
+int IRSensor ::codeF(){
+    
+        if(disF < minIrR) {
+            IrF = 1;
+            } else { IrF = 0; }
+        return IrF;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.h	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,64 @@
+// Deklaration IR Sensoren
+// V04.18
+// V. Ahlers
+
+
+#ifndef IRSENSOR_H_
+#define IRSENSOR_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+ class IRSensor {
+    
+    public:
+        IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F); 
+        
+        float disR;
+        float disL;
+        float disF;
+        int IrR;
+        int IrL;
+        int IrF;
+
+        float measR;
+        float measL;
+        float measF;
+
+        virtual ~IRSensor();
+        float readR();
+        float readL();
+        float readF();
+        int codeR();
+        int codeL();
+        int codeF();
+        
+        private:
+        AnalogIn& IrRight;
+        AnalogIn& IrLeft;
+        AnalogIn& IrFront;
+        float dis2R;
+        float dis2L;
+        float dis2F;
+        static const float PR1;
+        static const float PR2;
+        static const float PR3;
+        static const float PR4;
+        static const float PR5;
+        static const float PL1;
+        static const float PL2;
+        static const float PL3;
+        static const float PL4;
+        static const float PL5;
+        static const float PF1;
+        static const float PF2;
+        static const float PF3;
+        static const float PF4;
+        static const float PF5;
+        static const int minIrR;
+        static const int minIrL;
+        static const int minIrF;
+        
+};   
+
+#endif /*IRSensor_H_*/

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,113 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2018, 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 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 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	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,40 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2018, 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_ */
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,37 @@
+
+// Statemachine
+//V04.18
+// V. Ahlers
+
+#include <mbed.h>
+#include "StateMachine.h"
+
+using namespace std;
+
+StateMachine::StateMachine(int IrR, int IrL, int IrF) : IrR(IrR), IrL(IrL), IrF(IrF) {}
+
+StateMachine::~StateMachine (){}
+
+int StateMachine :: drive() {
+    
+           
+    
+    if((IrR==0) && (IrL==0) && (IrF==1)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
+    }else  if ((IrR==0) && (IrL==1) && (IrF==0)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
+    }else  if ((IrR==0) && (IrL==1) && (IrF==1)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen                 
+    }else  if ((IrR==1) && (IrL==0) && (IrF==0)){
+        caseDrive = 1;                                                          // Folge: geradeaus fahren
+    }else  if ((IrR==1) && (IrL==0) && (IrF==1)){
+        caseDrive = 3;                                                          // Folge: 270 Grad nach rechts drehen
+    }else  if ((IrR==1) && (IrL==1) && (IrF==0)){
+        caseDrive = 1;                                                          // Folge: geradeaus fahren
+    }else  if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 4;                                                          // Folge: 180 Grad nach rechts drehen
+    }else{ caseDrive=0;
+    }
+
+    return caseDrive;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.h	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,30 @@
+// Deklaration StateMachine
+// V04.18
+// V. Ahlers
+
+
+#ifndef STATEMACHINE_H_
+#define STATEMACHINE_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+class StateMachine {
+    
+    public:
+        StateMachine (int IrR, int IrL, int IrF);
+        
+        int IrR;
+        int IrL;
+        int IrF;
+        int caseDrive;
+        
+        virtual ~StateMachine();
+        int drive();
+        
+    private:
+    
+
+};   
+
+#endif /*StateMachine_H_*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,150 @@
+
+#include <mbed.h>
+
+#include "EncoderCounter.h"
+#include "Controller.h"
+#include "IRSensor.h"
+#include "Fahren.h"
+#include "StateMachine.h"
+
+DigitalOut myled(LED1);
+
+DigitalOut led0(PC_8);
+DigitalOut led1(PC_6);
+DigitalOut led2(PB_12);
+DigitalOut led3(PA_7);
+DigitalOut led4(PC_0);
+DigitalOut led5(PC_9);
+
+AnalogIn distance(PB_1);
+DigitalOut enable(PC_1);
+DigitalOut bit0(PH_1);
+DigitalOut bit1(PC_2);
+DigitalOut bit2(PC_3);
+
+/*IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
+IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
+IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
+IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
+IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
+IRSensor irSensor5(distance, bit0, bit1, bit2, 5);*/
+
+AnalogIn IrRight(PC_3);                                                         //von main Vincent kopiert
+AnalogIn IrLeft (PC_5);
+AnalogIn IrFront(PC_2);
+float disR = 0;
+float disL = 0;
+float disF = 0;
+
+float dis2R = 0;
+float dis2L = 0;
+float dis2F = 0;
+int IrR = 0;
+int IrL = 0;
+int IrF = 0;
+int caseDrive = 0;                                                              //von main Vincent kopiert
+
+IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F);               //von main Vincent kopiert
+StateMachine stateMachine(IrR, IrL, IrF);                                       //von main Vincent kopiert
+
+
+DigitalOut enableMotorDriver(PB_2);
+DigitalIn motorDriverFault(PB_14);
+DigitalIn motorDriverWarning(PB_15);
+
+PwmOut pwmLeft(PA_8);
+PwmOut pwmRight(PA_9);
+
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
+
+Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+
+Fahren fahren(controller, counterLeft, counterRight);                           //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
+
+int main()
+{
+
+    //controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm]              //DEFAULT VON TOBI
+    //controller.setDesiredSpeedRight(-150.0f);                                 //DEFAULT VON TOBI
+    enable = 1;
+    enableMotorDriver = 1;                                                      // Schaltet den Leistungstreiber ein
+
+    while(1) {
+
+// Test um Drehungen und geradeaus zu testen
+
+        /*fahren.geradeaus();                                                     //Geradeausfahren aufrufen
+        wait(1.0f);
+        
+        fahren.rechts90();
+        wait(1.0f);
+                
+        fahren.rechts180();
+        wait(1.0f);
+        
+        fahren.rechts270();
+        wait(1.0f);*/
+        
+                                                                                // IR Sensoren einlesen Programm Vincent
+        
+                    
+        float disR = iRSensor.readR(); // Distanz in mm
+        float disL = iRSensor.readL();
+        float disF = iRSensor.readF();
+        dis2R = disR;
+        dis2L = disL;
+        dis2F = disF;
+        int IrR = iRSensor.codeR();     // min Distanz unterschritten = 1
+        int IrL = iRSensor.codeL();
+        int IrF = iRSensor.codeF();
+        int caseDrive = stateMachine.drive(); //entscheidung welcher Drive Case
+        
+        
+        printf ("IR Right = %d \n", IrR);
+        printf("IR Left = %d\n",IrL);
+        printf("IR Front = %d\n",IrF);
+        
+        if((IrR==0) && (IrL==0) && (IrF==1)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
+        }   else  if ((IrR==0) && (IrL==1) && (IrF==0)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen
+        }   else  if ((IrR==0) && (IrL==1) && (IrF==1)){
+        caseDrive = 2;                                                          // Folge: 90 Grad nach rechts drehen                 
+        }   else  if ((IrR==1) && (IrL==0) && (IrF==0)){
+        caseDrive = 1;                                                          // Folge: geradeaus fahren
+        }   else  if ((IrR==1) && (IrL==0) && (IrF==1)){
+        caseDrive = 3;                                                          // Folge: 270 Grad nach rechts drehen
+        }   else  if ((IrR==1) && (IrL==1) && (IrF==0)){
+        caseDrive = 1;                                                          // Folge: geradeaus fahren
+        }   else  if ((IrR==1) && (IrL==1) && (IrF==1)){
+        caseDrive = 4;                                                          // Folge: 180 Grad nach rechts drehen
+        }   else{ caseDrive=0;
+        }
+        
+        printf("caseDrive = %d\n",caseDrive);
+
+        wait (1.0);
+        
+        /*switch (caseDrive){
+            caseDrive '2': 
+                buttonNow = button->read();
+                if (buttonNow && !buttonBefore){*/
+        
+        
+        if(caseDrive == 1){
+            fahren.geradeaus();
+        } else if (caseDrive == 2){
+            fahren.rechts90();
+        } else if (caseDrive == 3){
+            fahren.rechts270();
+        } else if (caseDrive == 4){
+            fahren.rechts180();
+        }
+                
+            
+    }
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 23 11:28:11 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file