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@1:6cd533a712c6, 2013-03-02 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 02 09:39:34 2013 +0000
Revision:
1:6cd533a712c6
Parent:
0:31f7be68e52d
Child:
2:d8e1613dc38b
Pos Regler funktioniert getestet im leerlauf;

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 <cmath>
chrigelburri 0:31f7be68e52d 5 #include "mbed.h"
chrigelburri 0:31f7be68e52d 6 #include "MaxonESCON.h"
chrigelburri 0:31f7be68e52d 7 #include "MotionState.h"
chrigelburri 0:31f7be68e52d 8 #include "Task.h"
chrigelburri 0:31f7be68e52d 9 #include "HMC5883L.h"
chrigelburri 0:31f7be68e52d 10 #include "HMC6352.h"
chrigelburri 0:31f7be68e52d 11 #include "defines.h"
chrigelburri 0:31f7be68e52d 12
chrigelburri 0:31f7be68e52d 13 /**
chrigelburri 0:31f7be68e52d 14 * @author Christian Burri
chrigelburri 0:31f7be68e52d 15 *
chrigelburri 0:31f7be68e52d 16 * @section LICENSE
chrigelburri 0:31f7be68e52d 17 *
chrigelburri 1:6cd533a712c6 18 * Copyright &copy; 2013 HSLU Pren Team #1 Cruising Crêpe
chrigelburri 0:31f7be68e52d 19 * All rights reserved.
chrigelburri 0:31f7be68e52d 20 *
chrigelburri 0:31f7be68e52d 21 * @section DESCRIPTION
chrigelburri 0:31f7be68e52d 22 *
chrigelburri 0:31f7be68e52d 23 * This class controls the position and orientation of the robot. It has
chrigelburri 0:31f7be68e52d 24 * a run loop that is called periodically. This run loop reads the actual
chrigelburri 0:31f7be68e52d 25 * positions of the wheels, calculates the actual position and orientation
chrigelburri 0:31f7be68e52d 26 * of the robot, calculates to move the robot,
chrigelburri 0:31f7be68e52d 27 * and writes these velocity values to the motor servo drives.
chrigelburri 0:31f7be68e52d 28 * This class offers methods to enable or disable the controller, and to set
chrigelburri 0:31f7be68e52d 29 * the desired translational and rotational speed values of the robot.
chrigelburri 0:31f7be68e52d 30 */
chrigelburri 0:31f7be68e52d 31
chrigelburri 0:31f7be68e52d 32 class RobotControl : public Task
chrigelburri 0:31f7be68e52d 33 {
chrigelburri 0:31f7be68e52d 34
chrigelburri 0:31f7be68e52d 35 private:
chrigelburri 0:31f7be68e52d 36
chrigelburri 0:31f7be68e52d 37 MaxonESCON* motorControllerLeft;
chrigelburri 0:31f7be68e52d 38 MaxonESCON* motorControllerRight;
chrigelburri 1:6cd533a712c6 39 // HMC6352* compass;
chrigelburri 0:31f7be68e52d 40 AnalogIn* battery;
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 0:31f7be68e52d 49 float rho; /* Distance to goal [m]*/
chrigelburri 0:31f7be68e52d 50 float gamma; /* Angle of the start position to goal position [rad]*/
chrigelburri 1:6cd533a712c6 51 float delta; /* Angle of the goal postition [rad]*/
chrigelburri 1:6cd533a712c6 52
chrigelburri 1:6cd533a712c6 53 float deltaXPostition; /* differenz of the actual X-value and the desired [m] */
chrigelburri 1:6cd533a712c6 54 float deltaYPostition; /* differenz of the actual X-value and the desired [m] */
chrigelburri 1:6cd533a712c6 55
chrigelburri 0:31f7be68e52d 56
chrigelburri 0:31f7be68e52d 57
chrigelburri 0:31f7be68e52d 58 public:
chrigelburri 0:31f7be68e52d 59
chrigelburri 0:31f7be68e52d 60 /**
chrigelburri 0:31f7be68e52d 61 * Creates a robot control object and initializes all private
chrigelburri 0:31f7be68e52d 62 * state variables.
chrigelburri 0:31f7be68e52d 63 * @param motorControllerLeft a reference to the servo drive for the left motor.
chrigelburri 0:31f7be68e52d 64 * @param motorControllerRight a reference to the servo drive for the right motor.
chrigelburri 0:31f7be68e52d 65 * @param compass Modul HMC5883L
chrigelburri 0:31f7be68e52d 66 * @param period the sampling period of the run loop of this controller, given in [s].
chrigelburri 0:31f7be68e52d 67 */
chrigelburri 1:6cd533a712c6 68 RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ float period);
chrigelburri 0:31f7be68e52d 69
chrigelburri 0:31f7be68e52d 70 /**
chrigelburri 0:31f7be68e52d 71 * Destructor of the Object to destroy the Object.
chrigelburri 0:31f7be68e52d 72 **/
chrigelburri 0:31f7be68e52d 73 virtual ~RobotControl();
chrigelburri 0:31f7be68e52d 74
chrigelburri 0:31f7be68e52d 75 /**
chrigelburri 0:31f7be68e52d 76 * Enables or disables the servo drives of the motors.
chrigelburri 0:31f7be68e52d 77 * @param enable if <code>true</code> enables the drives, <code>false</code> otherwise
chrigelburri 0:31f7be68e52d 78 * the servo drives are shut down.
chrigelburri 0:31f7be68e52d 79 */
chrigelburri 0:31f7be68e52d 80 void setEnable(bool enable);
chrigelburri 0:31f7be68e52d 81
chrigelburri 0:31f7be68e52d 82 /**
chrigelburri 0:31f7be68e52d 83 * Tests if the servo drives of the motors are enabled.
chrigelburri 0:31f7be68e52d 84 * @return <code>true</code> if the drives are enabled, <code>false</code> otherwise.
chrigelburri 0:31f7be68e52d 85 */
chrigelburri 0:31f7be68e52d 86 bool isEnabled();
chrigelburri 0:31f7be68e52d 87
chrigelburri 0:31f7be68e52d 88 /**
chrigelburri 0:31f7be68e52d 89 * Sets the maximum acceleration value.
chrigelburri 1:6cd533a712c6 90 * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
chrigelburri 0:31f7be68e52d 91 */
chrigelburri 1:6cd533a712c6 92 void setAcceleration(float acceleration);
chrigelburri 0:31f7be68e52d 93
chrigelburri 0:31f7be68e52d 94 /**
chrigelburri 0:31f7be68e52d 95 * Sets the maximum acceleration value of rotation.
chrigelburri 1:6cd533a712c6 96 * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/s<SUP>2</SUP>].
chrigelburri 0:31f7be68e52d 97 */
chrigelburri 1:6cd533a712c6 98 void setThetaAcceleration(float acceleration);
chrigelburri 0:31f7be68e52d 99
chrigelburri 0:31f7be68e52d 100 /**
chrigelburri 0:31f7be68e52d 101 * Sets the desired translational speed of the robot.
chrigelburri 0:31f7be68e52d 102 * @param speed the desired speed, given in [m/s].
chrigelburri 0:31f7be68e52d 103 */
chrigelburri 0:31f7be68e52d 104 void setDesiredSpeed(float speed);
chrigelburri 0:31f7be68e52d 105
chrigelburri 0:31f7be68e52d 106 /**
chrigelburri 0:31f7be68e52d 107 * Sets the desired rotational speed of the robot.
chrigelburri 1:6cd533a712c6 108 * @param omega the desired rotational speed, given in [rad/s].
chrigelburri 0:31f7be68e52d 109 */
chrigelburri 0:31f7be68e52d 110 void setDesiredOmega(float omega);
chrigelburri 0:31f7be68e52d 111
chrigelburri 0:31f7be68e52d 112 /**
chrigelburri 0:31f7be68e52d 113 * Sets the desired X- position of the robot.
chrigelburri 1:6cd533a712c6 114 * @param xposition the desired position, given in [m].
chrigelburri 0:31f7be68e52d 115 */
chrigelburri 1:6cd533a712c6 116 void setxPosition(float xposition);
chrigelburri 0:31f7be68e52d 117
chrigelburri 0:31f7be68e52d 118 /**
chrigelburri 0:31f7be68e52d 119 * Sets the desired Y-position of the robot.
chrigelburri 1:6cd533a712c6 120 * @param yposition the desired position, given in [m].
chrigelburri 0:31f7be68e52d 121 */
chrigelburri 1:6cd533a712c6 122 void setyPosition(float yposition);
chrigelburri 0:31f7be68e52d 123
chrigelburri 0:31f7be68e52d 124 /**
chrigelburri 0:31f7be68e52d 125 * Sets the angle of the robot.
chrigelburri 0:31f7be68e52d 126 * @param theta the desired angle, given in [rad].
chrigelburri 0:31f7be68e52d 127 */
chrigelburri 0:31f7be68e52d 128 void setTheta(float theta);
chrigelburri 1:6cd533a712c6 129
chrigelburri 1:6cd533a712c6 130 /**
chrigelburri 1:6cd533a712c6 131 * Gets the desired Theta. Angle of the goal Point.
chrigelburri 1:6cd533a712c6 132 * @return the desired Theta, given in [rad].
chrigelburri 1:6cd533a712c6 133 */
chrigelburri 1:6cd533a712c6 134 float getTheta();
chrigelburri 0:31f7be68e52d 135
chrigelburri 0:31f7be68e52d 136 /**
chrigelburri 0:31f7be68e52d 137 * Gets the desired translational speed of the robot.
chrigelburri 0:31f7be68e52d 138 * @return the desired speed, given in [m/s].
chrigelburri 0:31f7be68e52d 139 */
chrigelburri 0:31f7be68e52d 140 float getDesiredSpeed();
chrigelburri 0:31f7be68e52d 141
chrigelburri 0:31f7be68e52d 142 /**
chrigelburri 0:31f7be68e52d 143 * Gets the actual translational speed of the robot.
chrigelburri 0:31f7be68e52d 144 * @return the desired speed, given in [m/s].
chrigelburri 0:31f7be68e52d 145 */
chrigelburri 0:31f7be68e52d 146 float getActualSpeed();
chrigelburri 0:31f7be68e52d 147
chrigelburri 0:31f7be68e52d 148 /**
chrigelburri 0:31f7be68e52d 149 * Gets the desired rotational speed of the robot.
chrigelburri 0:31f7be68e52d 150 * @return the desired speed, given in [rad/s].
chrigelburri 0:31f7be68e52d 151 */
chrigelburri 0:31f7be68e52d 152 float getDesiredOmega();
chrigelburri 0:31f7be68e52d 153
chrigelburri 0:31f7be68e52d 154 /**
chrigelburri 0:31f7be68e52d 155 * Gets the actual rotational speed of the robot.
chrigelburri 0:31f7be68e52d 156 * @return the desired speed, given in [rad/s].
chrigelburri 0:31f7be68e52d 157 */
chrigelburri 0:31f7be68e52d 158 float getActualOmega();
chrigelburri 0:31f7be68e52d 159
chrigelburri 0:31f7be68e52d 160 /**
chrigelburri 0:31f7be68e52d 161 * Gets the actual translational X-position of the robot.
chrigelburri 0:31f7be68e52d 162 * @return the actual position, given in [m].
chrigelburri 0:31f7be68e52d 163 */
chrigelburri 0:31f7be68e52d 164 float getxActualPosition();
chrigelburri 0:31f7be68e52d 165
chrigelburri 0:31f7be68e52d 166 /**
chrigelburri 0:31f7be68e52d 167 * Gets the X-position following error of the robot.
chrigelburri 0:31f7be68e52d 168 * @return the position following error, given in [m].
chrigelburri 0:31f7be68e52d 169 */
chrigelburri 0:31f7be68e52d 170 float getxPositionError();
chrigelburri 0:31f7be68e52d 171
chrigelburri 0:31f7be68e52d 172 /**
chrigelburri 0:31f7be68e52d 173 * Gets the actual translational Y-position of the robot.
chrigelburri 0:31f7be68e52d 174 * @return the actual position, given in [m].
chrigelburri 0:31f7be68e52d 175 */
chrigelburri 0:31f7be68e52d 176 float getyActualPosition();
chrigelburri 0:31f7be68e52d 177
chrigelburri 0:31f7be68e52d 178 /**
chrigelburri 0:31f7be68e52d 179 * Gets the Y-position following error of the robot.
chrigelburri 0:31f7be68e52d 180 * @return the position following error, given in [m].
chrigelburri 0:31f7be68e52d 181 */
chrigelburri 0:31f7be68e52d 182 float getyPositionError();
chrigelburri 0:31f7be68e52d 183
chrigelburri 0:31f7be68e52d 184 /**
chrigelburri 0:31f7be68e52d 185 * Gets the actual orientation of the robot.
chrigelburri 0:31f7be68e52d 186 * @return the orientation, given in [rad].
chrigelburri 0:31f7be68e52d 187 */
chrigelburri 0:31f7be68e52d 188 float getActualTheta();
chrigelburri 0:31f7be68e52d 189
chrigelburri 0:31f7be68e52d 190 /**
chrigelburri 0:31f7be68e52d 191 * Gets the orientation following error of the robot.
chrigelburri 0:31f7be68e52d 192 * @return the orientation following error, given in [rad].
chrigelburri 0:31f7be68e52d 193 */
chrigelburri 1:6cd533a712c6 194
chrigelburri 0:31f7be68e52d 195 float getThetaError();
chrigelburri 1:6cd533a712c6 196
chrigelburri 1:6cd533a712c6 197 /**
chrigelburri 1:6cd533a712c6 198 * Gets the Distance to Disired point. Calculate witch pythagoras
chrigelburri 1:6cd533a712c6 199 * @return distance to goal, given in [m].
chrigelburri 1:6cd533a712c6 200 */
chrigelburri 1:6cd533a712c6 201 float getDistanceError();
chrigelburri 1:6cd533a712c6 202
chrigelburri 1:6cd533a712c6 203 /**
chrigelburri 1:6cd533a712c6 204 * Gets the Angle ot the pointing vector to the goal right the unicycle axis from actual point
chrigelburri 1:6cd533a712c6 205 * @return theta to goal, given in [rad].
chrigelburri 1:6cd533a712c6 206 */
chrigelburri 1:6cd533a712c6 207 float getThetaErrorToGoal();
chrigelburri 1:6cd533a712c6 208
chrigelburri 1:6cd533a712c6 209 /**
chrigelburri 1:6cd533a712c6 210 * Gets the Angle ot the pointing vector to the goal right the unicycle main axis
chrigelburri 1:6cd533a712c6 211 * @return theta from the goal, given in [rad].
chrigelburri 1:6cd533a712c6 212 */
chrigelburri 1:6cd533a712c6 213 float getThetaGoal();
chrigelburri 1:6cd533a712c6 214
chrigelburri 0:31f7be68e52d 215
chrigelburri 0:31f7be68e52d 216 /**
chrigelburri 0:31f7be68e52d 217 * Set all state to zero
chrigelburri 1:6cd533a712c6 218 * @param xZeroPos Sets the start X-position [m].
chrigelburri 1:6cd533a712c6 219 * @param yZeroPos Sets the start y-position [m].
chrigelburri 1:6cd533a712c6 220 * @param theta Sets the start angle [rad].
chrigelburri 0:31f7be68e52d 221 */
chrigelburri 1:6cd533a712c6 222 void setAllToZero(float xZeroPos, float yZeroPos, float theta);
chrigelburri 0:31f7be68e52d 223
chrigelburri 0:31f7be68e52d 224 void run();
chrigelburri 0:31f7be68e52d 225 };
chrigelburri 0:31f7be68e52d 226
chrigelburri 0:31f7be68e52d 227 #endif