Revision 18:6547e54ac803, committed 2017-05-09

Comitter:

itslinear

Date:

Tue May 09 13:21:49 2017 +0000

Parent:

17:caf5ae550f2e

Commit message:

f?r holdi;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter/EncoderCounter.cpp	Tue May 09 13:21:49 2017 +0000
@@ -0,0 +1,139 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2016, 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_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/EncoderCounter.h	Tue May 09 13:21:49 2017 +0000
@@ -0,0 +1,33 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2016, 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/EncoderCounter/LowpassFilter.cpp	Tue May 09 13:21:49 2017 +0000
@@ -0,0 +1,110 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2016, ZHAW
+ * All rights reserved.
+ */
+
+#include "LowpassFilter.h"
+
+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/EncoderCounter/LowpassFilter.h	Tue May 09 13:21:49 2017 +0000
@@ -0,0 +1,39 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2016, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef LOWPASS_FILTER_H_
+#define LOWPASS_FILTER_H_
+
+#include <cstdlib>
+#include <cmath>
+
+/**
+ * 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 /* LOWPASS_FILTER_H_ */

--- a/Roboter.cpp	Tue Apr 25 12:26:04 2017 +0000
+++ b/Roboter.cpp	Tue May 09 13:21:49 2017 +0000
@@ -1,7 +1,7 @@
 #include "Roboter.h"
 
 
-Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2)
+Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver)
 
 {
 
@@ -14,65 +14,242 @@
 
     this->pwmR = pwmR;
     this->pwmL = pwmL;
-    this->s1 = s1;
-    this->s2 = s2;
+    this->servo1 = servo1;
+    this->servo2 = servo2;
+    this->counterLeft = counterLeft;
+    this->counterRight = counterRight;
+    this->enableMotorDriver = enableMotorDriver;
 
+    initSpeedcontrol();
+
+   // t1.attach(this, &Roboter::speedCtrl, PERIOD);
 
 }
 
-float Roboter::readSensor1()
+float Roboter::readSensor1()            // Sensoren auslesen
 {
     return sensors[0].read();
 
 }
 
 
-void Roboter::bandeAusweichen()
+void Roboter::bandeAusweichen()         // Hindernissen ausweichen
 {
-    float offsetDir;
-    float offsetLin;
-
     float x=0.13f;       // Distanz ab welcher sensoren reagieren sollen
 
-    offsetDir = 0;
-    offsetLin = 0;
-
     while(sensors[0] < x && sensors[1] < x && sensors[5] < x) {  // alle sensoren aktiv, roboter fährt nach hinten
-        offsetLin = 0.1f;
-        *pwmL = 0.5f+offsetDir-offsetLin; // Setzt die Duty-Cycle auf 50%
-        *pwmR = 0.5f+offsetDir+offsetLin;
+        desiredSpeedLeft = -v;
+        desiredSpeedRight = v;
 
     }
 
-    while(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
-        offsetDir = -0.05;
-        *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
-        *pwmR = 0.5f+offsetDir-offsetLin;
+    while(sensors[0] < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
+        desiredSpeedLeft = -v;
+        desiredSpeedRight = -v;
 
     }
 
     while(sensors[1] < x) {     // sensor rechts, roboter dreht nach links
-        offsetDir = -0.05;
-        *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
-        *pwmR = 0.5f+offsetDir-offsetLin;
+        desiredSpeedLeft = -v;
+        desiredSpeedRight = -v;
 
     }
 
     while(sensors[5] < x && sensors[1]>(x+0.02f)) {     // sensor links, roboter dreht nach rechts
-        offsetDir = 0.08;
-        *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
-        *pwmR = 0.5f+offsetDir-offsetLin;
+        desiredSpeedLeft = v;
+        desiredSpeedRight = v;
 
     }
 
 }
 
 
+int Roboter::pickup()         // Klotz aufnehmen
+{
+    static int i = 0;
+    int r;      // Rückegabewert
+    desiredSpeedLeft = 0;
+    desiredSpeedRight = 0;
 
+    // while(i < 260) {
+    if(i < 70) {       // Arm nach unten fahren
+        servo2->write(0.96f);
+    }
+    if(i > 69 && i < 90) {      // gerade fahren
+        desiredSpeedLeft = v;
+        desiredSpeedRight = -v;
+    }
+    if(i > 89 && i < 140) {     // gerade fahren + Greifer schliessen
+        desiredSpeedLeft = v;
+        desiredSpeedRight = -v;
+        servo1->write(0.0f);
+    }
+    if(i > 139 && i < 210) {    // Arm nach oben fahren
+        desiredSpeedLeft = 0;
+        desiredSpeedRight = 0;
+        servo2->write(0.0f);
+    }
+    if(i > 209 && i < 260) {    // Greifer öffnen
+        servo1->write(0.5f);
+    }
+    i++;
+    //wait(0.01f);
+    // }
+    r = 5;                      // Rückegabewert
+
+    if( i > 260 )
+        r = 0;
+
+    return r;
+}
 
-void Roboter::pickup ()
+void Roboter::anfahren(float cam)
+{
+    float speedValue;
+    float maxWert = 20;
+    if(cam == 100) {
+        desiredSpeedLeft = -v/2;
+        desiredSpeedRight = -v/2;
+    }
+    if(cam == 200) {
+        desiredSpeedLeft = v/2;
+        desiredSpeedRight = v/2;
+    }
+    if(cam > 100 && cam <200 ) {                  // links fahren, wenn wir Werte von 100 bis 199 haben
+        cam = cam -99.0f;                           // cam nimmt die Werte von 1 bis 100 an
+        speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+        desiredSpeedLeft = -speedValue;
+        desiredSpeedRight = -speedValue;
+    }
+    if(cam > 200 && cam < 300) {                   // rechts fahren, wenn wir Werte von 200 bis 299 haben
+        cam = cam -199.0f;                          // cam nimmt die Werte von 1 bis 100 an
+        speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+        desiredSpeedLeft = speedValue;
+        desiredSpeedRight = speedValue;
+    }
+    if(cam >= 300 && cam <400) {                   // gerade fahren, wenn wir Werte von 300 bis 399 haben
+        cam = cam-299.0f;                           // cam nimmt die Werte von 1 bis 100 an
+        speedValue = v*1.5f + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+        desiredSpeedLeft = speedValue;
+        desiredSpeedRight = -speedValue;
+    }
+}
+
+int Roboter::geradeFahren()
+{
+    int r;
+    static int time2 = 0;
+    if(time2 < 200) {    // 1s gerade aus fahren
+        desiredSpeedLeft = v*3;
+        desiredSpeedRight = -v*3;
+        time2 ++;
+        r = 1;       // state
+        wait(0.01f);
+    } else {
+        time2 = 0;
+        desiredSpeedLeft = 0.0f;
+        desiredSpeedRight = 0.0f;
+        r = 2;       //state
+    }
+    return r;
+}
+
+int Roboter::kreisFahren()
 {
+    int r;
+    static int time1 = 0;
+    if(time1 < 310) {       // 1s im Kreis drehen
+        desiredSpeedLeft = v*1.7f;
+        desiredSpeedRight = v*1.7f;
+        time1 ++;
+        r = 1;          // state
+        wait(0.01f);
+    } else {
+        time1 = 0;
+        desiredSpeedLeft = 0.0f;
+        desiredSpeedRight = 0.0f;
+        r = 3;          // state
+    }
+    return r;
+}
 
+int Roboter::stateAuswahl(float cam, int temp)
+{
+    int s;
+    if(cam >= 100.0f && cam < 400.0f ) {           // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
+        s = 4;
+    }
+    if(cam == 400.0f) {     // Roboter in Position
+        s = 5;
+    }
+    if(cam == 0.0f) {
+        s = temp;
+    }
+    return s;
+
+    /*int s;
+    if(cam == 1 || cam == 2 || cam == 3) {           // Die Kamera erkennt ein grünen Klotz
+        s = 4;
+    }
+    if(cam == 4) {      // Roboter in Position
+        s = 5;
+    }
+    if(cam == 0) {      // kein Klotz in Sicht
+        s = temp;
+    }
+    return s;*/
+}
 
+void Roboter::initSpeedcontrol()
+{
+    // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmR->period(0.00005f);
+    *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
+    *pwmR = 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;
+
+    *enableMotorDriver = 1;
 }
+
+void Roboter::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*correction-actualSpeedRight)+desiredSpeedRight*correction/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;
+
+    *pwmL = 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;
+
+    *pwmR = dutyCycleRight;
+}

--- a/Roboter.h	Tue Apr 25 12:26:04 2017 +0000
+++ b/Roboter.h	Tue May 09 13:21:49 2017 +0000
@@ -5,37 +5,72 @@
 #include "IRSensor.h"
 #include "Servo.h"
 #include "readCamera.h"
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+
 
 class Roboter
 {
 
 public:
 
-    Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2,   PwmOut * pwmL, PwmOut* pwmR, Servo* s1, Servo* s2);
+    Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver);
 
     void bandeAusweichen();
     float readSensor1();
-    void pickup();
+    int pickup();
+    void anfahren(float cam);
+    int geradeFahren();
+    int kreisFahren();
+    int stateAuswahl(float cam, int temp);
+    void initSpeedcontrol();
+    void speedCtrl();
 
 
 
+    static const float PERIOD = 0.001f;                    // period of control task, given in [s]
+    static const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
+    static const float LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
+    static const float KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
+    static const float KP = 0.2f;                          // speed controller gain, given in [V/rpm]
+    static const float MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
+    static const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
+    static const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
+    static const float correction = 0.97f;                 // Korrekturfaktor für speedControl
+    static const float v = 20.0f;                          // Min. Geschw.
+
+    
 private:
+
     IRSensor sensors[6];
 
-
     PwmOut* pwmL;
     PwmOut* pwmR;
-    Servo*s1;
-    Servo*s2;
-    DigitalIn * switch1;
-    DigitalIn* switch2; 
+    Servo* servo1;
+    Servo* servo2;
+
+//speed control------------------
+
+    EncoderCounter* counterRight;
+    EncoderCounter* counterLeft;
+    DigitalOut* enableMotorDriver;
+
+    short previousValueCounterRight;
+    short previousValueCounterLeft;
 
+    float desiredSpeedLeft;
+    float desiredSpeedRight;
 
+    float actualSpeedLeft;
+    float actualSpeedRight;
 
+    LowpassFilter speedLeftFilter;
+    LowpassFilter speedRightFilter;
 
+    Ticker t1;
+
+//speed control------------------
 
 };
 
-
-
 #endif
\ No newline at end of file

--- a/Servo.lib	Tue Apr 25 12:26:04 2017 +0000
+++ b/Servo.lib	Tue May 09 13:21:49 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/simon/code/Servo/#36b69a7ced07
+https://developer.mbed.org/teams/Gruppe-3/code/Servo/#c29d0c32fe0a

--- a/main.cpp	Tue Apr 25 12:26:04 2017 +0000
+++ b/main.cpp	Tue May 09 13:21:49 2017 +0000
@@ -1,7 +1,11 @@
 #include <mbed.h>
 #include "Roboter.h"
 
-DigitalOut led(LED1);
+
+//Periphery for Encoder
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
+
 
 //Periphery for distance sensors
 AnalogIn distance(PB_1);
@@ -12,11 +16,10 @@
 IRSensor sensors[6];
 
 // Periphery for servos
-Servo servos1(PB_7);
-Servo servos2(PA_6);
+Servo servo1(PB_13);    //Greiferservo Anschluss D2
+Servo servo2(PA_10);    //Armservo Anschluss D0
 
 //Periphery for Switch ON/OFF
-
 DigitalIn switchOnOff(USER_BUTTON);
 
 //motor stuff
@@ -27,124 +30,74 @@
 //indicator leds arround robot
 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
 
-Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2);
+// Erstellen eines Roboterobjekts
+Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servo1, &servo2, &counterLeft, &counterRight, &enableMotorDriver);
 
 int main()
 {
-    enable = 1;  // Sensoren einschalten
-    enableMotorDriver = 1; // Schaltet die Motoren ein
-    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
-    pwmR.period(0.00005f);
-    pwmL = 0.5f;
-    pwmR = 0.5f;
-
-
-    int state = 0; // Diese Variable gibt an in welchem State man sich befindet
-    int tempState = 2;
-    int time1 = 0;
-    int time2 = 0;
+    int state = 0;         // Diese Variable gibt an in welchem State man sich befindet
+    int tempState = 2;     // Wird benötig um zwischen Kameraauswertung und fahren zu wechseln
+    enable = 1;            // Sensoren einschalten
+    float camValue;
+    servo1 = 0.0f;    // Startpos. Greifer
+    servo2 = 0.0f;    // Startpos. Arm
+    //wait(1);
 
     while(1) {
 
-        int camValue = readCamera();
+/*
+
+        //printf("%d\n", state);
 
         switch(state) {
 
             case 0: // Roboter Anschalten mit Knopf
                 printf("0");
 
-                if (switchOnOff == 0) {
-                    state = 1;
+                if (switchOnOff == 0) {     // Bei Betätigung des Userbuttons wird das Programm gestartet
+                    state = 5;
                 }
 
                 break;
 
             case 1:
-                printf("1");
-                if(camValue == 1 || camValue == 2 || camValue == 3) {           // Die Kamera erkennt ein grünen Klotz
-                    state = 4;
-                }
-                if(camValue == 4) {     // Roboter in Position
-                    state = 5;
-                }
-                if(camValue == 0){
-                    state = tempState;
-                }
+                //printf("1");
+                camValue = readCamera();
+                state = roboter1.stateAuswahl(camValue, tempState); // Case-Auswahl aufgrund von der Kamera
                 break;
 
             case 2:
-                printf("2");
-                if(time1 < 20) {       // Im Kreis drehen für 1s
-                    pwmL = 0.4f;
-                    pwmR = 0.4f;
-                    time1 ++;
-                    state = 1;
-                    tempState = 2;
-                } else {
-                    time1 = 0;
-                    pwmL = 0.5f;
-                    pwmR = 0.5f;
-                    state = 3;
-                }
+                //printf("2");
+                state = roboter1.kreisFahren(); // Im Kreis fahren
+                tempState = 2;
                 break;
 
 
             case 3:
-                printf("3");
-                roboter1.bandeAusweichen();
-                if(time2 < 40) {    // gerade aus fahren
-                    pwmL = 0.6f;
-                    pwmR = 0.4f;
-                    time2 ++;
-                    state = 1;
-                    tempState = 3;
-                } else {
-                    time2 = 0;
-                    pwmL = 0.5f;
-                    pwmR = 0.5f;
-                    state = 2;
-                }
+                //printf("3");
+                roboter1.bandeAusweichen();  // Hindernissen ausweichen
+                state = roboter1.geradeFahren(); // Gerade Fahren
+                tempState = 3;
                 break;
 
 
-            case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
-                printf("4");
-                roboter1.bandeAusweichen();
-                if(camValue == 1) {     // links fahren
-                    pwmL = 0.45f;
-                    pwmR = 0.45f;
-                }
-                if(camValue == 2) {     // rechts fahren
-                    pwmL = 0.55f;
-                    pwmR = 0.55f;
-                }
-                if(camValue == 3) {     // gerade fahren
-                    pwmL = 0.55f;
-                    pwmR = 0.45f;
-                }
+            case 4:
+                //printf("4");
+                roboter1.bandeAusweichen();  // Hindernissen ausweichen
+                roboter1.anfahren(camValue); // Roboter erkennt gründer Klotz, Klotz wird angefahren
                 state = 1;
-
                 break;
 
-            case 5: // Aufnehmen des Klotzes
-                printf("5");
-                pwmL = 0.5f;
-                pwmR = 0.5f;
-                state = 1;
-                time1 = 0;
-                time2 = 0;
-
+            case 5:
+                //printf("5");
+                state = roboter1.pickup(); // Aufnehmen des Klotzes
                 break;
 
-
             default:
                 break;
 
-
         }
-        
-        //wait(0.1f);
+        */
+        wait(0.01f);
     }
-
 }
-

--- a/mbed.bld	Tue Apr 25 12:26:04 2017 +0000
+++ b/mbed.bld	Tue May 09 13:21:49 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file
+https://mbed.org/users/mbed_official/code/mbed/builds/794e51388b66
\ No newline at end of file

--- a/readCamera.cpp	Tue Apr 25 12:26:04 2017 +0000
+++ b/readCamera.cpp	Tue May 09 13:21:49 2017 +0000
@@ -1,68 +1,73 @@
 #include "readCamera.h"
 
-
 Pixy pixy(I2C_SDA, I2C_SCL);
 Serial pc(SERIAL_TX, SERIAL_RX);
 
 static int i = 0;
 static int k = 0;
-static int range = 1;           // 1: fährt strahl an, 2: fährt kegel an
-static int state = 0;
+static float distance = 0;        // für dynamische Geschwindigkeit
+static float yLimit = 165;        // Punkt an dem Roboter anhalten soll
 int j;
 uint16_t blocks;
 
+float state;
 
 int readCamera()
 {
     pixy.setSerialOutput(&pc);
+    blocks = pixy.getBlocks(1);
 
-    while (1) {
-        blocks = pixy.getBlocks(1);
+    if (blocks) {
+        i++;
+        if (i % 5 == 0) {                                               // Jedes 5. Bild wird ausgewertet
+            for (j = 0; j < blocks; j++) {
+                float xAxis = pixy.blocks[j].x;                         // Auswertung der x-Koordinate
+                float yAxis = pixy.blocks[j].y;                         // Auswertung der y-Koordinate
+                printf("%f y\n", yAxis);
+                //printf("%f x\n", xAxis);
+
+                if(yAxis < yLimit && yAxis > 30) {
+
+                    if (xAxis < 130) {                                  // links fahren
+                        distance = (129 - xAxis)*(99.0/129.0);
+                        state = 100 + distance;
+
+                    } else if (xAxis > 190) {                           // rechts fahren
+                        distance = (xAxis - 191)*(99.0/128.0);
+                        state = 200 + distance;
+
+                    } else {                                            // fahre gerade
+                        distance = ((yAxis - yLimit) / yLimit)*(-99);
+                        state = 300 + distance;
+                    }
+                }
 
-        if (blocks) {
-            i++;
-            if (i % 5 == 0) {
-                for (j = 0; j < blocks; j++) {
-                    int xAxis = pixy.blocks[j].x;
-                    int yAxis = pixy.blocks[j].y;
-                    
-                     
-        
-                    // soll drehen bis in stahl, fahren bis kegel, 25 pixel links rechts kegel
-                    if(xAxis < 185 && xAxis > 135  &&  yAxis > 155 && yAxis < 190 ) {              // wenn Klotz an Position zum aufnehmen
-                        state = 4;
-                        range = 1;
-                    } else {
-                        if (range == 1){         // fährt Strahl an
-                            if (xAxis < 155){
-                                state = 1;
-                            } else if (xAxis > 165){
-                                state = 2;
-                            } else{   // fährt gerade
-                                state = 3;
-                                range = 2;
-                            }
-                        } else {                // fährt solange bis Kegelrand
-                            if (xAxis > 185 || xAxis < 135) {
-                                range = 1;
-                            }
-                        }
+                if(yAxis > yLimit) {
+
+                    if (xAxis < 145) {                                  // links fahren
+                        state = 100;
+
+                    } else if (xAxis > 175) {                           // rechts fahren
+                        state = 200;
+
+                    } else {                                            // Klotz bereit zum Aufnehmen
+                        state = 400;
                     }
-                    
-                    
-                    
                 }
-                k=0;
+                if(yAxis <= 30) {                                       // Zone mit vielen Fehlern
+                    state = 0;                                          // deshalb wird alles, was dort drin ist als "kein Klotz in Sichtweite"" gewertet 
+                }
             }
-        } else{
-            k++;
-            if (k % 20 == 0) {
-                i=0;
-                state = 0;
-            }
+            k = 0;
+            i = 0;
         }
-        //printf("%d\n\r",state);
-        //printf("%d, %d\n\r", xAxis, yAxis);
-        return state;
+    } else {
+        k++;
+        if (k % 20 == 0) {
+            i=0;
+            state = 0;                                                  // kein Klotz in Sichtweite
+        }
     }
+    //printf("%f\n\r",state);
+    return state;
 }
\ No newline at end of file

--- a/readCamera.h	Tue Apr 25 12:26:04 2017 +0000
+++ b/readCamera.h	Tue May 09 13:21:49 2017 +0000
@@ -6,11 +6,9 @@
 #include "PixyLink.h"
 #include "PixyLinkI2C.h"
 
+
 int readCamera();
 
 
 
-
-
-
 #endif 
\ No newline at end of file