Revision 3:92ba0254af87, committed 2013-03-07

Comitter:

chrigelburri

Date:

Thu Mar 07 09:47:07 2013 +0000

Parent:

2:d8e1613dc38b

Child:

4:3a97923ff2d4

Commit message:

bitte kommentare korriegieren;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Hallsensor.cpp	Thu Mar 07 09:47:07 2013 +0000
@@ -0,0 +1,115 @@
+#include "Hallsensor.h"
+
+Hallsensor::Hallsensor(
+    PinName hall1,
+    PinName hall2,
+    PinName hall3
+)
+    : hall1_(hall1),
+      hall2_(hall2),
+      hall3_(hall3)
+{
+    pulses_       = 0;
+
+    //Workout what the current state is.
+    int h1 = hall1_.read();
+    int h2 = hall2_.read();
+    int h3 = hall3_.read();
+
+    // Set the PullUp Resistor
+    hall1_.mode(PullUp);
+    hall2_.mode(PullUp);
+    hall3_.mode(PullUp);
+
+    //set interrupts on only hall 1-3 rise and fall.
+    hall1_.rise(this, &Hallsensor::encode);
+    hall1_.fall(this, &Hallsensor::encode);
+    hall2_.rise(this, &Hallsensor::encode);
+    hall2_.fall(this, &Hallsensor::encode);
+    hall3_.rise(this, &Hallsensor::encode);
+    hall3_.fall(this, &Hallsensor::encode);
+}
+
+void Hallsensor::reset(void)
+{
+    pulses_ = 0;
+}
+
+int Hallsensor::getPulses(void)
+{
+    return pulses_;
+}
+
+
+void Hallsensor::encode(void)
+{
+    // read the state
+    int h1  = hall1_.read();
+    int h2  = hall2_.read();
+    int h3  = hall3_.read();
+
+    //3-bit state.
+    currState_ = (h1 << 2) | (h2 << 1) | h3;
+
+    if ((prevState_ == 0x5) && (currState_ == 0x4)) {
+        pulses_++;
+        prevState_ = currState_;
+        return;
+    } else if (prevState_ == 0x5 && currState_ == 0x1) {
+        pulses_--;
+        prevState_ = currState_;
+        return;
+    }
+
+    if ((prevState_ == 0x4) && (currState_ == 0x6)) {
+        pulses_++;
+        prevState_ = currState_;
+        return;
+    } else if (prevState_ == 0x4 && currState_ == 0x5) {
+        pulses_--;
+        prevState_ = currState_;
+        return;
+    }
+
+    if ((prevState_ == 0x6) && (currState_ == 0x2)) {
+        pulses_++;
+        prevState_ = currState_;
+        return;
+    } else if (prevState_ == 0x6 && currState_ == 0x4) {
+        pulses_--;
+        prevState_ = currState_;
+        return;
+    }
+
+    if ((prevState_ == 0x2) && (currState_ == 0x3)) {
+        pulses_++;
+        prevState_ = currState_;
+        return;
+    } else if (prevState_ == 0x2 && currState_ == 0x6) {
+        pulses_--;
+        prevState_ = currState_;
+        return;
+    }
+
+    if ((prevState_ == 0x3) && (currState_ == 0x1)) {
+        pulses_++;
+        prevState_ = currState_;
+        return;
+    } else if (prevState_ == 0x3 && currState_ == 0x2) {
+        pulses_--;
+        prevState_ = currState_;
+        return;
+    }
+
+    if ((prevState_ == 0x1) && (currState_ == 0x5)) {
+        pulses_++;
+        prevState_ = currState_;
+        return;
+    } else if (prevState_ == 0x1 && currState_ == 0x3) {
+        pulses_--;
+        prevState_ = currState_;
+        return;
+    }
+    prevState_ = currState_;
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Hallsensor.h	Thu Mar 07 09:47:07 2013 +0000
@@ -0,0 +1,76 @@
+#ifndef HALLSENSOR_H
+#define HALLSENSOR_H
+
+#include "mbed.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * Interface to count the Hallsensor input from a EC-Motor.
+ * 
+ */
+class Hallsensor
+{
+
+public:
+
+    /**
+     * Constructor of the class <code>Hallsensor</code>.
+     *
+     * Reads the current values on Hall1 , Hall2 and Hall3 to determine the
+     * initial state.
+     * Attaches the encode function to the rise/fall interrupt edges of
+     * Hall1, Hall2 and Hall3.
+     * @param hall1    mbed pin for Hall1 input.
+     * @param hall2    mbed pin for Hall2 input.
+     * @param hall3    mbed pin for Hall3 input.
+     */
+    Hallsensor(PinName hall1, PinName hall2, PinName hall3);
+
+    /**
+     * Reset the encoder.
+     * Sets the pulses and revolutions count to zero.
+     */
+    void reset(void);
+
+    /**
+     * Read the number of pulses recorded by the encoder.
+     * @return Number of pulses which have occured, given in [count]
+     */
+    int getPulses(void);
+
+    /**
+     * Read the number of revolutions recorded by the encoder.
+     * @return Number of revolutions which have occured on the index channel.
+     */
+    int getRevolutions(void);
+
+private:
+
+    /**
+     * Update the pulse count.
+     * Called on every rising/falling edge of Hall 1-3.
+     * Reads the state of the channels and determines whether a pulse forward
+     * or backward has occured, updating the count appropriately.
+     */
+    void encode(void);
+
+    InterruptIn hall1_;
+    InterruptIn hall2_;
+    InterruptIn hall3_;
+
+    int          prevState_;
+    int          currState_;
+
+    volatile int pulses_;
+
+};
+
+#endif /* Hallsensor_H */

--- a/Actuators/Hallsensor/Hallsensor.cpp	Sun Mar 03 16:26:47 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,115 +0,0 @@
-#include "Hallsensor.h"
-
-Hallsensor::Hallsensor(
-    PinName hall1,
-    PinName hall2,
-    PinName hall3
-)
-    : hall1_(hall1),
-      hall2_(hall2),
-      hall3_(hall3)
-{
-    pulses_       = 0;
-
-    //Workout what the current state is.
-    int h1 = hall1_.read();
-    int h2 = hall2_.read();
-    int h3 = hall3_.read();
-
-    // Set the PullUp Resistor
-    hall1_.mode(PullUp);
-    hall2_.mode(PullUp);
-    hall3_.mode(PullUp);
-
-    //set interrupts on only hall 1-3 rise and fall.
-    hall1_.rise(this, &Hallsensor::encode);
-    hall1_.fall(this, &Hallsensor::encode);
-    hall2_.rise(this, &Hallsensor::encode);
-    hall2_.fall(this, &Hallsensor::encode);
-    hall3_.rise(this, &Hallsensor::encode);
-    hall3_.fall(this, &Hallsensor::encode);
-}
-
-void Hallsensor::reset(void)
-{
-    pulses_ = 0;
-}
-
-int Hallsensor::getPulses(void)
-{
-    return pulses_;
-}
-
-
-void Hallsensor::encode(void)
-{
-    // read the state
-    int h1  = hall1_.read();
-    int h2  = hall2_.read();
-    int h3  = hall3_.read();
-
-    //3-bit state.
-    currState_ = (h1 << 2) | (h2 << 1) | h3; // | Bitweise oder Verknüpfung Beispiel: 1100 | 1010 = 1110
-
-    if ((prevState_ == 0x5) && (currState_ == 0x4)) {
-        pulses_++;
-        prevState_ = currState_;
-        return;
-    } else if (prevState_ == 0x5 && currState_ == 0x1) {
-        pulses_--;
-        prevState_ = currState_;
-        return;
-    }
-
-    if ((prevState_ == 0x4) && (currState_ == 0x6)) {
-        pulses_++;
-        prevState_ = currState_;
-        return;
-    } else if (prevState_ == 0x4 && currState_ == 0x5) {
-        pulses_--;
-        prevState_ = currState_;
-        return;
-    }
-
-    if ((prevState_ == 0x6) && (currState_ == 0x2)) {
-        pulses_++;
-        prevState_ = currState_;
-        return;
-    } else if (prevState_ == 0x6 && currState_ == 0x4) {
-        pulses_--;
-        prevState_ = currState_;
-        return;
-    }
-
-    if ((prevState_ == 0x2) && (currState_ == 0x3)) {
-        pulses_++;
-        prevState_ = currState_;
-        return;
-    } else if (prevState_ == 0x2 && currState_ == 0x6) {
-        pulses_--;
-        prevState_ = currState_;
-        return;
-    }
-
-    if ((prevState_ == 0x3) && (currState_ == 0x1)) {
-        pulses_++;
-        prevState_ = currState_;
-        return;
-    } else if (prevState_ == 0x3 && currState_ == 0x2) {
-        pulses_--;
-        prevState_ = currState_;
-        return;
-    }
-
-    if ((prevState_ == 0x1) && (currState_ == 0x5)) {
-        pulses_++;
-        prevState_ = currState_;
-        return;
-    } else if (prevState_ == 0x1 && currState_ == 0x3) {
-        pulses_--;
-        prevState_ = currState_;
-        return;
-    }
-    prevState_ = currState_;
-
-}

--- a/Actuators/Hallsensor/Hallsensor.h	Sun Mar 03 16:26:47 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,84 +0,0 @@
-#ifndef HALLSENSOR_H
-#define HALLSENSOR_H
-
-#include "mbed.h"
-
-/**
- * @author Christian Burri
- *
- * @section LICENSE
- *
- * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
- * All rights reserved.
- *
- * @section DESCRIPTION
- *
- * Interface to count the Hallsensor input from a EC-Motor.
- * 
- */
-class Hallsensor
-{
-
-public:
-
-    /**
-     * Constructor.
-     *
-     * Reads the current values on Hall1 , Hall2 and Hall3 to determine the
-     * initial state.
-     *
-     * Attaches the encode function to the rise/fall interrupt edges of
-     * Hall1, Hall2 and Hall3.
-     *
-     *
-     * @param hall1    mbed pin for Hall1 input.
-     * @param hall2    mbed pin for Hall2 input.
-     * @param hall3    mbed pin for Hall3 input.
-     */
-    Hallsensor(PinName hall1, PinName hall2, PinName hall3);
-
-    /**
-     * Reset the encoder.
-     *
-     * Sets the pulses and revolutions count to zero.
-     */
-    void reset(void);
-
-    /**
-     * Read the number of pulses recorded by the encoder.
-     *
-     * @return Number of pulses which have occured.
-     */
-    int getPulses(void);
-
-    /**
-     * Read the number of revolutions recorded by the encoder on the index channel.
-     *
-     * @return Number of revolutions which have occured on the index channel.
-     */
-    int getRevolutions(void);
-
-private:
-
-    /**
-     * Update the pulse count.
-     *
-     * Called on every rising/falling edge of Hall 1-3.
-     *
-     * Reads the state of the channels and determines whether a pulse forward
-     * or backward has occured, updating the count appropriately.
-     */
-    void encode(void);
-
-    InterruptIn hall1_;
-    InterruptIn hall2_;
-    InterruptIn hall3_;
-
-    int          prevState_;
-    int          currState_;
-
-    volatile int pulses_;
-
-};
-
-#endif /* Hallsensor_H */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/MaxonESCON.cpp	Thu Mar 07 09:47:07 2013 +0000
@@ -0,0 +1,86 @@
+#include "MaxonESCON.h"
+
+using namespace std;
+
+MaxonESCON::MaxonESCON(
+    PinName enb,
+    PinName isenb,
+    PinName pwm,
+    PinName actualSpeed,
+    Hallsensor *hall
+)
+    :
+    _enb(enb),
+    _isenb(isenb),
+    _pwm(pwm),
+    _actualSpeed(actualSpeed),
+    _hall(hall)
+{
+
+    _pwm = 0;
+
+    // Initial condition of output enables
+    _enb = 0;
+
+    // Set initial condition of PWM 2kHz
+    period(0.0005);
+
+    // Set the pulses to zero
+    _pulses = 0;
+
+    // Set the Pull Up Resistor
+    _isenb.mode(PullUp);
+}
+
+void MaxonESCON::setVelocity(float speed)
+{
+    speed = speed / ESCON_SET_FACTOR * 60.0f;
+    if(speed > 1 ) {
+        _pwm = 0.9f;
+    } else if(speed < -1) {
+        _pwm = 0.1f;
+    } else {
+        _pwm = 0.4f*speed + 0.5f;
+    }
+}
+
+float MaxonESCON::getActualSpeed(void)
+{
+    return (_actualSpeed.read()* 2.0f - 1.0f) * ESCON_GET_FACTOR / 60.0f;
+}
+
+void MaxonESCON::period(float period)
+{
+    _pwm.period(period);
+}
+
+void MaxonESCON::enable(bool enb)
+{
+    if(enb == false) {
+        _enb = 1;
+    } else {
+        _enb = 0;
+    }
+}
+
+bool MaxonESCON::isEnabled()
+{
+    if(_isenb.read() == 1) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+int MaxonESCON::getPulses(void)
+{
+    _pulses = _hall->getPulses();
+    return _pulses;
+}
+
+int MaxonESCON::setPulses(int setPos)
+{
+    _hall->reset();
+    _pulses = _hall->getPulses();
+    return _pulses;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/MaxonESCON.h	Thu Mar 07 09:47:07 2013 +0000
@@ -0,0 +1,97 @@
+#ifndef _MAXON_ESCON_H_
+#define _MAXON_ESCON_H_
+
+#include "mbed.h"
+#include "Hallsensor.h"
+#include "defines.h"
+
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
+ * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * This class implements the driver for the Maxon ESCON servo driver.
+ * For more information see on the Datasheet:
+ *
+ * Datasheet:
+ * <a href="http://escon.maxonmotor.com">http://escon.maxonmotor.com</a>
+ */
+class MaxonESCON
+{
+
+private:
+
+    /** To Enable the amplifier */
+    DigitalOut _enb;
+    /** Duty Cycle to set the speed */
+    PwmOut _pwm;
+    /** Hallsensor Class */
+    Hallsensor* _hall;
+    /** Ready output from ESCON */
+    DigitalIn _isenb;
+    /** Actual speed from ESCON analog Output 1 */
+    AnalogIn _actualSpeed;
+    /** increment the Hallpattern */
+    int    _pulses;
+
+public:
+
+    /** Create a motor control object.
+       * @param enb DigitalOut, set high for enable
+       * @param isenb DigitalIn, high for enable
+       * @param pwm PwmOut pin, set the velocity
+       * @param actualSpeed AnalogIn filtered signal for ActualSpeed from Motor
+       * @param hall The object of the hallsensor from Motor
+       */
+    MaxonESCON(PinName enb,
+               PinName isenb,
+               PinName pwm,
+               PinName actualSpeed,
+               Hallsensor *hall);
+
+    /** Set the speed of the motor with a pwm for 10%..90%.
+    * 50% PWM is 0rpm.
+    * Caclulate from [1/s] in [1/min] and the Factor of the ESCON.
+    * @param speed The speed of the motor as a normalised value, given in [1/s]
+    */
+    void setVelocity(float speed);
+
+    /**Return the speed from ESCON.
+    * 0 rpm is defined in the Analog input as 1.65V
+    * @return speed of the motor, given in [1/s]
+    */
+    float getActualSpeed(void);
+
+    /** Set the period of the pwm duty cycle.
+    * Wrapper for PwmOut::period()
+    * @param period Pwm duty cycle, given in [s].
+    */
+    void period(float period);
+
+    /** Set the Motor to a enable sate.
+    * @param enb <code>false</code> for disable <code>true</code> for enable.
+    */
+    void enable(bool enb);
+
+    /**Tests if the servo drive is enabled.
+    * @return <code>true</code> if the drive is enabled, <code>false</code> otherwise.
+    */
+    bool isEnabled(void);
+
+    /** Return the number of Pulses.
+    * @return Pulses, given in [count]
+    */
+    int getPulses(void);
+
+    /** Set the Pulses of the Motor, given in [count]
+    * @return Pulses, given in [count]
+    */
+    int setPulses(int setPos);
+};
+
+#endif

--- a/Actuators/MaxonESCON/MaxonESCON.cpp	Sun Mar 03 16:26:47 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,86 +0,0 @@
-#include "MaxonESCON.h"
-
-using namespace std;
-
-MaxonESCON::MaxonESCON(
-    PinName enb,
-    PinName isenb,
-    PinName pwm,
-    PinName actualSpeed,
-    Hallsensor *hall
-)
-    :
-    _enb(enb),
-    _isenb(isenb),
-    _pwm(pwm),
-    _actualSpeed(actualSpeed),
-    _hall(hall)
-{
-
-    _pwm = 0;
-
-    // Initial condition of output enables
-    _enb = 0;
-
-    // Set initial condition of PWM 2kHz
-    period(0.0005);
-
-    // Set the pulses to zero
-    _pulses = 0;
-
-    // Set the Pull Up Resistor
-    _isenb.mode(PullUp);
-}
-
-void MaxonESCON::setVelocity(float speed)
-{
-    speed = speed / ESCON_SET_FACTOR * 60.0f;
-    if(speed > 1 ) {
-        _pwm = 0.9f;
-    } else if(speed < -1) {
-        _pwm = 0.1f;
-    } else {
-        _pwm = 0.4f*speed + 0.5f;
-    }
-}
-
-float MaxonESCON::getActualSpeed(void)
-{
-    return (_actualSpeed.read()* 2.0f - 1.0f) * ESCON_GET_FACTOR / 60.0f;
-}
-
-void MaxonESCON::period(float period)
-{
-    _pwm.period(period);
-}
-
-void MaxonESCON::enable(bool enb)
-{
-    if(enb == false) {
-        _enb = 1;
-    } else {
-        _enb = 0;
-    }
-}
-
-bool MaxonESCON::isEnabled()
-{
-    if(_isenb.read() == 1) {
-        return true;
-    } else {
-        return false;
-    }
-}
-
-int MaxonESCON::getPulses(void)
-{
-    _pulses = _hall->getPulses();
-    return _pulses;
-}
-
-int MaxonESCON::setPulses(int setPos)
-{
-    _hall->reset();
-    _pulses = _hall->getPulses();
-    return _pulses;
-}

--- a/Actuators/MaxonESCON/MaxonESCON.h	Sun Mar 03 16:26:47 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,107 +0,0 @@
-#ifndef _MAXON_ESCON_H_
-#define _MAXON_ESCON_H_
-
-#include "mbed.h"
-#include "Hallsensor.h"
-#include "defines.h"
-
-/**
- * @author Christian Burri
- *
- * @section LICENSE
- *
- * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
- * All rights reserved.
- *
- * @section DESCRIPTION
- *
- * This class implements the driver for the Maxon ESCON servo driver.....
- *
- * Datasheet:
- *
- * http://escon.maxonmotor.com
- */
-class MaxonESCON
-{
-
-private:
-
-    /** To Enable the amplifier */
-    DigitalOut _enb;
-    /** Duty Cycle to set the speed */
-    PwmOut _pwm;
-    /** Hallsensor Class */
-    Hallsensor* _hall;
-    /** Ready output from ESCON */
-    DigitalIn _isenb;
-    /** Actual speed from ESCON analog Output 1 */
-    AnalogIn _actualSpeed;
-    /** increment the Hallpattern */
-    int    _pulses;
-
-public:
-
-    /** Create a motor control object
-       *
-       * @param enb DigitalOut, set high for enable
-       * @param isenb DigitalIn, high for enable
-       * @param pwm PwmOut pin, set the Velocity
-       * @param hall HALL Object
-       * @param actualSpeed AnalogIn Filtered Signal for ActualSpeed from Motor
-       * @param hall The Object of the Hallsensor from Motor
-       */
-    MaxonESCON(PinName enb,
-               PinName isenb,
-               PinName pwm,
-               PinName actualSpeed,
-               Hallsensor *hall);
-
-    /** Set the speed of the motor with a pwm for 10%..90%
-    *  50% PWM is 0rpm
-    *  Caclulate from [1/s] in [1/min] and the Factor of the ESCON
-    * @param speed The speed of the motor as a normalised value in [1/s]
-    */
-    void setVelocity(float speed);
-
-    /**Return the speed from ESCON
-    *
-    * Analog input 1.65V = 0 rpm
-    *
-    * @return speed of the motor [1/s]
-    */
-    float getActualSpeed(void);
-
-    /** Set the period of the pwm duty cycle.
-    *
-    * Wrapper for PwmOut::period()
-    *
-    * @param period Pwm duty cycle in seconds.
-    */
-    void period(float period);
-
-    /** Set the Motor to a enable sate
-    *
-    * @param enb <code>0</code> for disable <code>1</code> for enable.
-    */
-    void enable (bool enb);
-
-    /**Tests if the servo drive is enabled.
-    *
-    * @return <code>true</code> if the drive is enabled, <code>false</code> otherwise.
-    */
-    bool isEnabled(void);
-
-    /** Return the translativ Position
-    *
-    * @return position in meter
-    */
-    int getPulses(void);
-
-    /** Set the Pulses of the Motor
-    *
-    * @return number of turns
-    */
-    int setPulses(int setPos);
-};
-
-#endif

--- a/RobotControl/MotionState.h	Sun Mar 03 16:26:47 2013 +0000
+++ b/RobotControl/MotionState.h	Thu Mar 07 09:47:07 2013 +0000
@@ -1,7 +1,6 @@
 #ifndef _MOTION_STATE_H_
 #define _MOTION_STATE_H_
 
-#include <cmath>
 #include "defines.h"
 
 /**
@@ -14,7 +13,11 @@
  *
  * @section DESCRIPTION
  *
- * ?????
+ * This help class is for calculate and save the actual or desired value.
+ * There have the setter and the getter methode to change the value
+ * Is also possible to limit the translational and rotational speed
+ * by the value acceleration and thetaAcceleration. Therefore
+ * can adjust the ramp.
  *
  */
 class MotionState
@@ -31,9 +34,9 @@
     float xposition;
     /** The yposition value of this motion state. */
     float yposition;
-    /** The angle of this motion state. */
+    /** The &theta; of this motion state. */
     float theta;
-    /** The angle of this motion state from the compass. */      
+    /** The &theta; of this motion state from the compass. */      
     float thetaCompass;
     /** The speed value of this motion state. */
     float speed;
@@ -48,11 +51,11 @@
 
     /**
     * Creates a <code>MotionState</code> object with given values for position and speed.
-    * @param xposition the initial position value of this motion state, given in [m].
-    * @param yposition the initial position value of this motion state, given in [m].
-    * @param theta the initial angle value of this motion state, given in [rad].
-    * @param speed the initial speed value of this motion state, given in [m/s].
-    * @param omega the initial &omega; speed value of this motion state, given in [rad/s].
+    * @param xposition the initial position value of this motion state, given in [m]
+    * @param yposition the initial position value of this motion state, given in [m]
+    * @param theta the initial &theta; value of this motion state, given in [rad]
+    * @param speed the initial speed value of this motion state, given in [m/s]
+    * @param omega the initial &omega; speed value of this motion state, given in [rad/s]
     */
     MotionState(float xposition, float yposition, float theta, float speed, float omega);
 
@@ -62,102 +65,102 @@
     virtual     ~MotionState();
 
     /**
-    * Sets the values for xPosition, yPostion and angle.
-    * @param xposition the initial position value of this motion state, given in [m].
-    * @param yposition the initial position value of this motion state, given in [m].
-    * @param theta the initial angle value of this motion state, given in [rad].
-    * @param speed the initial speed value of this motion state, given in [m/s].
-    * @param omega the initial &omega; speed value of this motion state, given in [rad/s].
+    * Sets the values for xPosition, yPostion and &theta;.
+    * @param xposition the initial position value of this motion state, given in [m]
+    * @param yposition the initial position value of this motion state, given in [m]
+    * @param theta the initial &theta; value of this motion state, given in [rad]
+    * @param speed the initial speed value of this motion state, given in [m/s]
+    * @param omega the initial &omega; speed value of this motion state, given in [rad/s]
     */
     void        setState(float xposition, float yposition, float theta, float speed, float omega);
 
     /**
-    * Sets the values for xPosition, yPostion and angle.
-    * @param motionState another <code>MotionState</code> object to copy the state values from.
+    * Sets the values for X-Position, Y-Postion and &theta;.
+    * @param motionState another <code>MotionState</code> object to copy the state values from
     */
     void        setState(MotionState* motionState);
 
     /**
     * Sets the X-position value.
-    * @param xposition the desired xposition value of this motion state, given in [m].
+    * @param xposition the desired xposition value of this motion state, given in [m]
     */
     void        setxPosition(float xposition);
 
     /**
     * Gets the X-position value.
-    * @return the xposition value of this motion state, given in [m].
+    * @return the xposition value of this motion state, given in [m]
     */
     float       getxPosition();
 
     /**
     * Sets the Y-position value.
-    * @param yposition the desired yposition value of this motion state, given in [m].
+    * @param yposition the desired yposition value of this motion state, given in [m]
     */
     void        setyPosition(float yposition);
 
     /**
     * Gets the Y-position value.
-    * @return the xposition value of this motion state, given in [m].
+    * @return the xposition value of this motion state, given in [m]
     */
     float       getyPosition();
 
     /**
-    * Sets the theta value.
-    * @param theta the desired theta value of this motion state, given in [m].
+    * Sets the &theta; value.
+    * @param theta the desired &theta; value of this motion state, given in [rad]
     */
     void        setTheta(float theta);
 
     /**
-    * Gets the angle value.
-    * @return the theta value of this motion state, given in [rad].
+    * Gets the &theta; value.
+    * @return the &theta; value of this motion state, given in [rad]
     */
     float       getTheta();
 
     /**
     * Sets the speed value.
-    * @param speed the desired speed value of this motion state, given in [m/s].
+    * @param speed the desired speed value of this motion state, given in [m/s]
     */
     void        setSpeed(float speed);
 
     /**
     * Gets the speed value.
-    * @return the speed value of this motion state, given in [m/s].
+    * @return the speed value of this motion state, given in [m/s]
     */
     float       getSpeed();
 
     /**
     * Sets the &omega; value.
-    * @param omega the desired &omega; value of this motion state, given in [rad/s].
+    * @param omega the desired &omega; value of this motion state, given in [rad/s]
     */
     void        setOmega(float omega);
 
     /**
     * Gets the &omega; value.
-    * @return the &omega; value of this motion state, given in [rad/s].
+    * @return the &omega; value of this motion state, given in [rad/s]
     */
     float       getOmega();
 
     /**
     * Sets the maximum acceleration value.
-    * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
+    * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>]
     */
     void        setAcceleration(float acceleration);
 
     /**
     * Gets the maximum acceleration value.
-    * @return the maximum acceleration value used for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
+    * @return the maximum acceleration value used for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>]
     */
     float       getAcceleration();
 
     /**
     * Sets the maximum acceleration value of rotation.
-    * @param thetaAcceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>].
+    * @param thetaAcceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>]
     */
     void        setThetaAcceleration(float thetaAcceleration);
 
     /**
     * Gets the maximum acceleration value of rotation.
-    * @return the maximum acceleration value used for the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>].
+    * @return the maximum acceleration value used for the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>]
     */
     float       getThetaAcceleration();
 
@@ -165,7 +168,7 @@
     * Increments the current motion state towards a given desired speed.
     * @param desiredSpeed the desired speed given in [m/s].
     * @param desiredOmega the desired &omega; given in [rad/s].
-    * @param period the time period to increment the motion state for, given in [s].
+    * @param period the time period to increment the motion state for, given in [s]
     */
     void        increment(float desiredSpeed, float desiredOmega, float period);
 };

--- a/RobotControl/RobotControl.cpp	Sun Mar 03 16:26:47 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Thu Mar 07 09:47:07 2013 +0000
@@ -145,7 +145,8 @@
 
 float RobotControl::getThetaErrorToGoal()
 {
-    float temp;
+    return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
+    /*float temp;
     temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta();
 
     if(temp <= -PI) {
@@ -155,14 +156,14 @@
     } else {
         //nothing
     }
-    return temp;
+    return temp;*/
 }
 
 float RobotControl::getThetaGoal()
 {
-    float temp;
-    temp = atan2(getyPositionError(),getxPositionError()) - getTheta();
+    return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta());
 
+    /*
     if(temp <= -PI) {
         temp  += 2*  PI;
     } else if (temp > PI) {
@@ -170,7 +171,7 @@
     } else {
         //nothing
     }
-    return temp;
+    return temp;*/
 }
 
 void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta)
@@ -214,14 +215,16 @@
 
     /* rotational theta of the Robot integrate the omega with the time*/
     Actual.theta += Actual.omega * period;
-    if(Actual.theta <= -PI) {
-        Actual.theta  += 2*  PI;
-    } else if (Actual.theta > PI) {
-        Actual.theta  -= 2*  PI;
-    } else {
-        //nothing
-    }
-
+    Actual.theta = PiRange(Actual.theta);
+    /*
+        if(Actual.theta <= -PI) {
+            Actual.theta  += 2*  PI;
+        } else if (Actual.theta > PI) {
+            Actual.theta  -= 2*  PI;
+        } else {
+            //nothing
+        }
+    */
     /* translational X and Y Position. integrate the speed with the time */
     Actual.xposition += (Actual.speed * period * cos(Actual.theta));
     Actual.yposition += (Actual.speed * period * sin(Actual.theta));
@@ -250,3 +253,15 @@
 
     }
 }
+
+float RobotControl::PiRange(float theta)
+{
+    if(theta <= -PI) {
+        return theta  += 2*PI;
+    } else if (theta > PI) {
+        return theta  -= 2*PI;
+    } else {
+        return theta;
+    }
+}
+

--- a/RobotControl/RobotControl.h	Sun Mar 03 16:26:47 2013 +0000
+++ b/RobotControl/RobotControl.h	Thu Mar 07 09:47:07 2013 +0000
@@ -1,7 +1,6 @@
 #ifndef _ROBOT_CONTROL_H_
 #define _ROBOT_CONTROL_H_
 
-#include <cmath>
 #include "mbed.h"
 #include "MaxonESCON.h"
 #include "MotionState.h"
@@ -20,13 +19,13 @@
  *
  * @section DESCRIPTION
  *
- * This class controls the position and orientation of the robot. It has
+ * This class controls the position of the robot. It has
  * a run loop that is called periodically. This run loop reads the actual
  * positions of the wheels, calculates the actual position and orientation
- * of the robot, calculates to move the robot,
+ * of the robot, calculates to move the robot
  * and writes these velocity values to the motor servo drives.
  * This class offers methods to enable or disable the controller, and to set
- * the desired translational and rotational speed values of the robot.
+ * the desired x- and y-postion and the &theta; values of the robot.
  */
 
 class RobotControl : public Task
@@ -49,12 +48,12 @@
 public:
 
     /**
-     * Creates a robot control object and initializes all private
+     * Creates a <code>Robot Control</code> object and initializes all private
      * state variables.
-     * @param motorControllerLeft a reference to the servo drive for the left motor.
-     * @param motorControllerRight a reference to the servo drive for the right motor.
+     * @param motorControllerLeft a reference to the servo drive for the left motor
+     * @param motorControllerRight a reference to the servo drive for the right motor
      * @param compass Modul HMC5883L
-     * @param period the sampling period of the run loop of this controller, given in [s].
+     * @param period the sampling period of the run loop of this controller, given in [s]
      */
     RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ float period);
 
@@ -72,152 +71,159 @@
 
     /**
     * Tests if the servo drives of the motors are enabled.
-    * @return <code>true</code> if the drives are enabled, <code>false</code> otherwise.
+    * @return <code>true</code> if the drives are enabled, <code>false</code> otherwise
     */
     bool        isEnabled();
 
     /**
     * Sets the maximum acceleration value.
-    * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
+    * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>]
     */
     void        setAcceleration(float acceleration);
 
     /**
     * Sets the maximum acceleration value of rotation.
-    * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/s<SUP>2</SUP>].
+    * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/s<SUP>2</SUP>]
     */
     void        setThetaAcceleration(float acceleration);
 
     /**
     * Sets the desired translational speed of the robot.
-    * @param speed the desired speed, given in [m/s].
+    * @param speed the desired speed, given in [m/s]
     */
     void        setDesiredSpeed(float speed);
 
     /**
     * Sets the desired rotational speed of the robot.
-    * @param omega the desired rotational speed, given in [rad/s].
+    * @param omega the desired rotational speed, given in [rad/s]
     */
     void        setDesiredOmega(float omega);
 
     /**
     * Sets the desired X-position of the robot.
-    * @param xposition the desired position, given in [m].
+    * @param xposition the desired position, given in [m]
     */
     void        setxPosition(float xposition);
 
     /**
     * Sets the desired Y-position of the robot.
-    * @param yposition the desired position, given in [m].
+    * @param yposition the desired position, given in [m]
     */
     void        setyPosition(float yposition);
 
     /**
-    * Sets the desired angle of the robot.
-    * @param theta the desired angle, given in [rad].
+    * Sets the desired &theta; of the robot.
+    * @param theta the desired &theta;, given in [rad]
     */
     void        setTheta(float theta);
-    
+
     /**
-    * Sets the desired Position and angle.
-    * @param xposition the desired position, given in [m].
-    * @param yposition the desired position, given in [m]. 
-    * @param theta the desired angle, given in [rad].  
+    * Sets the desired Position and &theta;.
+    * @param xposition the desired position, given in [m]
+    * @param yposition the desired position, given in [m]
+    * @param theta the desired &theta;, given in [rad]
     */
     void        setPositionAngle(float xposition, float yposition, float theta);
 
     /**
-     * Gets the desired Theta. Angle of the goal Point.
-     * @return the desired Theta, given in [rad].
+     * Gets the desired &theta; of the goal point.
+     * @return the desired &theta;, given in [rad]
      */
     float       getTheta();
 
     /**
      * Gets the desired translational speed of the robot.
-     * @return the desired speed, given in [m/s].
+     * @return the desired speed, given in [m/s]
      */
     float       getDesiredSpeed();
 
     /**
      * Gets the actual translational speed of the robot.
-     * @return the desired speed, given in [m/s].
+     * @return the desired speed, given in [m/s]
      */
     float       getActualSpeed();
 
     /**
     * Gets the desired rotational speed of the robot.
-    * @return the desired speed, given in [rad/s].
+    * @return the desired speed, given in [rad/s]
     */
     float       getDesiredOmega();
 
     /**
     * Gets the actual rotational speed of the robot.
-    * @return the desired speed, given in [rad/s].
+    * @return the desired speed, given in [rad/s]
     */
     float       getActualOmega();
 
     /**
     * Gets the actual translational X-position of the robot.
-    * @return the actual position, given in [m].
+    * @return the actual position, given in [m]
     */
     float       getxActualPosition();
 
     /**
     * Gets the X-position following error of the robot.
-    * @return the position following error, given in [m].
+    * @return the position following error, given in [m]
     */
     float       getxPositionError();
 
     /**
     * Gets the actual translational Y-position of the robot.
-    * @return the actual position, given in [m].
+    * @return the actual position, given in [m]
     */
     float       getyActualPosition();
 
     /**
     * Gets the Y-position following error of the robot.
-    * @return the position following error, given in [m].
+    * @return the position following error, given in [m]
     */
     float       getyPositionError();
 
     /**
     * Gets the actual orientation of the robot.
-    * @return the orientation, given in [rad].
+    * @return the orientation, given in [rad]
     */
     float       getActualTheta();
 
     /**
     * Gets the orientation following error of the robot.
-    * @return the orientation following error, given in [rad].
+    * @return the orientation following error, given in [rad]
     */
     float       getThetaError();
-    
+
     /**
-    * Gets the Distance to Disired point. Calculate witch pythagoras
-    * @return distance to goal, given in [m].
+    * Gets the distance to disired point. Calculate with pythagoras.
+    * @return distance to goal, given in [m]
     */
     float       getDistanceError();
-    
+
     /**
-    * Gets the Angle ot the pointing vector to the goal right the unicycle axis from actual point
-    * @return theta to goal, given in [rad].
+    * Gets the &theta; ot the pointing vector to the goal right the unicycle axis from actual point.
+    * @return theta to goal, given in [rad]
     */
     float       getThetaErrorToGoal();
-    
+
     /**
-    * Gets the Angle ot the pointing vector to the goal right the unicycle main axis
-    * @return theta from the goal, given in [rad].
+    * Gets the &theta; ot the pointing vector to the goal right the unicycle main axis.
+    * @return theta from the goal, given in [rad]
     */
     float       getThetaGoal();
 
     /**
-    * Set all state to zero
-    * @param xZeroPos Sets the start X-position [m].
-    * @param yZeroPos Sets the start y-position [m].
-    * @param theta Sets the start angle [rad].
+    * Set all state to zero, except the X-position, y-position and &theta;.
+    * @param xZeroPos Sets the start X-position, given in [m]
+    * @param yZeroPos Sets the start y-position, given in [m]
+    * @param theta Sets the start &theta;, given in [rad]
     */
     void        setAllToZero(float xZeroPos, float yZeroPos, float theta);
 
+    /**
+    * Add &plusmn;2&pi; when the range of the radian is over +&pi; or under -&pi;.
+    * @param theta to check the value
+    * @return the value in the range from -&pi; to +&pi;
+    */
+    float        PiRange(float theta);
+
     void        run();
 };
 

--- a/StateDefines/State.h	Sun Mar 03 16:26:47 2013 +0000
+++ b/StateDefines/State.h	Thu Mar 07 09:47:07 2013 +0000
@@ -22,6 +22,7 @@
  *
  * State is the main mechanism for communicating current realtime system state to
  * the rest of the system for logging etc.
+ * 
  */
 
 class State : public Task
@@ -42,16 +43,15 @@
 
 public:
 
-
     /**
      * Creates a robot control object and initializes all private state variables.
-     * @param s struct to read and write the state.
-     * @param robotControl Object to read the state.
-     * @param motorControllerLeft a reference to the servo drive for the left motor.
-     * @param motorControllerRight a reference to the servo drive for the right motor.
+     * @param s struct to read and write the state
+     * @param robotControl Object to read the state
+     * @param motorControllerLeft a reference to the servo drive for the left motor
+     * @param motorControllerRight a reference to the servo drive for the right motor
      * @param compass Modul HMC5883L
      * @param battery Analog Input Battery Voltage input
-     * @param period the sampling period of the run loop of this controller, given in [s].
+     * @param period the sampling period of the run loop of this controller, given in [s]
      */
     State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ AnalogIn* battery, float period);
 
@@ -61,7 +61,7 @@
     virtual     ~State();
     
     /**
-     * Initzialize the File. Open the File plots.txt and set the title at first line
+     * Initzialize the File. Open the File plots.txt and set the title at first line.
      */
     void initPlotFile(void);
 
@@ -73,7 +73,7 @@
     void savePlotText(char text[]);
 
     /**
-    * Close the File
+    * Close the File.
     */
     void closePlotFile(void);
 
@@ -85,19 +85,18 @@
     float readBattery();
 
      /**
-     * Starts the timer from zero
+     * Starts the timer from zero.
      * The timer is for the logging Time.
      */
     void startTimerFromZero();
 
     /**
-    * Save the new state to a new line
+    * Save the new state to a new line.
     */
     void savePlotFile(state_t s);
 
     void        run();
 
-
 private:
 
     void setBatteryBit();

--- a/StateDefines/defines.h	Sun Mar 03 16:26:47 2013 +0000
+++ b/StateDefines/defines.h	Thu Mar 07 09:47:07 2013 +0000
@@ -3,13 +3,13 @@
 
 #include "mbed.h"
 
-//Physical dimensions.
-#define PI         3.141592654f
+// Physical dimensions
+#define PI                    3.141592654f  // Physical dimensions π. 
 
-// Motor  #339282 EC 45 flat 30W and GEAR
-#define POLE_PAIRS            8u            // 8
+// maxon motor #339282 EC 45 flat 30W and GEAR 
+#define POLE_PAIRS            8u            // Number of of pole pairs
 #define GEAR                  1.0f          // Gear on the motor
-#define PULSES_PER_STEP       6u            // 6 step for Hallsensor
+#define PULSES_PER_STEP       6u            // Pulses per step Hallsensor have 6 steps
 
 // Physical Dimension of the car
 #define WHEEL_RADIUS          0.042f        // radius of the wheel, given in [m]
@@ -21,7 +21,7 @@
 #define STATE_LEFT            4u            // Bit2 = left ESCON in error state
 #define STATE_RIGHT           8u            // Bit3 = right ESCON in error state     
 
-// ESCON Dimenstion
+// ESCON Constands
 #define ESCON_SET_FACTOR      100.0f        // Speed Factor how set in the ESCON
 #define ESCON_GET_FACTOR      100.0f        // Speed Factor how get in the ESCON       
 
@@ -33,12 +33,11 @@
 #define ACCELERATION          0.25f         // maximum translational acceleration, given in [m/s2]
 #define THETA_ACCELERATION    1.0f          // maximum rotational acceleration, given in [rad/s2]
 
-// Gains of the position controller
+// position controller
 #define GAIN                  0.30f         // Main Gain
 #define K1                    1.0f * GAIN
 #define K2                    3.0f * GAIN
 #define K3                    2.0f * GAIN
-
 #define MIN_DISTANCE_ERROR 0.03             // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m]                 
 
 // LiPo Batterie

--- a/Task/Task.cpp	Sun Mar 03 16:26:47 2013 +0000
+++ b/Task/Task.cpp	Thu Mar 07 09:47:07 2013 +0000
@@ -1,31 +1,31 @@
-#include "Task.h"
-
-Task::Task(float period)
-{
-    this->period = period;
-}
-
-Task::~Task()
-{
-
-}
-
-float Task::getPeriod()
-{
-    return period;
-}
-
-void Task::start()
-{
-    ticker.attach(this, &Task::run, period);
-}
-
-void Task::stop()
-{
-    ticker.detach();
-}
-
-void Task::run()
-{
-
-}
+#include "Task.h"
+
+Task::Task(float period)
+{
+    this->period = period;
+}
+
+Task::~Task()
+{
+
+}
+
+float Task::getPeriod()
+{
+    return period;
+}
+
+void Task::start()
+{
+    ticker.attach(this, &Task::run, period);
+}
+
+void Task::stop()
+{
+    ticker.detach();
+}
+
+void Task::run()
+{
+
+}

--- a/main.cpp	Sun Mar 03 16:26:47 2013 +0000
+++ b/main.cpp	Thu Mar 07 09:47:07 2013 +0000
@@ -1,4 +1,5 @@
 /**
+ * @file main.cpp
  * @author Christian Burri
  *
  * @section LICENSE
@@ -8,7 +9,19 @@
  *
  * @section DESCRIPTION
  *
- * ?????
+ * This Programm is for a autonomous robot for the competition
+ * at the Hochschule Luzern. 
+ * We are one of the 32 teams. In the team #1 is:
+ * - Bauernfeind Julia <B>WI</B> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a> 
+ * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a> 
+ * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a> 
+ * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a> 
+ * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a> 
+ * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a> 
+ * - Burri Christian <B>ET</B> <a href="christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
+ *  
+ * The postition control is based on polar coordiantes. 
+ * For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
  *
  */