Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

RobotControl/RobotControl.h@39:a4fd6206da89, 2013-06-10 (annotated)

Committer:
chrigelburri
Date:
Mon Jun 10 14:40:37 2013 +0000
Revision:
39:a4fd6206da89
Parent:
38:d76e488e725f 
V1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 0:31f7be68e52d 1 #ifndef _ROBOT_CONTROL_H_
chrigelburri 0:31f7be68e52d 2 #define _ROBOT_CONTROL_H_
chrigelburri 0:31f7be68e52d 3
chrigelburri 0:31f7be68e52d 4 #include "MaxonESCON.h"
chrigelburri 0:31f7be68e52d 5 #include "MotionState.h"
chrigelburri 0:31f7be68e52d 6 #include "Task.h"
chrigelburri 0:31f7be68e52d 7 #include "defines.h"
chrigelburri 0:31f7be68e52d 8
chrigelburri 0:31f7be68e52d 9 /**
chrigelburri 0:31f7be68e52d 10 * @author Christian Burri
chrigelburri 0:31f7be68e52d 11 *
chrigelburri 38:d76e488e725f 12 * @copyright Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
chrigelburri 0:31f7be68e52d 13 * All rights reserved.
chrigelburri 0:31f7be68e52d 14 *
chrigelburri 11:775ebb69d5e1 15 * @brief
chrigelburri 0:31f7be68e52d 16 *
chrigelburri 3:92ba0254af87 17 * This class controls the position of the robot. It has
chrigelburri 0:31f7be68e52d 18 * a run loop that is called periodically. This run loop reads the actual
chrigelburri 0:31f7be68e52d 19 * positions of the wheels, calculates the actual position and orientation
chrigelburri 3:92ba0254af87 20 * of the robot, calculates to move the robot
chrigelburri 0:31f7be68e52d 21 * and writes these velocity values to the motor servo drives.
chrigelburri 0:31f7be68e52d 22 * This class offers methods to enable or disable the controller, and to set
chrigelburri 3:92ba0254af87 23 * the desired x- and y-postion and the &theta; values of the robot.
chrigelburri 12:235e318a414f 24 *
chrigelburri 38:d76e488e725f 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
chrigelburri 38:d76e488e725f 26 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
chrigelburri 38:d76e488e725f 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
chrigelburri 38:d76e488e725f 28 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
chrigelburri 38:d76e488e725f 29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
chrigelburri 38:d76e488e725f 30 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
chrigelburri 38:d76e488e725f 31 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
chrigelburri 0:31f7be68e52d 32 */
chrigelburri 0:31f7be68e52d 33
chrigelburri 0:31f7be68e52d 34 class RobotControl : public Task
chrigelburri 0:31f7be68e52d 35 {
chrigelburri 0:31f7be68e52d 36
chrigelburri 0:31f7be68e52d 37 private:
chrigelburri 0:31f7be68e52d 38
chrigelburri 0:31f7be68e52d 39 MaxonESCON* motorControllerLeft;
chrigelburri 0:31f7be68e52d 40 MaxonESCON* motorControllerRight;
chrigelburri 0:31f7be68e52d 41 MotionState Desired;
chrigelburri 0:31f7be68e52d 42 MotionState Actual;
chrigelburri 0:31f7be68e52d 43 MotionState stateLeft;
chrigelburri 0:31f7be68e52d 44 MotionState stateRight;
chrigelburri 0:31f7be68e52d 45 float period;
chrigelburri 0:31f7be68e52d 46 float speed;
chrigelburri 0:31f7be68e52d 47 float omega;
chrigelburri 1:6cd533a712c6 48
chrigelburri 6:48eeb41188dd 49 /**
chrigelburri 11:775ebb69d5e1 50 * @brief Add &plusmn;2&pi; when the range of
chrigelburri 6:48eeb41188dd 51 * the radian is over +&pi; or under -&pi;.
chrigelburri 6:48eeb41188dd 52 * @param theta to check the value
chrigelburri 6:48eeb41188dd 53 * @return the value in the range from -&pi; to +&pi;
chrigelburri 6:48eeb41188dd 54 */
chrigelburri 6:48eeb41188dd 55 float PiRange(float theta);
chrigelburri 6:48eeb41188dd 56
chrigelburri 0:31f7be68e52d 57 public:
chrigelburri 0:31f7be68e52d 58
chrigelburri 0:31f7be68e52d 59 /**
chrigelburri 11:775ebb69d5e1 60 * @brief Creates a <code>Robot Control</code> object and
chrigelburri 6:48eeb41188dd 61 * initializes all private state variables.
chrigelburri 6:48eeb41188dd 62 * @param motorControllerLeft a reference to the servo
chrigelburri 6:48eeb41188dd 63 * drive for the left motor
chrigelburri 6:48eeb41188dd 64 * @param motorControllerRight a reference to the servo
chrigelburri 6:48eeb41188dd 65 * drive for the right motor
chrigelburri 11:775ebb69d5e1 66 * @param period the sampling period of the run loop of
chrigelburri 6:48eeb41188dd 67 * this controller, given in [s]
chrigelburri 0:31f7be68e52d 68 */
chrigelburri 6:48eeb41188dd 69 RobotControl(MaxonESCON* motorControllerLeft,
chrigelburri 11:775ebb69d5e1 70 MaxonESCON* motorControllerRight,
chrigelburri 6:48eeb41188dd 71 float period);
chrigelburri 0:31f7be68e52d 72
chrigelburri 0:31f7be68e52d 73 /**
chrigelburri 11:775ebb69d5e1 74 * @brief Destructor of the Object to destroy the Object.
chrigelburri 6:48eeb41188dd 75 **/
chrigelburri 0:31f7be68e52d 76 virtual ~RobotControl();
chrigelburri 0:31f7be68e52d 77
chrigelburri 0:31f7be68e52d 78 /**
chrigelburri 11:775ebb69d5e1 79 * @brief Enables or disables the servo drives of the motors.
chrigelburri 6:48eeb41188dd 80 * @param enable if <code>true</code> enables the drives,
chrigelburri 6:48eeb41188dd 81 * <code>false</code> otherwise
chrigelburri 6:48eeb41188dd 82 * the servo drives are shut down.
chrigelburri 6:48eeb41188dd 83 */
chrigelburri 0:31f7be68e52d 84 void setEnable(bool enable);
chrigelburri 0:31f7be68e52d 85
chrigelburri 0:31f7be68e52d 86 /**
chrigelburri 11:775ebb69d5e1 87 * @brief Tests if the servo drives of the motors are enabled.
chrigelburri 6:48eeb41188dd 88 * @return <code>true</code> if the drives are enabled,
chrigelburri 6:48eeb41188dd 89 * <code>false</code> otherwise
chrigelburri 6:48eeb41188dd 90 */
chrigelburri 0:31f7be68e52d 91 bool isEnabled();
chrigelburri 0:31f7be68e52d 92
chrigelburri 0:31f7be68e52d 93 /**
chrigelburri 11:775ebb69d5e1 94 * @brief Sets the desired translational speed of the robot.
chrigelburri 6:48eeb41188dd 95 * @param speed the desired speed, given in [m/s]
chrigelburri 6:48eeb41188dd 96 */
chrigelburri 0:31f7be68e52d 97 void setDesiredSpeed(float speed);
chrigelburri 0:31f7be68e52d 98
chrigelburri 0:31f7be68e52d 99 /**
chrigelburri 11:775ebb69d5e1 100 * @brief Sets the desired rotational speed of the robot.
chrigelburri 6:48eeb41188dd 101 * @param omega the desired rotational speed, given in [rad/s]
chrigelburri 6:48eeb41188dd 102 */
chrigelburri 0:31f7be68e52d 103 void setDesiredOmega(float omega);
chrigelburri 0:31f7be68e52d 104
chrigelburri 0:31f7be68e52d 105 /**
chrigelburri 11:775ebb69d5e1 106 * @brief Sets the desired X-position of the robot.
chrigelburri 6:48eeb41188dd 107 * @param xposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 108 */
chrigelburri 5:48a258f6335e 109 void setDesiredxPosition(float xposition);
chrigelburri 0:31f7be68e52d 110
chrigelburri 0:31f7be68e52d 111 /**
chrigelburri 11:775ebb69d5e1 112 * @brief Sets the desired Y-position of the robot.
chrigelburri 6:48eeb41188dd 113 * @param yposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 114 */
chrigelburri 5:48a258f6335e 115 void setDesiredyPosition(float yposition);
chrigelburri 0:31f7be68e52d 116
chrigelburri 0:31f7be68e52d 117 /**
chrigelburri 11:775ebb69d5e1 118 * @brief Sets the desired &theta; of the robot.
chrigelburri 6:48eeb41188dd 119 * @param theta the desired &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 120 */
chrigelburri 5:48a258f6335e 121 void setDesiredTheta(float theta);
chrigelburri 6:48eeb41188dd 122
chrigelburri 6:48eeb41188dd 123 /**
chrigelburri 11:775ebb69d5e1 124 * @brief Get the desired X-position of the robot.
chrigelburri 6:48eeb41188dd 125 * @return xposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 126 */
chrigelburri 5:48a258f6335e 127 float getDesiredxPosition();
chrigelburri 5:48a258f6335e 128
chrigelburri 5:48a258f6335e 129 /**
chrigelburri 11:775ebb69d5e1 130 * @brief Get the desired Y-position of the robot.
chrigelburri 6:48eeb41188dd 131 * @return yposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 132 */
chrigelburri 5:48a258f6335e 133 float getDesiredyPosition();
chrigelburri 5:48a258f6335e 134
chrigelburri 5:48a258f6335e 135 /**
chrigelburri 11:775ebb69d5e1 136 * @brief Get the desired &theta; of the robot.
chrigelburri 6:48eeb41188dd 137 * @return theta the desired &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 138 */
chrigelburri 5:48a258f6335e 139 float getDesiredTheta();
chrigelburri 3:92ba0254af87 140
chrigelburri 1:6cd533a712c6 141 /**
chrigelburri 11:775ebb69d5e1 142 * @brief Sets the desired Position and &theta;.
chrigelburri 6:48eeb41188dd 143 * @param xposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 144 * @param yposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 145 * @param theta the desired &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 146 */
chrigelburri 6:48eeb41188dd 147 void setDesiredPositionAndAngle(float xposition,
chrigelburri 6:48eeb41188dd 148 float yposition,
chrigelburri 6:48eeb41188dd 149 float theta);
chrigelburri 2:d8e1613dc38b 150
chrigelburri 2:d8e1613dc38b 151 /**
chrigelburri 11:775ebb69d5e1 152 * @brief Gets the desired &theta; of the goal point.
chrigelburri 3:92ba0254af87 153 * @return the desired &theta;, given in [rad]
chrigelburri 1:6cd533a712c6 154 */
chrigelburri 1:6cd533a712c6 155 float getTheta();
chrigelburri 0:31f7be68e52d 156
chrigelburri 0:31f7be68e52d 157 /**
chrigelburri 11:775ebb69d5e1 158 * @brief Gets the desired translational speed of the robot.
chrigelburri 3:92ba0254af87 159 * @return the desired speed, given in [m/s]
chrigelburri 0:31f7be68e52d 160 */
chrigelburri 0:31f7be68e52d 161 float getDesiredSpeed();
chrigelburri 0:31f7be68e52d 162
chrigelburri 0:31f7be68e52d 163 /**
chrigelburri 11:775ebb69d5e1 164 * @brief Gets the actual translational speed of the robot.
chrigelburri 3:92ba0254af87 165 * @return the desired speed, given in [m/s]
chrigelburri 0:31f7be68e52d 166 */
chrigelburri 0:31f7be68e52d 167 float getActualSpeed();
chrigelburri 0:31f7be68e52d 168
chrigelburri 0:31f7be68e52d 169 /**
chrigelburri 11:775ebb69d5e1 170 * @brief Gets the desired rotational speed of the robot.
chrigelburri 6:48eeb41188dd 171 * @return the desired speed, given in [rad/s]
chrigelburri 6:48eeb41188dd 172 */
chrigelburri 0:31f7be68e52d 173 float getDesiredOmega();
chrigelburri 0:31f7be68e52d 174
chrigelburri 0:31f7be68e52d 175 /**
chrigelburri 11:775ebb69d5e1 176 * @brief Gets the actual rotational speed of the robot.
chrigelburri 6:48eeb41188dd 177 * @return the desired speed, given in [rad/s]
chrigelburri 6:48eeb41188dd 178 */
chrigelburri 0:31f7be68e52d 179 float getActualOmega();
chrigelburri 0:31f7be68e52d 180
chrigelburri 0:31f7be68e52d 181 /**
chrigelburri 11:775ebb69d5e1 182 * @brief Gets the actual translational X-position of the robot.
chrigelburri 6:48eeb41188dd 183 * @return the actual position, given in [m]
chrigelburri 6:48eeb41188dd 184 */
chrigelburri 0:31f7be68e52d 185 float getxActualPosition();
chrigelburri 0:31f7be68e52d 186
chrigelburri 0:31f7be68e52d 187 /**
chrigelburri 11:775ebb69d5e1 188 * @brief Gets the X-position following error of the robot.
chrigelburri 6:48eeb41188dd 189 * @return the position following error, given in [m]
chrigelburri 6:48eeb41188dd 190 */
chrigelburri 0:31f7be68e52d 191 float getxPositionError();
chrigelburri 0:31f7be68e52d 192
chrigelburri 0:31f7be68e52d 193 /**
chrigelburri 11:775ebb69d5e1 194 * @brief Gets the actual translational Y-position of the robot.
chrigelburri 6:48eeb41188dd 195 * @return the actual position, given in [m]
chrigelburri 6:48eeb41188dd 196 */
chrigelburri 0:31f7be68e52d 197 float getyActualPosition();
chrigelburri 0:31f7be68e52d 198
chrigelburri 0:31f7be68e52d 199 /**
chrigelburri 11:775ebb69d5e1 200 * @brief Gets the Y-position following error of the robot.
chrigelburri 6:48eeb41188dd 201 * @return the position following error, given in [m]
chrigelburri 6:48eeb41188dd 202 */
chrigelburri 0:31f7be68e52d 203 float getyPositionError();
chrigelburri 0:31f7be68e52d 204
chrigelburri 0:31f7be68e52d 205 /**
chrigelburri 11:775ebb69d5e1 206 * @brief Gets the actual orientation of the robot.
chrigelburri 6:48eeb41188dd 207 * @return the orientation, given in [rad]
chrigelburri 6:48eeb41188dd 208 */
chrigelburri 0:31f7be68e52d 209 float getActualTheta();
chrigelburri 0:31f7be68e52d 210
chrigelburri 0:31f7be68e52d 211 /**
chrigelburri 11:775ebb69d5e1 212 * @brief Gets the orientation following error of the robot.
chrigelburri 6:48eeb41188dd 213 * @return the orientation following error, given in [rad]
chrigelburri 6:48eeb41188dd 214 */
chrigelburri 0:31f7be68e52d 215 float getThetaError();
chrigelburri 3:92ba0254af87 216
chrigelburri 1:6cd533a712c6 217 /**
chrigelburri 11:775ebb69d5e1 218 * @brief Gets the distance to disired point. Calculate with pythagoras.
chrigelburri 6:48eeb41188dd 219 * @return distance to goal, given in [m]
chrigelburri 6:48eeb41188dd 220 */
chrigelburri 1:6cd533a712c6 221 float getDistanceError();
chrigelburri 3:92ba0254af87 222
chrigelburri 1:6cd533a712c6 223 /**
chrigelburri 11:775ebb69d5e1 224 * @brief Gets the &theta; ot the pointing vector to the goal right
chrigelburri 6:48eeb41188dd 225 * the unicycle axis from actual point.
chrigelburri 6:48eeb41188dd 226 * @return theta to goal, given in [rad]
chrigelburri 6:48eeb41188dd 227 */
chrigelburri 1:6cd533a712c6 228 float getThetaErrorToGoal();
chrigelburri 3:92ba0254af87 229
chrigelburri 1:6cd533a712c6 230 /**
chrigelburri 11:775ebb69d5e1 231 * @brief Gets the &theta; ot the pointing vector to the goal right the unicycle main axis.
chrigelburri 6:48eeb41188dd 232 * @return theta from the goal, given in [rad]
chrigelburri 6:48eeb41188dd 233 */
chrigelburri 1:6cd533a712c6 234 float getThetaGoal();
chrigelburri 1:6cd533a712c6 235
chrigelburri 0:31f7be68e52d 236 /**
chrigelburri 11:775ebb69d5e1 237 * @brief Set all state to zero, except the X-position, y-position and &theta;.
chrigelburri 6:48eeb41188dd 238 * @param xZeroPos Sets the start X-position, given in [m]
chrigelburri 6:48eeb41188dd 239 * @param yZeroPos Sets the start y-position, given in [m]
chrigelburri 6:48eeb41188dd 240 * @param theta Sets the start &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 241 */
chrigelburri 6:48eeb41188dd 242 void setAllToZero(float xZeroPos,
chrigelburri 6:48eeb41188dd 243 float yZeroPos,
chrigelburri 6:48eeb41188dd 244 float theta);
chrigelburri 0:31f7be68e52d 245
chrigelburri 11:775ebb69d5e1 246 /**
chrigelburri 11:775ebb69d5e1 247 * @brief Run method actualize every period.
chrigelburri 11:775ebb69d5e1 248 */
chrigelburri 0:31f7be68e52d 249 void run();
chrigelburri 0:31f7be68e52d 250 };
chrigelburri 0:31f7be68e52d 251
chrigelburri 0:31f7be68e52d 252 #endif