File content as of revision 38:d76e488e725f:

#ifndef _ROBOT_CONTROL_H_
#define _ROBOT_CONTROL_H_

#include "MaxonESCON.h"
#include "MotionState.h"
#include "Task.h"
#include "defines.h"

/**
 * @author Christian Burri
 *
 * @copyright Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
 * All rights reserved.
 *
 * @brief
 *
 * 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

class RobotControl : public Task
{

private:

    MaxonESCON*         motorControllerLeft;
    MaxonESCON*         motorControllerRight;
    MotionState         Desired;
    MotionState         Actual;
    MotionState         stateLeft;
    MotionState         stateRight;
    float               period;
    float               speed;
    float               omega;

    /**
     * @brief Add ±2π when the range of
     * the radian is over +π or under -π.
     * @param theta to check the value
     * @return the value in the range from -π to +π
     */
    float        PiRange(float theta);

public:

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

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

    /**
     * @brief 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);

    /**
     * @brief 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();

    /**
     * @brief Sets the desired translational speed of the robot.
     * @param speed the desired speed, given in [m/s]
     */
    void        setDesiredSpeed(float speed);

    /**
     * @brief Sets the desired rotational speed of the robot.
     * @param omega the desired rotational speed, given in [rad/s]
     */
    void        setDesiredOmega(float omega);

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

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

    /**
     * @brief Sets the desired &theta; of the robot.
     * @param theta the desired &theta;, given in [rad]
     */
    void        setDesiredTheta(float theta);

    /**
     * @brief Get the desired X-position of the robot.
     * @return xposition the desired position, given in [m]
     */
    float        getDesiredxPosition();

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

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

    /**
     * @brief 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);

    /**
     * @brief Gets the desired &theta; of the goal point.
     * @return the desired &theta;, given in [rad]
     */
    float       getTheta();

    /**
     * @brief Gets the desired translational speed of the robot.
     * @return the desired speed, given in [m/s]
     */
    float       getDesiredSpeed();

    /**
     * @brief Gets the actual translational speed of the robot.
     * @return the desired speed, given in [m/s]
     */
    float       getActualSpeed();

    /**
     * @brief Gets the desired rotational speed of the robot.
     * @return the desired speed, given in [rad/s]
     */
    float       getDesiredOmega();

    /**
     * @brief Gets the actual rotational speed of the robot.
     * @return the desired speed, given in [rad/s]
     */
    float       getActualOmega();

    /**
     * @brief Gets the actual translational X-position of the robot.
     * @return the actual position, given in [m]
     */
    float       getxActualPosition();

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

    /**
     * @brief Gets the actual translational Y-position of the robot.
     * @return the actual position, given in [m]
     */
    float       getyActualPosition();

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

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

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

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

    /**
     * @brief 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();

    /**
     * @brief 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();

    /**
     * @brief 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);

    /**
     * @brief Run method actualize every period.
     */
    void        run();
};

#endif