File content as of revision 2:d8e1613dc38b:

#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 &copy; 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;

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<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        setxPosition(float xposition);

    /**
    * Sets the desired Y-position of the robot.
    * @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].
    */
    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].  
    */
    void        setPositionAngle(float xposition, float yposition, float theta);

    /**
     * Gets the desired Theta. Angle 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 witch 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].
    */
    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].
    */
    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].
    */
    void        setAllToZero(float xZeroPos, float yZeroPos, float theta);

    void        run();
};

#endif