Christian Burri / Mbed 2 deprecated autonomous Robot Android

This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ 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: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

RobotControl/RobotControl.h@6:48eeb41188dd, 2013-03-23 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 23 13:52:48 2013 +0000
Revision:
6:48eeb41188dd
Parent:
5:48a258f6335e
Child:
11:775ebb69d5e1
mit link und rechten Radradius

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 "mbed.h"
chrigelburri 0:31f7be68e52d 5 #include "MaxonESCON.h"
chrigelburri 0:31f7be68e52d 6 #include "MotionState.h"
chrigelburri 0:31f7be68e52d 7 #include "Task.h"
chrigelburri 0:31f7be68e52d 8 #include "HMC5883L.h"
chrigelburri 0:31f7be68e52d 9 #include "HMC6352.h"
chrigelburri 0:31f7be68e52d 10 #include "defines.h"
chrigelburri 0:31f7be68e52d 11
chrigelburri 0:31f7be68e52d 12 /**
chrigelburri 0:31f7be68e52d 13 * @author Christian Burri
chrigelburri 0:31f7be68e52d 14 *
chrigelburri 0:31f7be68e52d 15 * @section LICENSE
chrigelburri 0:31f7be68e52d 16 *
chrigelburri 1:6cd533a712c6 17 * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
chrigelburri 0:31f7be68e52d 18 * All rights reserved.
chrigelburri 0:31f7be68e52d 19 *
chrigelburri 0:31f7be68e52d 20 * @section DESCRIPTION
chrigelburri 0:31f7be68e52d 21 *
chrigelburri 3:92ba0254af87 22 * This class controls the position of the robot. It has
chrigelburri 0:31f7be68e52d 23 * a run loop that is called periodically. This run loop reads the actual
chrigelburri 0:31f7be68e52d 24 * positions of the wheels, calculates the actual position and orientation
chrigelburri 3:92ba0254af87 25 * of the robot, calculates to move the robot
chrigelburri 0:31f7be68e52d 26 * and writes these velocity values to the motor servo drives.
chrigelburri 0:31f7be68e52d 27 * This class offers methods to enable or disable the controller, and to set
chrigelburri 3:92ba0254af87 28 * the desired x- and y-postion and the θ values of the robot.
chrigelburri 0:31f7be68e52d 29 */
chrigelburri 0:31f7be68e52d 30
chrigelburri 0:31f7be68e52d 31 class RobotControl : public Task
chrigelburri 0:31f7be68e52d 32 {
chrigelburri 0:31f7be68e52d 33
chrigelburri 0:31f7be68e52d 34 private:
chrigelburri 0:31f7be68e52d 35
chrigelburri 0:31f7be68e52d 36 MaxonESCON* motorControllerLeft;
chrigelburri 0:31f7be68e52d 37 MaxonESCON* motorControllerRight;
chrigelburri 1:6cd533a712c6 38 // HMC6352* compass;
chrigelburri 0:31f7be68e52d 39 AnalogIn* battery;
chrigelburri 0:31f7be68e52d 40 MotionState Desired;
chrigelburri 0:31f7be68e52d 41 MotionState Actual;
chrigelburri 0:31f7be68e52d 42 MotionState stateLeft;
chrigelburri 0:31f7be68e52d 43 MotionState stateRight;
chrigelburri 0:31f7be68e52d 44 float period;
chrigelburri 0:31f7be68e52d 45 float speed;
chrigelburri 0:31f7be68e52d 46 float omega;
chrigelburri 1:6cd533a712c6 47
chrigelburri 6:48eeb41188dd 48 /**
chrigelburri 6:48eeb41188dd 49 * Add ±2π when the range of
chrigelburri 6:48eeb41188dd 50 * the radian is over +π or under -π.
chrigelburri 6:48eeb41188dd 51 * @param theta to check the value
chrigelburri 6:48eeb41188dd 52 * @return the value in the range from -π to +π
chrigelburri 6:48eeb41188dd 53 */
chrigelburri 6:48eeb41188dd 54 float PiRange(float theta);
chrigelburri 6:48eeb41188dd 55
chrigelburri 0:31f7be68e52d 56 public:
chrigelburri 0:31f7be68e52d 57
chrigelburri 0:31f7be68e52d 58 /**
chrigelburri 6:48eeb41188dd 59 * Creates a <code>Robot Control</code> object and
chrigelburri 6:48eeb41188dd 60 * initializes all private state variables.
chrigelburri 6:48eeb41188dd 61 * @param motorControllerLeft a reference to the servo
chrigelburri 6:48eeb41188dd 62 * drive for the left motor
chrigelburri 6:48eeb41188dd 63 * @param motorControllerRight a reference to the servo
chrigelburri 6:48eeb41188dd 64 * drive for the right motor
chrigelburri 0:31f7be68e52d 65 * @param compass Modul HMC5883L
chrigelburri 6:48eeb41188dd 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 6:48eeb41188dd 70 MaxonESCON*
chrigelburri 6:48eeb41188dd 71 motorControllerRight,
chrigelburri 6:48eeb41188dd 72 /*HMC6352* compass,*/
chrigelburri 6:48eeb41188dd 73 float period);
chrigelburri 0:31f7be68e52d 74
chrigelburri 0:31f7be68e52d 75 /**
chrigelburri 6:48eeb41188dd 76 * Destructor of the Object to destroy the Object.
chrigelburri 6:48eeb41188dd 77 **/
chrigelburri 0:31f7be68e52d 78 virtual ~RobotControl();
chrigelburri 0:31f7be68e52d 79
chrigelburri 0:31f7be68e52d 80 /**
chrigelburri 6:48eeb41188dd 81 * Enables or disables the servo drives of the motors.
chrigelburri 6:48eeb41188dd 82 * @param enable if <code>true</code> enables the drives,
chrigelburri 6:48eeb41188dd 83 * <code>false</code> otherwise
chrigelburri 6:48eeb41188dd 84 * the servo drives are shut down.
chrigelburri 6:48eeb41188dd 85 */
chrigelburri 0:31f7be68e52d 86 void setEnable(bool enable);
chrigelburri 0:31f7be68e52d 87
chrigelburri 0:31f7be68e52d 88 /**
chrigelburri 6:48eeb41188dd 89 * Tests if the servo drives of the motors are enabled.
chrigelburri 6:48eeb41188dd 90 * @return <code>true</code> if the drives are enabled,
chrigelburri 6:48eeb41188dd 91 * <code>false</code> otherwise
chrigelburri 6:48eeb41188dd 92 */
chrigelburri 0:31f7be68e52d 93 bool isEnabled();
chrigelburri 0:31f7be68e52d 94
chrigelburri 0:31f7be68e52d 95 /**
chrigelburri 6:48eeb41188dd 96 * Sets the maximum acceleration value.
chrigelburri 6:48eeb41188dd 97 * @param acceleration the maximum acceleration value to use
chrigelburri 6:48eeb41188dd 98 * for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>]
chrigelburri 6:48eeb41188dd 99 */
chrigelburri 1:6cd533a712c6 100 void setAcceleration(float acceleration);
chrigelburri 0:31f7be68e52d 101
chrigelburri 0:31f7be68e52d 102 /**
chrigelburri 6:48eeb41188dd 103 * Sets the maximum acceleration value of rotation.
chrigelburri 6:48eeb41188dd 104 * @param acceleration the maximum acceleration value
chrigelburri 6:48eeb41188dd 105 * to use for the calculation of the motion trajectory,
chrigelburri 6:48eeb41188dd 106 * given in [rad/s<SUP>2</SUP>]
chrigelburri 6:48eeb41188dd 107 */
chrigelburri 1:6cd533a712c6 108 void setThetaAcceleration(float acceleration);
chrigelburri 0:31f7be68e52d 109
chrigelburri 0:31f7be68e52d 110 /**
chrigelburri 6:48eeb41188dd 111 * Sets the desired translational speed of the robot.
chrigelburri 6:48eeb41188dd 112 * @param speed the desired speed, given in [m/s]
chrigelburri 6:48eeb41188dd 113 */
chrigelburri 0:31f7be68e52d 114 void setDesiredSpeed(float speed);
chrigelburri 0:31f7be68e52d 115
chrigelburri 0:31f7be68e52d 116 /**
chrigelburri 6:48eeb41188dd 117 * Sets the desired rotational speed of the robot.
chrigelburri 6:48eeb41188dd 118 * @param omega the desired rotational speed, given in [rad/s]
chrigelburri 6:48eeb41188dd 119 */
chrigelburri 0:31f7be68e52d 120 void setDesiredOmega(float omega);
chrigelburri 0:31f7be68e52d 121
chrigelburri 0:31f7be68e52d 122 /**
chrigelburri 6:48eeb41188dd 123 * Sets the desired X-position of the robot.
chrigelburri 6:48eeb41188dd 124 * @param xposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 125 */
chrigelburri 5:48a258f6335e 126 void setDesiredxPosition(float xposition);
chrigelburri 0:31f7be68e52d 127
chrigelburri 0:31f7be68e52d 128 /**
chrigelburri 6:48eeb41188dd 129 * Sets the desired Y-position of the robot.
chrigelburri 6:48eeb41188dd 130 * @param yposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 131 */
chrigelburri 5:48a258f6335e 132 void setDesiredyPosition(float yposition);
chrigelburri 0:31f7be68e52d 133
chrigelburri 0:31f7be68e52d 134 /**
chrigelburri 6:48eeb41188dd 135 * Sets the desired &theta; of the robot.
chrigelburri 6:48eeb41188dd 136 * @param theta the desired &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 137 */
chrigelburri 5:48a258f6335e 138 void setDesiredTheta(float theta);
chrigelburri 6:48eeb41188dd 139
chrigelburri 6:48eeb41188dd 140 /**
chrigelburri 6:48eeb41188dd 141 * Get the desired X-position of the robot.
chrigelburri 6:48eeb41188dd 142 * @return xposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 143 */
chrigelburri 5:48a258f6335e 144 float getDesiredxPosition();
chrigelburri 5:48a258f6335e 145
chrigelburri 5:48a258f6335e 146 /**
chrigelburri 6:48eeb41188dd 147 * Get the desired Y-position of the robot.
chrigelburri 6:48eeb41188dd 148 * @return yposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 149 */
chrigelburri 5:48a258f6335e 150 float getDesiredyPosition();
chrigelburri 5:48a258f6335e 151
chrigelburri 5:48a258f6335e 152 /**
chrigelburri 6:48eeb41188dd 153 * Get the desired &theta; of the robot.
chrigelburri 6:48eeb41188dd 154 * @return theta the desired &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 155 */
chrigelburri 5:48a258f6335e 156 float getDesiredTheta();
chrigelburri 3:92ba0254af87 157
chrigelburri 1:6cd533a712c6 158 /**
chrigelburri 6:48eeb41188dd 159 * Sets the desired Position and &theta;.
chrigelburri 6:48eeb41188dd 160 * @param xposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 161 * @param yposition the desired position, given in [m]
chrigelburri 6:48eeb41188dd 162 * @param theta the desired &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 163 */
chrigelburri 6:48eeb41188dd 164 void setDesiredPositionAndAngle(float xposition,
chrigelburri 6:48eeb41188dd 165 float yposition,
chrigelburri 6:48eeb41188dd 166 float theta);
chrigelburri 2:d8e1613dc38b 167
chrigelburri 2:d8e1613dc38b 168 /**
chrigelburri 3:92ba0254af87 169 * Gets the desired &theta; of the goal point.
chrigelburri 3:92ba0254af87 170 * @return the desired &theta;, given in [rad]
chrigelburri 1:6cd533a712c6 171 */
chrigelburri 1:6cd533a712c6 172 float getTheta();
chrigelburri 0:31f7be68e52d 173
chrigelburri 0:31f7be68e52d 174 /**
chrigelburri 0:31f7be68e52d 175 * Gets the desired translational speed of the robot.
chrigelburri 3:92ba0254af87 176 * @return the desired speed, given in [m/s]
chrigelburri 0:31f7be68e52d 177 */
chrigelburri 0:31f7be68e52d 178 float getDesiredSpeed();
chrigelburri 0:31f7be68e52d 179
chrigelburri 0:31f7be68e52d 180 /**
chrigelburri 0:31f7be68e52d 181 * Gets the actual translational speed of the robot.
chrigelburri 3:92ba0254af87 182 * @return the desired speed, given in [m/s]
chrigelburri 0:31f7be68e52d 183 */
chrigelburri 0:31f7be68e52d 184 float getActualSpeed();
chrigelburri 0:31f7be68e52d 185
chrigelburri 0:31f7be68e52d 186 /**
chrigelburri 6:48eeb41188dd 187 * Gets the desired rotational speed of the robot.
chrigelburri 6:48eeb41188dd 188 * @return the desired speed, given in [rad/s]
chrigelburri 6:48eeb41188dd 189 */
chrigelburri 0:31f7be68e52d 190 float getDesiredOmega();
chrigelburri 0:31f7be68e52d 191
chrigelburri 0:31f7be68e52d 192 /**
chrigelburri 6:48eeb41188dd 193 * Gets the actual rotational speed of the robot.
chrigelburri 6:48eeb41188dd 194 * @return the desired speed, given in [rad/s]
chrigelburri 6:48eeb41188dd 195 */
chrigelburri 0:31f7be68e52d 196 float getActualOmega();
chrigelburri 0:31f7be68e52d 197
chrigelburri 0:31f7be68e52d 198 /**
chrigelburri 6:48eeb41188dd 199 * Gets the actual translational X-position of the robot.
chrigelburri 6:48eeb41188dd 200 * @return the actual position, given in [m]
chrigelburri 6:48eeb41188dd 201 */
chrigelburri 0:31f7be68e52d 202 float getxActualPosition();
chrigelburri 0:31f7be68e52d 203
chrigelburri 0:31f7be68e52d 204 /**
chrigelburri 6:48eeb41188dd 205 * Gets the X-position following error of the robot.
chrigelburri 6:48eeb41188dd 206 * @return the position following error, given in [m]
chrigelburri 6:48eeb41188dd 207 */
chrigelburri 0:31f7be68e52d 208 float getxPositionError();
chrigelburri 0:31f7be68e52d 209
chrigelburri 0:31f7be68e52d 210 /**
chrigelburri 6:48eeb41188dd 211 * Gets the actual translational Y-position of the robot.
chrigelburri 6:48eeb41188dd 212 * @return the actual position, given in [m]
chrigelburri 6:48eeb41188dd 213 */
chrigelburri 0:31f7be68e52d 214 float getyActualPosition();
chrigelburri 0:31f7be68e52d 215
chrigelburri 0:31f7be68e52d 216 /**
chrigelburri 6:48eeb41188dd 217 * Gets the Y-position following error of the robot.
chrigelburri 6:48eeb41188dd 218 * @return the position following error, given in [m]
chrigelburri 6:48eeb41188dd 219 */
chrigelburri 0:31f7be68e52d 220 float getyPositionError();
chrigelburri 0:31f7be68e52d 221
chrigelburri 0:31f7be68e52d 222 /**
chrigelburri 6:48eeb41188dd 223 * Gets the actual orientation of the robot.
chrigelburri 6:48eeb41188dd 224 * @return the orientation, given in [rad]
chrigelburri 6:48eeb41188dd 225 */
chrigelburri 0:31f7be68e52d 226 float getActualTheta();
chrigelburri 0:31f7be68e52d 227
chrigelburri 0:31f7be68e52d 228 /**
chrigelburri 6:48eeb41188dd 229 * Gets the orientation following error of the robot.
chrigelburri 6:48eeb41188dd 230 * @return the orientation following error, given in [rad]
chrigelburri 6:48eeb41188dd 231 */
chrigelburri 0:31f7be68e52d 232 float getThetaError();
chrigelburri 3:92ba0254af87 233
chrigelburri 1:6cd533a712c6 234 /**
chrigelburri 6:48eeb41188dd 235 * Gets the distance to disired point. Calculate with pythagoras.
chrigelburri 6:48eeb41188dd 236 * @return distance to goal, given in [m]
chrigelburri 6:48eeb41188dd 237 */
chrigelburri 1:6cd533a712c6 238 float getDistanceError();
chrigelburri 3:92ba0254af87 239
chrigelburri 1:6cd533a712c6 240 /**
chrigelburri 6:48eeb41188dd 241 * Gets the &theta; ot the pointing vector to the goal right
chrigelburri 6:48eeb41188dd 242 * the unicycle axis from actual point.
chrigelburri 6:48eeb41188dd 243 * @return theta to goal, given in [rad]
chrigelburri 6:48eeb41188dd 244 */
chrigelburri 1:6cd533a712c6 245 float getThetaErrorToGoal();
chrigelburri 3:92ba0254af87 246
chrigelburri 1:6cd533a712c6 247 /**
chrigelburri 6:48eeb41188dd 248 * Gets the &theta; ot the pointing vector to the goal right the unicycle main axis.
chrigelburri 6:48eeb41188dd 249 * @return theta from the goal, given in [rad]
chrigelburri 6:48eeb41188dd 250 */
chrigelburri 1:6cd533a712c6 251 float getThetaGoal();
chrigelburri 1:6cd533a712c6 252
chrigelburri 0:31f7be68e52d 253 /**
chrigelburri 6:48eeb41188dd 254 * Set all state to zero, except the X-position, y-position and &theta;.
chrigelburri 6:48eeb41188dd 255 * @param xZeroPos Sets the start X-position, given in [m]
chrigelburri 6:48eeb41188dd 256 * @param yZeroPos Sets the start y-position, given in [m]
chrigelburri 6:48eeb41188dd 257 * @param theta Sets the start &theta;, given in [rad]
chrigelburri 6:48eeb41188dd 258 */
chrigelburri 6:48eeb41188dd 259 void setAllToZero(float xZeroPos,
chrigelburri 6:48eeb41188dd 260 float yZeroPos,
chrigelburri 6:48eeb41188dd 261 float theta);
chrigelburri 0:31f7be68e52d 262
chrigelburri 3:92ba0254af87 263
chrigelburri 0:31f7be68e52d 264 void run();
chrigelburri 0:31f7be68e52d 265 };
chrigelburri 0:31f7be68e52d 266
chrigelburri 0:31f7be68e52d 267 #endif