File content as of revision 5:48a258f6335e:

#ifndef _ROBOT_CONTROL_H_
#define _ROBOT_CONTROL_H_

#include "mbed.h"
#include "MaxonESCON.h"
#include "MotionState.h"
#include "Task.h"
#include "HMC5883L.h"
#include "HMC6352.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 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
 * 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 x- and y-postion and the θ values of the robot.
 */

class RobotControl : public Task
{

private:

    MaxonESCON*         motorControllerLeft;
    MaxonESCON*         motorControllerRight;
//   HMC6352*            compass;
    AnalogIn*           battery;
    MotionState         Desired;
    MotionState         Actual;
    MotionState         stateLeft;
    MotionState         stateRight;
    float               period;
    float               speed;
    float               omega;

public:

    /**
     * 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 compass Modul HMC5883L
     * @param period the sampling period of the run loop of this controller, given in [s]
     */
    RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ float period);

    /**
    * Destructor of the Object to destroy the Object.
    **/
    virtual     ~RobotControl();

    /**
    * Enables or disables the servo drives of the motors.
    * @param enable if <code>true</code> enables the drives, <code>false</code> otherwise
    * the servo drives are shut down.
    */
    void        setEnable(bool enable);

    /**
    * Tests if the servo drives of the motors are enabled.
    * @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>]
    */
    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>]
    */
    void        setThetaAcceleration(float acceleration);

    /**
    * Sets the desired translational speed of the robot.
    * @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]
    */
    void        setDesiredOmega(float omega);

    /**
    * Sets the desired X-position of the robot.
    * @param xposition the desired position, given in [m]
    */
    void        setDesiredxPosition(float xposition);

    /**
    * Sets the desired Y-position of the robot.
    * @param yposition the desired position, given in [m]
    */
    void        setDesiredyPosition(float yposition);

    /**
    * Sets the desired &theta; of the robot.
    * @param theta the desired &theta;, given in [rad]
    */
    void        setDesiredTheta(float theta);
    
        /**
    * Get the desired X-position of the robot.
    * @return xposition the desired position, given in [m]
    */
    float        getDesiredxPosition();

    /**
    * Get the desired Y-position of the robot.
    * @return yposition the desired position, given in [m]
    */
    float        getDesiredyPosition();

    /**
    * Get the desired &theta; of the robot.
    * @return theta the desired &theta;, given in [rad]
    */
    float        getDesiredTheta();

    /**
    * 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        setDesiredPositionAndAngle(float xposition, float yposition, float theta);

    /**
     * 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]
     */
    float       getDesiredSpeed();

    /**
     * Gets the actual translational speed of the robot.
     * @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]
    */
    float       getDesiredOmega();

    /**
    * Gets the actual rotational speed of the robot.
    * @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]
    */
    float       getxActualPosition();

    /**
    * Gets the X-position following error of the robot.
    * @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]
    */
    float       getyActualPosition();

    /**
    * Gets the Y-position following error of the robot.
    * @return the position following error, given in [m]
    */
    float       getyPositionError();

    /**
    * Gets the actual orientation of the robot.
    * @return the orientation, given in [rad]
    */
    float       getActualTheta();

    /**
    * Gets the orientation following error of the robot.
    * @return the orientation following error, given in [rad]
    */
    float       getThetaError();

    /**
    * Gets the distance to disired point. Calculate with pythagoras.
    * @return distance to goal, given in [m]
    */
    float       getDistanceError();

    /**
    * 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 &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, 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();
};

#endif