File content as of revision 0:31f7be68e52d:

#ifndef _ROBOT_CONTROL_H_
#define _ROBOT_CONTROL_H_

#include <cmath>
#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 (c) 2013 HSLU Pren Team #1 Cruising Crêpe
 * All rights reserved.
 *
 * @section DESCRIPTION
 *
 * This class controls the position and orientation 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 translational and rotational speed 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;
    
    float               rho; /* Distance to goal [m]*/
    float               gamma; /* Angle of the start position to goal position [rad]*/
    


public:

    /**
     * Creates a robot control 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&sup2;].
    */
    void        setAcceleration(float acc);

    /**
    * 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&sup2;].
    */
    void        setThetaAcceleration(float acc);

    /**
    * 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 speed the desired speed, given in [rad/s].
    */
    void        setDesiredOmega(float omega);

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

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

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

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

    /**
    * Set all state to zero
    * @param Sets the start X-position [m].
    * @param Sets the start y-position [m].
    */
    void        setAllToZero(float xZeroPos, float xZeroPos);

    void        run();
};

#endif