Revision 1:08ca9b208045, committed 2017-03-31

Comitter:

solcager

Date:

Fri Mar 31 11:00:19 2017 +0000

Parent:

0:646b6cf24af2

Commit message:

P3

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,239 @@
+/*
+ * Controller.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f;                    // period of control task, given in [s]
+const float Controller::PI = 3.14159265f;                   // the constant PI
+const float Controller::WHEEL_DISTANCE = 0.170f;            // distance between wheels, given in [m]
+const float Controller::WHEEL_RADIUS = 0.0375f;             // radius of wheels, given in [m]
+const float Controller::COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s] 
+const float Controller::KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
+const float Controller::KP = 0.2f;                          // speed controller gain, given in [V/rpm]
+const float Controller::MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
+const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
+
+/**
+ * Creates and initializes a Controller object.
+ * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
+ * @param pwmRight a pwm output object to set the duty cycle for the right motor.
+ * @param counterLeft an encoder counter object to read the position of the left motor.
+ * @param counterRight an encoder counter object to read the position of the right motor.
+ */
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
+    
+    // initialize periphery drivers
+    
+    pwmLeft.period(0.00005f);
+    pwmLeft.write(0.5f);
+    
+    pwmRight.period(0.00005f);
+    pwmRight.write(0.5f);
+    
+    // initialize local variables
+    
+    translationalMotion.setProfileVelocity(0.8f);
+    translationalMotion.setProfileAcceleration(3.0f);
+    translationalMotion.setProfileDeceleration(3.0f);
+    
+    rotationalMotion.setProfileVelocity(6.0f);
+    rotationalMotion.setProfileAcceleration(6.0f);
+    rotationalMotion.setProfileDeceleration(6.0f);
+    
+    translationalVelocity = 0.0f;
+    rotationalVelocity = 0.0f;
+    actualTranslationalVelocity = 0.0f;
+    actualRotationalVelocity = 0.0f;
+    
+    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;
+    
+    x = 0.0f;
+    y = 0.0f;
+    alpha = 0.0f;
+    
+    // start periodic task
+    
+    ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+/**
+ * Deletes the Controller object and releases all allocated resources.
+ */
+Controller::~Controller() {
+    
+    ticker.detach();
+}
+
+/**
+ * Sets the desired translational velocity of the robot.
+ * @param velocity the desired translational velocity, given in [m/s].
+ */
+void Controller::setTranslationalVelocity(float velocity) {
+    
+    this->translationalVelocity = velocity;
+}
+
+/**
+ * Sets the desired rotational velocity of the robot.
+ * @param velocity the desired rotational velocity, given in [rad/s].
+ */
+void Controller::setRotationalVelocity(float velocity) {
+    
+    this->rotationalVelocity = velocity;
+}
+
+/**
+ * Gets the actual translational velocity of the robot.
+ * @return the actual translational velocity, given in [m/s].
+ */
+float Controller::getActualTranslationalVelocity() {
+    
+    return actualTranslationalVelocity;
+}
+
+/**
+ * Gets the actual rotational velocity of the robot.
+ * @return the actual rotational velocity, given in [rad/s].
+ */
+float Controller::getActualRotationalVelocity() {
+    
+    return actualRotationalVelocity;
+}
+
+/**
+ * Sets the actual x coordinate of the robots position.
+ * @param x the x coordinate of the position, given in [m].
+ */
+void Controller::setX(float x) {
+    
+    this->x = x;
+}
+
+/**
+ * Gets the actual x coordinate of the robots position.
+ * @return the x coordinate of the position, given in [m].
+ */
+float Controller::getX() {
+    
+    return x;
+}
+
+/**
+ * Sets the actual y coordinate of the robots position.
+ * @param y the y coordinate of the position, given in [m].
+ */
+void Controller::setY(float y) {
+    
+    this->y = y;
+}
+
+/**
+ * Gets the actual y coordinate of the robots position.
+ * @return the y coordinate of the position, given in [m].
+ */
+float Controller::getY() {
+    
+    return y;
+}
+
+/**
+ * Sets the actual orientation of the robot.
+ * @param alpha the orientation, given in [rad].
+ */
+void Controller::setAlpha(float alpha) {
+    
+    this->alpha = alpha;
+}
+
+/**
+ * Gets the actual orientation of the robot.
+ * @return the orientation, given in [rad].
+ */
+float Controller::getAlpha() {
+    
+    return alpha;
+}
+
+/**
+ * This method is called periodically by the ticker object and contains the algorithms
+ * of the motion planner, the coordinate transformation and the speed controller.
+ */
+void Controller::run() {
+    
+    // calculate the planned velocities using the motion planner
+    
+    translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
+    rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
+    
+    // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
+    
+    desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
+    desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
+    
+    // calculate actual speed of 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 motor phase voltages
+    
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+    
+    // calculate, limit and set duty cycles
+    
+    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.write(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.write(dutyCycleRight);
+    
+    // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
+    
+    actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
+    actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
+    
+    // calculate the actual robot pose
+    
+    x += cos(alpha+actualRotationalVelocity*PERIOD)*actualTranslationalVelocity*PERIOD;
+    y += sin(alpha+actualRotationalVelocity*PERIOD)*actualTranslationalVelocity*PERIOD;
+    alpha += actualRotationalVelocity*PERIOD;
+    
+    while (alpha > PI) alpha -= 2.0f*PI;
+    while (alpha < -PI) alpha += 2.0f*PI;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,78 @@
+/*
+ * Controller.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef CONTROLLER_H_
+#define CONTROLLER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "Motion.h"
+#include "LowpassFilter.h"
+
+/**
+ * This class implements the coordinate transformation, speed control and
+ * the position estimation of a mobile robot with differential drive.
+ */
+class Controller {
+
+    public:
+    
+                    Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight);
+        virtual     ~Controller();
+        void        setTranslationalVelocity(float velocity);
+        void        setRotationalVelocity(float velocity);
+        float       getActualTranslationalVelocity();
+        float       getActualRotationalVelocity();
+        void        setX(float x);
+        float       getX();
+        void        setY(float y);
+        float       getY();
+        void        setAlpha(float alpha);
+        float       getAlpha();
+        
+    private:
+        
+        static const float  PERIOD;
+        static const float  PI;
+        static const float  WHEEL_DISTANCE;
+        static const float  WHEEL_RADIUS;
+        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;
+        Motion              translationalMotion;
+        Motion              rotationalMotion;
+        float               translationalVelocity;
+        float               rotationalVelocity;
+        float               actualTranslationalVelocity;
+        float               actualRotationalVelocity;
+        short               previousValueCounterLeft;
+        short               previousValueCounterRight;
+        LowpassFilter       speedLeftFilter;
+        LowpassFilter       speedRightFilter;
+        float               desiredSpeedLeft;
+        float               desiredSpeedRight;
+        float               actualSpeedLeft;
+        float               actualSpeedRight;
+        float               x;
+        float               y;
+        float               alpha;
+        Ticker              ticker;
+        
+        void                run();
+};
+
+#endif /* CONTROLLER_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,140 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2017, 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.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,34 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2017, 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	Fri Mar 10 17:00:01 2017 +0000
+++ b/IRSensor.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -1,58 +1,54 @@
+/*
+ * IRSensor.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
 
 #include <cmath>
-#include <IRSensor.h>
+#include "IRSensor.h"
 
 using namespace std;
 
-IRSensor::IRSensor(AnalogIn& distance, DigitalOut& bit0,
-                   DigitalOut& bit1, DigitalOut& bit2, int number) :
-                   distance(distance), bit0(bit0), bit1(bit1), bit2(bit2) {
+/**
+ * Creates an IRSensor object.
+ * @param distance an analog input object to read the voltage of the sensor.
+ * @param bit0 a digital output to set the first bit of the multiplexer.
+ * @param bit1 a digital output to set the second bit of the multiplexer.
+ * @param bit2 a digital output to set the third bit of the multiplexer.
+ * @param number the number of the sensor, either 0, 1, 2, 3, 4 or 5.
+ */
+IRSensor::IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number) : distance(distance), bit0(bit0), bit1(bit1), bit2(bit2) {
+    
+    // set local references to objects
+    
     this->number = number;
-
 }
 
+/**
+ * Deletes the IRSensor object.
+ */
 IRSensor::~IRSensor() {}
 
+/**
+ * Gets the distance measured with the IR sensor in [m].
+ * @return the distance, given in [m].
+ */
 float IRSensor::read() {
-        
-        switch (number) {
-        case 0:     // vorne
-        bit0 = 0;
-        bit1 = 0;
-        bit2 = 0;
-        break;
-        
-        case 1:     // vorne rechts
-        bit0 = 1;
-        bit1 = 0;
-        bit2 = 0;
-        break;    
-        
-        case 2:     // hinten rechts
-        bit0 = 0;
-        bit1 = 1;
-        bit2 = 0;
-        break;    
-        
-        case 3:     // hinten
-        bit0 = 1;
-        bit1 = 1;
-        bit2 = 0;
-        break;    
-        
-        case 4:     // hinten links
-        bit0 = 0;
-        bit1 = 0;
-        bit2 = 1; 
-        break;
-        
-        case 5:     // vorne links
-        bit0 = 1;
-        bit1 = 0;
-        bit2 = 1;  
-        break;
-    }
+    
+    bit0 = (number >> 0) & 1;
+    bit1 = (number >> 1) & 1;
+    bit2 = (number >> 2) & 1;
+    
+    float d = -0.58f*sqrt(distance)+0.58f;  // calculate the distance in [m]
     
-    float d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
     return d;
-}
\ No newline at end of file
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+IRSensor::operator float() {
+    
+    return read();
+}
+

--- a/IRSensor.h	Fri Mar 10 17:00:01 2017 +0000
+++ b/IRSensor.h	Fri Mar 31 11:00:19 2017 +0000
@@ -1,18 +1,36 @@
+/*
+ * IRSensor.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
 #ifndef IR_SENSOR_H_
 #define IR_SENSOR_H_
+
 #include <cstdlib>
 #include <mbed.h>
+
+/**
+ * This is a device driver class to read the distance measured with a Sharp IR sensor.
+ */
 class IRSensor {
+    
     public:
-    IRSensor(AnalogIn& distance, DigitalOut& bit0,
-             DigitalOut& bit1, DigitalOut& bit2, int number);
-    virtual ~IRSensor();
-    float read();
+        
+                    IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number);
+        virtual     ~IRSensor();
+        float       read();
+                    operator float();
+        
     private:
-    AnalogIn& distance;
-    DigitalOut& bit0;
-    DigitalOut& bit1;
-    DigitalOut& bit2;
-    int number;
+        
+        AnalogIn&       distance;
+        DigitalOut&     bit0;
+        DigitalOut&     bit1;
+        DigitalOut&     bit2;
+        
+        int             number;
 };
-#endif /* IR_SENSOR_H_ */
\ No newline at end of file
+
+#endif /* IR_SENSOR_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,112 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2017, 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	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,39 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2017, 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/Main.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,76 @@
+/*
+ * Main.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include <cstdlib>
+#include <mbed.h>
+#include "IRSensor.h"
+#include "EncoderCounter.h"
+#include "Controller.h"
+#include "StateMachine.h"
+
+using namespace std;
+
+/**
+ * This is the main program of the mobile robot control software.
+ */
+int main() {
+    
+    // create and initialize miscellaneous periphery objects
+    
+    DigitalOut led(LED1);
+    DigitalIn button(USER_BUTTON);
+    
+    DigitalOut led0(PC_8);
+    DigitalOut led1(PC_6);
+    DigitalOut led2(PB_12);
+    DigitalOut led3(PA_7);
+    DigitalOut led4(PC_0);
+    DigitalOut led5(PC_9);
+    
+    // create distance sensor objects
+    
+    DigitalOut enableIRSensors(PC_1);
+    enableIRSensors = 1;
+    
+    AnalogIn distance(PB_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);
+    
+    // create motor control objects
+    
+    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);
+    
+    // create robot controller objects
+    
+    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+    StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
+    
+    // enter main loop
+    
+    while (true) {
+        
+        led = !led;
+        
+        wait((stateMachine.getState() != StateMachine::ROBOT_OFF) ? 0.1f : 0.2f);
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,602 @@
+/*
+ * Motion.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include <algorithm>
+#include "Motion.h"
+
+using namespace std;
+
+const float Motion::DEFAULT_LIMIT = 1.0f;       // default value for limits
+const float Motion::MINIMUM_LIMIT = 1.0e-9f;    // smallest value allowed for limits
+
+/**
+ * Creates a <code>Motion</code> object.
+ * The values for position, velocity and acceleration are set to 0.
+ */
+Motion::Motion() {
+    
+    position = 0.0;
+    velocity = 0.0f;
+    
+    profileVelocity = DEFAULT_LIMIT;
+    profileAcceleration = DEFAULT_LIMIT;
+    profileDeceleration = DEFAULT_LIMIT;
+}
+
+/**
+ * Creates a <code>Motion</code> object with given values for position and velocity.
+ * @param position the initial position value of this motion, given in [m] or [rad].
+ * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
+ */
+Motion::Motion(double position, float velocity) {
+    
+    this->position = position;
+    this->velocity = velocity;
+    
+    profileVelocity = DEFAULT_LIMIT;
+    profileAcceleration = DEFAULT_LIMIT;
+    profileDeceleration = DEFAULT_LIMIT;
+}
+
+/**
+ * Creates a <code>Motion</code> object with given values for position and velocity.
+ * @param motion another <code>Motion</code> object to copy the values from.
+ */
+Motion::Motion(const Motion& motion) {
+    
+    position = motion.position;
+    velocity = motion.velocity;
+    
+    profileVelocity = motion.profileVelocity;
+    profileAcceleration = motion.profileAcceleration;
+    profileDeceleration = motion.profileDeceleration;
+}
+
+/**
+ * Deletes the Motion object.
+ */
+Motion::~Motion() {}
+
+/**
+ * Sets the values for position and velocity.
+ * @param position the desired position value of this motion, given in [m] or [rad].
+ * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
+ */
+void Motion::set(double position, float velocity) {
+    
+    this->position = position;
+    this->velocity = velocity;
+}
+
+/**
+ * Sets the values for position and velocity.
+ * @param motion another <code>Motion</code> object to copy the values from.
+ */
+void Motion::set(const Motion& motion) {
+    
+    position = motion.position;
+    velocity = motion.velocity;
+}
+
+/**
+ * Sets the position value.
+ * @param position the desired position value of this motion, given in [m] or [rad].
+ */
+void Motion::setPosition(double position) {
+    
+    this->position = position;
+}
+
+/**
+ * Gets the position value.
+ * @return the position value of this motion, given in [m] or [rad].
+ */
+double Motion::getPosition() {
+    
+    return position;
+}
+
+/**
+ * Sets the velocity value.
+ * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
+ */
+void Motion::setVelocity(float velocity) {
+    
+    this->velocity = velocity;
+}
+
+/**
+ * Gets the velocity value.
+ * @return the velocity value of this motion, given in [m/s] or [rad/s].
+ */
+float Motion::getVelocity() {
+    
+    return velocity;
+}
+
+/**
+ * Sets the limit for the velocity value.
+ * @param profileVelocity the limit of the velocity.
+ */
+void Motion::setProfileVelocity(float profileVelocity) {
+    
+    if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
+}
+
+/**
+ * Sets the limit for the acceleration value.
+ * @param profileAcceleration the limit of the acceleration.
+ */
+void Motion::setProfileAcceleration(float profileAcceleration) {
+    
+    if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
+}
+
+/**
+ * Sets the limit for the deceleration value.
+ * @param profileDeceleration the limit of the deceleration.
+ */
+void Motion::setProfileDeceleration(float profileDeceleration) {
+    
+    if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
+}
+
+/**
+ * Sets the limits for velocity, acceleration and deceleration values.
+ * @param profileVelocity the limit of the velocity.
+ * @param profileAcceleration the limit of the acceleration.
+ * @param profileDeceleration the limit of the deceleration.
+ */
+void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) {
+    
+    if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
+    if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
+    if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
+}
+
+/**
+ * Gets the time needed to move to a given target position.
+ * @param targetPosition the desired target position given in [m] or [rad].
+ * @return the time to move to the target position, given in [s].
+ */
+float Motion::getTimeToPosition(double targetPosition) {
+    
+    // calculate position, when velocity is reduced to zero
+    
+    double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
+    
+    if (targetPosition > stopPosition) { // positive velocity required
+        
+        if (velocity > profileVelocity) { // slow down to profile velocity first
+            
+            float t1 = (velocity-profileVelocity)/profileDeceleration;
+            float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
+            float t3 = profileVelocity/profileDeceleration;
+            
+            return t1+t2+t3;
+            
+        } else if (velocity > 0.0f) { // speed up to profile velocity
+            
+            float t1 = (profileVelocity-velocity)/profileAcceleration;
+            float t3 = profileVelocity/profileDeceleration;
+            float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+            
+            if (t2 < 0.0f) {
+                float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+                t1 = (maxVelocity-velocity)/profileAcceleration;
+                t2 = 0.0f;
+                t3 = maxVelocity/profileDeceleration;
+            }
+            
+            return t1+t2+t3;
+            
+        } else { // slow down to zero first, and then speed up to profile velocity
+            
+            float t1 = -velocity/profileDeceleration;
+            float t2 = profileVelocity/profileAcceleration;
+            float t4 = profileVelocity/profileDeceleration;
+            float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+            
+            if (t3 < 0.0f) {
+                float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+                t2 = maxVelocity/profileAcceleration;
+                t3 = 0.0f;
+                t4 = maxVelocity/profileDeceleration;
+            }
+            
+            return t1+t2+t3+t4;
+        }
+        
+    } else { // negative velocity required
+        
+        if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
+            
+            float t1 = (-profileVelocity-velocity)/profileDeceleration;
+            float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
+            float t3 = profileVelocity/profileDeceleration;
+            
+            return t1+t2+t3;
+            
+        } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
+            
+            float t1 = (velocity+profileVelocity)/profileAcceleration;
+            float t3 = profileVelocity/profileDeceleration;
+            float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+            
+            if (t2 < 0.0f) {
+                float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+                t1 = (velocity-minVelocity)/profileAcceleration;
+                t2 = 0.0f;
+                t3 = -minVelocity/profileDeceleration;
+            }
+            
+            return t1+t2+t3;
+            
+        } else { // slow down to zero first, and then speed up to (negative) profile velocity
+            
+            float t1 = velocity/profileDeceleration;
+            float t2 = profileVelocity/profileAcceleration;
+            float t4 = profileVelocity/profileDeceleration;
+            float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+            
+            if (t3 < 0.0f) {
+                float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+                t2 = -minVelocity/profileAcceleration;
+                t3 = 0.0f;
+                t4 = -minVelocity/profileDeceleration;
+            }
+            
+            return t1+t2+t3+t4;
+        }
+    }
+}
+
+/**
+ * Increments the current motion towards a given target velocity.
+ * @param targetVelocity the desired target velocity given in [m/s] or [rad/s].
+ * @param period the time period to increment the motion values for, given in [s].
+ */
+void Motion::incrementToVelocity(float targetVelocity, float period) {
+    
+    if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
+    else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
+    
+    if (targetVelocity > 0.0f) {
+        
+        if (velocity > targetVelocity) { // slow down to target velocity
+            
+            float t1 = (velocity-targetVelocity)/profileDeceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+                velocity += -profileDeceleration*period;
+            } else {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)(velocity*(period-t1));
+            }
+            
+        } else if (velocity > 0.0f) { // speed up to target velocity
+            
+            float t1 = (targetVelocity-velocity)/profileAcceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity+profileAcceleration*0.5f*period)*period);
+                velocity += profileAcceleration*period;
+            } else {
+                position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+                velocity += profileAcceleration*t1;
+                position += (double)(velocity*(period-t1));
+            }
+            
+        } else { // slow down to zero first, and then speed up to target velocity
+            
+            float t1 = -velocity/profileDeceleration;
+            float t2 = targetVelocity/profileAcceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+                velocity += profileDeceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
+                velocity += profileAcceleration*(period-t1);
+            } else {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+                velocity += profileAcceleration*t2;
+                position += (double)(velocity*(period-t1-t2));
+            }
+        }
+        
+    } else {
+        
+        if (velocity < targetVelocity) { // slow down to (negative) target velocity
+            
+            float t1 = (targetVelocity-velocity)/profileDeceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+                velocity += profileDeceleration*period;
+            } else {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)(velocity*(period-t1));
+            }
+            
+        } else if (velocity < 0.0f) { // speed up to (negative) target velocity
+            
+            float t1 = (velocity-targetVelocity)/profileAcceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity-profileAcceleration*0.5f*period)*period);
+                velocity += -profileAcceleration*period;
+            } else {
+                position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+                velocity += -profileAcceleration*t1;
+                position += (double)(velocity*(period-t1));
+            }
+            
+        } else { // slow down to zero first, and then speed up to (negative) target velocity
+            
+            float t1 = velocity/profileDeceleration;
+            float t2 = -targetVelocity/profileAcceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+                velocity += -profileDeceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
+                velocity += -profileAcceleration*(period-t1);
+            } else {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+                velocity += -profileAcceleration*t2;
+                position += (double)(velocity*(period-t1-t2));
+            }
+        }
+    }
+}
+
+/**
+ * Increments the current motion towards a given target position.
+ * @param targetPosition the desired target position given in [m] or [rad].
+ * @param period the time period to increment the motion values for, given in [s].
+ */
+void Motion::incrementToPosition(double targetPosition, float period) {
+    
+    // calculate position, when velocity is reduced to zero
+    
+    double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
+    
+    if (targetPosition > stopPosition) { // positive velocity required
+        
+        if (velocity > profileVelocity) { // slow down to profile velocity first
+            
+            float t1 = (velocity-profileVelocity)/profileDeceleration;
+            float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
+            float t3 = profileVelocity/profileDeceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+                velocity += -profileDeceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)(velocity*(period-t1));
+            } else if (t1+t2+t3 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+                velocity += -profileDeceleration*(period-t1-t2);
+            } else {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
+                velocity += -profileDeceleration*t3;
+            }
+            
+        } else if (velocity > 0.0f) { // speed up to profile velocity
+            
+            float t1 = (profileVelocity-velocity)/profileAcceleration;
+            float t3 = profileVelocity/profileDeceleration;
+            float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+            
+            if (t2 < 0.0f) {
+                float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+                t1 = (maxVelocity-velocity)/profileAcceleration;
+                t2 = 0.0f;
+                t3 = maxVelocity/profileDeceleration;
+            }
+            
+            if (t1 > period) {
+                position += (double)((velocity+profileAcceleration*0.5f*period)*period);
+                velocity += profileAcceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+                velocity += profileAcceleration*t1;
+                position += (double)(velocity*(period-t1));
+            } else if (t1+t2+t3 > period) {
+                position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+                velocity += profileAcceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+                velocity += -profileDeceleration*(period-t1-t2);
+            } else {
+                position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+                velocity += profileAcceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
+                velocity += -profileDeceleration*t3;
+            }
+            
+        } else { // slow down to zero first, and then speed up to profile velocity
+            
+            float t1 = -velocity/profileDeceleration;
+            float t2 = profileVelocity/profileAcceleration;
+            float t4 = profileVelocity/profileDeceleration;
+            float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+            
+            if (t3 < 0.0f) {
+                float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+                t2 = maxVelocity/profileAcceleration;
+                t3 = 0.0f;
+                t4 = maxVelocity/profileDeceleration;
+            }
+            
+            if (t1 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+                velocity += profileDeceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
+                velocity += profileAcceleration*(period-t1);
+            } else if (t1+t2+t3 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+                velocity += profileAcceleration*t2;
+                position += (double)(velocity*(period-t1-t2));
+            } else if (t1+t2+t3+t4 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+                velocity += profileAcceleration*t2;
+                position += (double)(velocity*t3);
+                position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
+                velocity += -profileDeceleration*(period-t1-t2-t3);
+            } else {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+                velocity += profileAcceleration*t2;
+                position += (double)(velocity*t3);
+                position += (double)((velocity-profileDeceleration*0.5f*t4)*t4);
+                velocity += -profileDeceleration*t4;
+            }
+        }
+        
+    } else { // negative velocity required
+        
+        if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
+            
+            float t1 = (-profileVelocity-velocity)/profileDeceleration;
+            float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
+            float t3 = profileVelocity/profileDeceleration;
+            
+            if (t1 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+                velocity += profileDeceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)(velocity*(period-t1));
+            } else if (t1+t2+t3 > period) {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+                velocity += profileDeceleration*(period-t1-t2);
+            } else {
+                position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+                velocity += profileDeceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
+                velocity += profileDeceleration*t3;
+            }
+            
+        } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
+            
+            float t1 = (velocity+profileVelocity)/profileAcceleration;
+            float t3 = profileVelocity/profileDeceleration;
+            float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+            
+            if (t2 < 0.0f) {
+                float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+                t1 = (velocity-minVelocity)/profileAcceleration;
+                t2 = 0.0f;
+                t3 = -minVelocity/profileDeceleration;
+            }
+            
+            if (t1 > period) {
+                position += (double)((velocity-profileAcceleration*0.5f*period)*period);
+                velocity += -profileAcceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+                velocity += -profileAcceleration*t1;
+                position += (double)(velocity*(period-t1));
+            } else if (t1+t2+t3 > period) {
+                position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+                velocity += -profileAcceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+                velocity += profileDeceleration*(period-t1-t2);
+            } else {
+                position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+                velocity += -profileAcceleration*t1;
+                position += (double)(velocity*t2);
+                position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
+                velocity += profileDeceleration*t3;
+            }
+            
+        } else { // slow down to zero first, and then speed up to (negative) profile velocity
+            
+            float t1 = velocity/profileDeceleration;
+            float t2 = profileVelocity/profileAcceleration;
+            float t4 = profileVelocity/profileDeceleration;
+            float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+            
+            if (t3 < 0.0f) {
+                float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+                t2 = -minVelocity/profileAcceleration;
+                t3 = 0.0f;
+                t4 = -minVelocity/profileDeceleration;
+            }
+            
+            if (t1 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+                velocity += -profileDeceleration*period;
+            } else if (t1+t2 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
+                velocity += -profileAcceleration*(period-t1);
+            } else if (t1+t2+t3 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+                velocity += -profileAcceleration*t2;
+                position += (double)(velocity*(period-t1-t2));
+            } else if (t1+t2+t3+t4 > period) {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+                velocity += -profileAcceleration*t2;
+                position += (double)(velocity*t3);
+                position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
+                velocity += profileDeceleration*(period-t1-t2-t3);
+            } else {
+                position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+                velocity += -profileDeceleration*t1;
+                position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+                velocity += -profileAcceleration*t2;
+                position += (double)(velocity*t3);
+                position += (double)((velocity+profileDeceleration*0.5f*t4)*t4);
+                velocity += profileDeceleration*t4;
+            }
+        }
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,60 @@
+/*
+ * Motion.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef MOTION_H_
+#define MOTION_H_
+
+#include <cstdlib>
+
+/**
+ * This class keeps the motion values <code>position</code> and <code>velocity</code>, and
+ * offers methods to increment these values towards a desired target position or velocity.
+ * <br/>
+ * To increment the current motion values, this class uses a simple 2nd order motion planner.
+ * This planner calculates the motion to the target position or velocity with the various motion
+ * phases, based on given limits for the profile velocity, acceleration and deceleration.
+ * <br/>
+ * Note that the trajectory is calculated every time the motion state is incremented.
+ * This allows to change the target position or velocity, as well as the limits for profile
+ * velocity, acceleration and deceleration at any time.
+ */
+class Motion {
+    
+    public:
+        
+        double      position;       /**< The position value of this motion, given in [m] or [rad]. */
+        float       velocity;       /**< The velocity value of this motion, given in [m/s] or [rad/s]. */
+        
+                    Motion();
+                    Motion(double position, float velocity);
+                    Motion(const Motion& motion);
+        virtual     ~Motion();
+        void        set(double position, float velocity);
+        void        set(const Motion& motion);
+        void        setPosition(double position);
+        double      getPosition();
+        void        setVelocity(float velocity);
+        float       getVelocity();
+        void        setProfileVelocity(float profileVelocity);
+        void        setProfileAcceleration(float profileAcceleration);
+        void        setProfileDeceleration(float profileDeceleration);
+        void        setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration);
+        float       getTimeToPosition(double targetPosition);
+        void        incrementToVelocity(float targetVelocity, float period);
+        void        incrementToPosition(double targetPosition, float period);
+        
+    private:
+        
+        static const float  DEFAULT_LIMIT;  // default value for limits
+        static const float  MINIMUM_LIMIT;  // smallest value allowed for limits
+        
+        float       profileVelocity;
+        float       profileAcceleration;
+        float       profileDeceleration;
+};
+
+#endif /* MOTION_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,142 @@
+/*
+ * StateMachine.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include <algorithm>
+#include "StateMachine.h"
+
+using namespace std;
+
+const float StateMachine::PERIOD = 0.01f;                   // period of task in [s]
+const float StateMachine::DISTANCE_THRESHOLD = 0.25f;        // minimum allowed distance to obstacle in [m]
+const float StateMachine::TRANSLATIONAL_VELOCITY = 0.8f;    // translational velocity in [m/s]
+const float StateMachine::ROTATIONAL_VELOCITY = 4.0f;       // rotational velocity in [rad/s]
+
+/**
+ * Creates and initializes a state machine object.
+ */
+StateMachine::StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5) : controller(controller), enableMotorDriver(enableMotorDriver), led0(led0), led1(led1), led2(led2), led3(led3), led4(led4), led5(led5), button(button), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3), irSensor4(irSensor4), irSensor5(irSensor5) {
+    
+    enableMotorDriver = 0;
+    state = ROBOT_OFF;
+    buttonNow = button;
+    buttonBefore = buttonNow;
+    
+    ticker.attach(callback(this, &StateMachine::run), PERIOD);
+}
+
+/**
+ * Deletes the state machine object and releases all allocated resources.
+ */
+StateMachine::~StateMachine() {
+    
+    ticker.detach();
+}
+
+/**
+ * Gets the actual state of this state machine.
+ * @return the actual state as an int constant.
+ */
+int StateMachine::getState() {
+    
+    return state;
+}
+
+/**
+ * This method is called periodically by the ticker object and contains the
+ * implementation of the state machine.
+ */
+void StateMachine::run() {
+    
+    // control leds based on distance measurements
+    
+    led0 = irSensor0 < DISTANCE_THRESHOLD;
+    led1 = irSensor1 < DISTANCE_THRESHOLD;
+    led2 = irSensor2 < DISTANCE_THRESHOLD;
+    led3 = irSensor3 < DISTANCE_THRESHOLD;
+    led4 = irSensor4 < DISTANCE_THRESHOLD;
+    led5 = irSensor5 < DISTANCE_THRESHOLD;
+    
+    // implementation of the state machine
+    
+    switch (state) {
+        
+        case ROBOT_OFF:
+            
+            buttonNow = button;
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                enableMotorDriver = 1;
+                controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                state = MOVE_FORWARD;
+            }
+            buttonBefore = buttonNow;
+            
+            break;
+            
+        case MOVE_FORWARD:
+            
+            buttonNow = button;
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                controller.setTranslationalVelocity(0.0f);
+                state = SLOWING_DOWN;
+            } else if ((irSensor0 < DISTANCE_THRESHOLD) || (irSensor1 < DISTANCE_THRESHOLD)) {
+                controller.setTranslationalVelocity(0.0f);
+                controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
+                state = TURN_LEFT;
+            } else if (irSensor5.read() < DISTANCE_THRESHOLD) {
+                controller.setTranslationalVelocity(0.0f);
+                controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
+                state = TURN_RIGHT;
+            }
+            buttonBefore = buttonNow;
+            
+            break;
+            
+        case TURN_LEFT:
+            
+            buttonNow = button;
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                controller.setRotationalVelocity(0.0f);
+                state = SLOWING_DOWN;
+            } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
+                controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                controller.setRotationalVelocity(0.0f);
+                state = MOVE_FORWARD;
+            }
+            buttonBefore = buttonNow;
+            
+            break;
+            
+        case TURN_RIGHT:
+            
+            buttonNow = button;
+            if (buttonNow && !buttonBefore) {   // detect button rising edge
+                controller.setRotationalVelocity(0.0f);
+                state = SLOWING_DOWN;
+            } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
+                controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+                controller.setRotationalVelocity(0.0f);
+                state = MOVE_FORWARD;
+            }
+            buttonBefore = buttonNow;
+            
+            break;
+            
+        case SLOWING_DOWN:
+            
+            if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) {
+                enableMotorDriver = 0;
+                state = ROBOT_OFF;
+            }
+            
+            break;
+            
+        default:
+            
+            state = ROBOT_OFF;
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,73 @@
+/*
+ * StateMachine.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef STATE_MACHINE_H_
+#define STATE_MACHINE_H_
+
+#include <cstdlib>
+#include <mbed.h>
+#include "Controller.h"
+#include "IRSensor.h"
+
+#include <deque>
+#include "Task.h"
+#include "TaskWait.h"
+#include "TaskMoveTo.h"
+
+/**
+ * This class implements a simple state machine for a mobile robot.
+ * It allows to move the robot forward, and to turn left or right,
+ * depending on distance measurements, to avoid collisions with
+ * obstacles.
+ */
+class StateMachine {
+    
+    public:
+        
+        static const int   ROBOT_OFF = 0;   // discrete states of this state machine
+        static const int   MOVE_FORWARD = 1;
+        static const int   TURN_LEFT = 2;
+        static const int   TURN_RIGHT = 3;
+        static const int   SLOWING_DOWN = 4;
+        
+                    StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5);
+        virtual     ~StateMachine();
+        int         getState();
+        
+    private:
+        
+        static const float PERIOD;                  // period of task in [s]
+        static const float DISTANCE_THRESHOLD;      // minimum allowed distance to obstacle in [m]
+        static const float TRANSLATIONAL_VELOCITY;  // translational velocity in [m/s]
+        static const float ROTATIONAL_VELOCITY;     // rotational velocity in [rad/s]
+        
+        Controller&     controller;
+        DigitalOut&     enableMotorDriver;
+        DigitalOut&     led0;
+        DigitalOut&     led1;
+        DigitalOut&     led2;
+        DigitalOut&     led3;
+        DigitalOut&     led4;
+        DigitalOut&     led5;
+        DigitalIn&      button;
+        IRSensor&       irSensor0;
+        IRSensor&       irSensor1;
+        IRSensor&       irSensor2;
+        IRSensor&       irSensor3;
+        IRSensor&       irSensor4;
+        IRSensor&       irSensor5;
+        int             state;
+        int             buttonNow;
+        int             buttonBefore;
+        Ticker          ticker;
+        
+        void            run();
+        
+        deque<Task*> taskList;
+};
+
+#endif /* STATE_MACHINE_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Task.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,31 @@
+/*
+ * Task.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include "Task.h"
+
+using namespace std;
+
+/**
+ * Creates an abstract task object.
+ */
+Task::Task() {}
+
+/**
+ * Deletes the task object.
+ */
+Task::~Task() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * It contains the code this task has to work on.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. TASK_RUNNING or TASK_DONE.
+ */
+int Task::run(float period) {
+    
+    return DONE;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Task.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,30 @@
+/*
+ * Task.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_H_
+#define TASK_H_
+
+#include <cstdlib>
+
+/**
+ * This is an abstract task class with a method that
+ * is called periodically by a task sequencer.
+ */
+class Task {
+    
+    public:
+        
+        static const int    FAULT = -1;     /**< Task return value. */
+        static const int    RUNNING = 0;    /**< Task return value. */
+        static const int    DONE = 1;       /**< Task return value. */
+        
+                        Task();
+        virtual         ~Task();
+        virtual int     run(float period);
+};
+
+#endif /* TASK_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMoveTo.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,117 @@
+/*
+ * TaskMoveTo.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "TaskMoveTo.h"
+
+using namespace std;
+
+const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f;
+const float TaskMoveTo::DEFAULT_ZONE = 0.01f;
+const float TaskMoveTo::PI = 3.14159265f;
+const float TaskMoveTo::K1 = 2.0f;
+const float TaskMoveTo::K2 = 1.0f;
+const float TaskMoveTo::K3 = 0.5f;
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param alpha the target orientation, given in [rad].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->alpha = alpha;
+    this->velocity = DEFAULT_VELOCITY;
+    this->zone = DEFAULT_ZONE;
+}
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param alpha the target orientation, given in [rad].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->alpha = alpha;
+    this->velocity = velocity;
+    this->zone = DEFAULT_ZONE;
+}
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param alpha the target orientation, given in [rad].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ * @param zone the zone threshold around the target position, given in [m].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
+    
+    this->x = x;
+    this->y = y;
+    this->alpha = alpha;
+    this->velocity = velocity;
+    this->zone = zone;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskMoveTo::~TaskMoveTo() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskMoveTo::run(float period) {
+    
+    // bitte implementieren!
+    
+    float rho  = sqrt(pow(this->x - this->controller.getX(),2.0f) + pow(this->y - this->controller.getY(),2.0f));
+    if (rho == 0.0f) { // no need to move, already in range
+    }
+    else {
+        float gama = atan2(this->y - this->controller.getY(),this->x - this->controller.getX()) - this->alpha;
+        if (gama == 0) {
+            this->controller.setTranslationalVelocity(this->K1 * rho * cos(gama));
+        }
+        else if (gama > this->PI) {
+            gama = gama - PI;
+        }
+        else if (gama < this->PI) {
+            gama = gama + PI;
+        }
+        else {
+            float delta = gama + this->alpha - this->controller.getAlpha();
+            if (delta > this->PI) {
+                delta = delta - PI;
+            }
+            else if (delta < this->PI) {
+                delta = delta + PI;
+            }
+            this->controller.setTranslationalVelocity(this->K1 * rho * cos(gama));
+            this->controller.setRotationalVelocity(this->K2*gama+this->K1*sin(gama)*cos(gama)*(gama+this->K3*delta)/gama);
+        }
+    }
+            
+    if (rho < zone)
+        return DONE;
+    else 
+        return RUNNING;
+
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMoveTo.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,46 @@
+/*
+ * TaskMoveTo.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_MOVE_TO_H_
+#define TASK_MOVE_TO_H_
+
+#include <cstdlib>
+#include "Task.h"
+#include "Controller.h"
+
+/**
+ * This is a specific implementation of a task class that moves the robot to a given pose.
+ */
+class TaskMoveTo : public Task {
+    
+    public:
+        
+        static const float  DEFAULT_VELOCITY;   /**< Default velocity value, given in [m/s]. */
+        static const float  DEFAULT_ZONE;       /**< Default zone value, given in [m]. */
+        
+                    TaskMoveTo(Controller& controller, float x, float y, float alpha);
+                    TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity);
+                    TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone);
+        virtual     ~TaskMoveTo();
+        virtual int run(float period);
+        
+    private:
+        
+        static const float  PI;
+        static const float  K1;
+        static const float  K2;
+        static const float  K3;
+        
+        Controller& controller; // reference to the controller object to use
+        float       x;          // x coordinate of target position, given in [m]
+        float       y;          // y coordinate of target position, given in [m]
+        float       alpha;      // target orientation, given in [rad]
+        float       velocity;   // maximum translational velocity, given in [m/s]
+        float       zone;       // zone threshold around target position, given in [m]
+};
+
+#endif /* TASK_MOVE_TO_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskWait.cpp	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,47 @@
+/*
+ * TaskWait.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include "TaskWait.h"
+
+using namespace std;
+
+/**
+ * Creates a task object that waits for a given duration.
+ */
+TaskWait::TaskWait(Controller& controller, float duration) : controller(controller) {
+    
+    this->duration = duration;
+    
+    time = 0.0f;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskWait::~TaskWait() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskWait::run(float period) {
+    
+    controller.setTranslationalVelocity(0.0f);
+    controller.setRotationalVelocity(0.0f);
+    
+    time += period;
+    
+    if (time < duration) {
+        
+        return RUNNING;
+        
+    } else {
+        
+        return DONE;
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskWait.h	Fri Mar 31 11:00:19 2017 +0000
@@ -0,0 +1,33 @@
+/*
+ * TaskWait.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_WAIT_H_
+#define TASK_WAIT_H_
+
+#include <cstdlib>
+#include "Task.h"
+#include "Controller.h"
+
+/**
+ * This is a specific implementation of a task class that waits for a given duration.
+ */
+class TaskWait : public Task {
+    
+    public:
+        
+                    TaskWait(Controller& controller, float duration);
+        virtual     ~TaskWait();
+        virtual int run(float period);
+        
+    private:
+        
+        Controller& controller;
+        float       duration;
+        float       time;
+};
+
+#endif /* TASK_WAIT_H_ */
+

--- a/main.cpp	Fri Mar 10 17:00:01 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,45 +0,0 @@
-#include "mbed.h"
-#include "IRSensor.h"
-
-
-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); // Kreieren der Ein- und Ausgangsobjekte
-DigitalOut enable(PC_1);
-DigitalOut bit0(PH_1);
-DigitalOut bit1(PC_2);
-DigitalOut bit2(PC_3);
-
-DigitalOut enableMotorDriver(PB_2);
-DigitalIn motorDriverFault(PB_14);
-DigitalIn motorDriverWarning(PB_15);
-PwmOut pwmLeft(PA_8);
-PwmOut pwmRight(PA_9);
-
-
-DigitalOut myled(LED1);
-
-int main() {
-
-IRSensor irSensor0(distance, bit0, bit1, bit2, 0); // Objekte kreieren
-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);
-enable = 1; // schaltet die Sensoren ein
-pwmLeft.period(0.00005); // Setzt die Periode auf 50 μs
-pwmRight.period(0.00005);
-pwmLeft = 0.7; // Setzt die Duty-Cycle auf 50%
-pwmRight = 0.7;
-enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
-    while(1) {
-        
-  
-    }
-}