This matrix will be use to control the position and direction of a mobile robot in a 2D enviroment. See header file for more info
Dependents: Kinematics Matrix_class
TrackVector2D.h@0:958e1e10e536, 2011-12-03 (annotated)
- Committer:
- Yo_Robot
- Date:
- Sat Dec 03 17:50:55 2011 +0000
- Revision:
- 0:958e1e10e536
version 1.0, but still under test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yo_Robot | 0:958e1e10e536 | 1 | /** |
Yo_Robot | 0:958e1e10e536 | 2 | * @file TrackVector2D.h |
Yo_Robot | 0:958e1e10e536 | 3 | * @author Ernesto Palacios |
Yo_Robot | 0:958e1e10e536 | 4 | * |
Yo_Robot | 0:958e1e10e536 | 5 | * @brief |
Yo_Robot | 0:958e1e10e536 | 6 | * Develop Under GPL v3.0 License |
Yo_Robot | 0:958e1e10e536 | 7 | * http://www.gnu.org/licenses/gpl-3.0.html . |
Yo_Robot | 0:958e1e10e536 | 8 | * |
Yo_Robot | 0:958e1e10e536 | 9 | * Created on 22 de octubre de 2011, 11:26 PM |
Yo_Robot | 0:958e1e10e536 | 10 | */ |
Yo_Robot | 0:958e1e10e536 | 11 | |
Yo_Robot | 0:958e1e10e536 | 12 | #ifndef TRACKVECTOR2D_H |
Yo_Robot | 0:958e1e10e536 | 13 | #define TRACKVECTOR2D_H |
Yo_Robot | 0:958e1e10e536 | 14 | |
Yo_Robot | 0:958e1e10e536 | 15 | #include "mbed.h" |
Yo_Robot | 0:958e1e10e536 | 16 | #include "Matrix.h" |
Yo_Robot | 0:958e1e10e536 | 17 | #include "MatrixMath.h" |
Yo_Robot | 0:958e1e10e536 | 18 | |
Yo_Robot | 0:958e1e10e536 | 19 | #include <complex> |
Yo_Robot | 0:958e1e10e536 | 20 | |
Yo_Robot | 0:958e1e10e536 | 21 | /**@brief |
Yo_Robot | 0:958e1e10e536 | 22 | * This class keeps track of a "vector" on a 2D plane. |
Yo_Robot | 0:958e1e10e536 | 23 | * Vector is defined as any point on an XY plane, with a determine direction. |
Yo_Robot | 0:958e1e10e536 | 24 | * Could be a mobile robot for instance. Distance is unit-less depends on the use. |
Yo_Robot | 0:958e1e10e536 | 25 | * Angles are internally measured in radiands but the API uses degrees. |
Yo_Robot | 0:958e1e10e536 | 26 | * |
Yo_Robot | 0:958e1e10e536 | 27 | * Static Functions are provided for Angle and Coordenates transformations. |
Yo_Robot | 0:958e1e10e536 | 28 | */ |
Yo_Robot | 0:958e1e10e536 | 29 | class TrackVector2D { |
Yo_Robot | 0:958e1e10e536 | 30 | |
Yo_Robot | 0:958e1e10e536 | 31 | public: |
Yo_Robot | 0:958e1e10e536 | 32 | |
Yo_Robot | 0:958e1e10e536 | 33 | /**@brief default Constructor. |
Yo_Robot | 0:958e1e10e536 | 34 | * Initializes Matrices to default Values |
Yo_Robot | 0:958e1e10e536 | 35 | */ |
Yo_Robot | 0:958e1e10e536 | 36 | TrackVector2D(); |
Yo_Robot | 0:958e1e10e536 | 37 | |
Yo_Robot | 0:958e1e10e536 | 38 | |
Yo_Robot | 0:958e1e10e536 | 39 | /**@brief Copies all elements. |
Yo_Robot | 0:958e1e10e536 | 40 | * @param orig Original "Vector" |
Yo_Robot | 0:958e1e10e536 | 41 | */ |
Yo_Robot | 0:958e1e10e536 | 42 | TrackVector2D(const TrackVector2D& orig); |
Yo_Robot | 0:958e1e10e536 | 43 | |
Yo_Robot | 0:958e1e10e536 | 44 | |
Yo_Robot | 0:958e1e10e536 | 45 | /**@brief |
Yo_Robot | 0:958e1e10e536 | 46 | * Move the "vector" to position x, y in the plane. |
Yo_Robot | 0:958e1e10e536 | 47 | * Calculates the angle the vector should rotate, and the distance |
Yo_Robot | 0:958e1e10e536 | 48 | * it should travel. Distance depeneds on metric units of coordenates. |
Yo_Robot | 0:958e1e10e536 | 49 | * @param x Coordenate in "X" axis. |
Yo_Robot | 0:958e1e10e536 | 50 | * @param y Coordenate in "Y" axis. |
Yo_Robot | 0:958e1e10e536 | 51 | * @return 1x2 Matrix, [1][1] Distance to travel -no units. |
Yo_Robot | 0:958e1e10e536 | 52 | * [1][2] Rotation Angle in degrees |
Yo_Robot | 0:958e1e10e536 | 53 | */ |
Yo_Robot | 0:958e1e10e536 | 54 | const Matrix moveto( float x, float y ); |
Yo_Robot | 0:958e1e10e536 | 55 | |
Yo_Robot | 0:958e1e10e536 | 56 | |
Yo_Robot | 0:958e1e10e536 | 57 | /**@brief |
Yo_Robot | 0:958e1e10e536 | 58 | * Changes the direction of the "vector" to point to a new direction. |
Yo_Robot | 0:958e1e10e536 | 59 | * Keeps track of ratation. Doesn't return anything because does not travels |
Yo_Robot | 0:958e1e10e536 | 60 | * any distance, and rotation is already known. |
Yo_Robot | 0:958e1e10e536 | 61 | * @param degrees Angle in degrees to Rotate |
Yo_Robot | 0:958e1e10e536 | 62 | */ |
Yo_Robot | 0:958e1e10e536 | 63 | void rotate( float degrees ); |
Yo_Robot | 0:958e1e10e536 | 64 | |
Yo_Robot | 0:958e1e10e536 | 65 | |
Yo_Robot | 0:958e1e10e536 | 66 | /**@brief Returns 3x3 Matrix Rotation |
Yo_Robot | 0:958e1e10e536 | 67 | * |
Yo_Robot | 0:958e1e10e536 | 68 | * @return 3x3 Matrix Rotation only |
Yo_Robot | 0:958e1e10e536 | 69 | */ |
Yo_Robot | 0:958e1e10e536 | 70 | const Matrix GetRotM(); |
Yo_Robot | 0:958e1e10e536 | 71 | |
Yo_Robot | 0:958e1e10e536 | 72 | |
Yo_Robot | 0:958e1e10e536 | 73 | /**@brief Retirns 4x4 Full Transformation Matrix |
Yo_Robot | 0:958e1e10e536 | 74 | * |
Yo_Robot | 0:958e1e10e536 | 75 | * @return 4x4 Full Transfrmation Matrix. |
Yo_Robot | 0:958e1e10e536 | 76 | */ |
Yo_Robot | 0:958e1e10e536 | 77 | const Matrix GetTrasfM(); |
Yo_Robot | 0:958e1e10e536 | 78 | |
Yo_Robot | 0:958e1e10e536 | 79 | |
Yo_Robot | 0:958e1e10e536 | 80 | /**@brief Returns 1x2 Matrix last [ X ][ Y ] coordenates |
Yo_Robot | 0:958e1e10e536 | 81 | * @return 1x2 with last only Cordenates. |
Yo_Robot | 0:958e1e10e536 | 82 | */ |
Yo_Robot | 0:958e1e10e536 | 83 | const Matrix GetLastMove(); |
Yo_Robot | 0:958e1e10e536 | 84 | |
Yo_Robot | 0:958e1e10e536 | 85 | |
Yo_Robot | 0:958e1e10e536 | 86 | /**@brief Retruns nx2 Matrix with all n-Moves. |
Yo_Robot | 0:958e1e10e536 | 87 | * @return Large nx2 Matrix with all moves done by "vector" |
Yo_Robot | 0:958e1e10e536 | 88 | */ |
Yo_Robot | 0:958e1e10e536 | 89 | const Matrix GetMove( int index ); |
Yo_Robot | 0:958e1e10e536 | 90 | |
Yo_Robot | 0:958e1e10e536 | 91 | /**@brief Cuerrent X position of Robot |
Yo_Robot | 0:958e1e10e536 | 92 | * @return X position in Coordenate Sistem. |
Yo_Robot | 0:958e1e10e536 | 93 | */ |
Yo_Robot | 0:958e1e10e536 | 94 | float getX(); |
Yo_Robot | 0:958e1e10e536 | 95 | |
Yo_Robot | 0:958e1e10e536 | 96 | |
Yo_Robot | 0:958e1e10e536 | 97 | /**@brief Current Y position of Robot |
Yo_Robot | 0:958e1e10e536 | 98 | * @return Y position in Coordenate Sistem |
Yo_Robot | 0:958e1e10e536 | 99 | */ |
Yo_Robot | 0:958e1e10e536 | 100 | float getY(); |
Yo_Robot | 0:958e1e10e536 | 101 | |
Yo_Robot | 0:958e1e10e536 | 102 | |
Yo_Robot | 0:958e1e10e536 | 103 | /**@brief Current direction with respect to "X" axis. |
Yo_Robot | 0:958e1e10e536 | 104 | * @return Angle in radians |
Yo_Robot | 0:958e1e10e536 | 105 | */ |
Yo_Robot | 0:958e1e10e536 | 106 | float getRad(); |
Yo_Robot | 0:958e1e10e536 | 107 | |
Yo_Robot | 0:958e1e10e536 | 108 | |
Yo_Robot | 0:958e1e10e536 | 109 | /**@brief Current direction with respect to "X" axis. |
Yo_Robot | 0:958e1e10e536 | 110 | * @return Angle in Degrees |
Yo_Robot | 0:958e1e10e536 | 111 | */ |
Yo_Robot | 0:958e1e10e536 | 112 | float getDeg(); |
Yo_Robot | 0:958e1e10e536 | 113 | |
Yo_Robot | 0:958e1e10e536 | 114 | |
Yo_Robot | 0:958e1e10e536 | 115 | /**@brief Transform from RADIANS to DEGREES. |
Yo_Robot | 0:958e1e10e536 | 116 | * @param angle in radians |
Yo_Robot | 0:958e1e10e536 | 117 | * @return angle in degrees |
Yo_Robot | 0:958e1e10e536 | 118 | */ |
Yo_Robot | 0:958e1e10e536 | 119 | static float rad2deg( float angle ); |
Yo_Robot | 0:958e1e10e536 | 120 | |
Yo_Robot | 0:958e1e10e536 | 121 | |
Yo_Robot | 0:958e1e10e536 | 122 | /**@brief Transform from DEGREES to RADIANS. |
Yo_Robot | 0:958e1e10e536 | 123 | * @param angle in degrees |
Yo_Robot | 0:958e1e10e536 | 124 | * @return angle in radians |
Yo_Robot | 0:958e1e10e536 | 125 | */ |
Yo_Robot | 0:958e1e10e536 | 126 | static float deg2rad( float angle ); |
Yo_Robot | 0:958e1e10e536 | 127 | |
Yo_Robot | 0:958e1e10e536 | 128 | |
Yo_Robot | 0:958e1e10e536 | 129 | /**@brief Transforms from Rectangular to Polar coordenates. |
Yo_Robot | 0:958e1e10e536 | 130 | * @param 1x2 Matrix [x][y] |
Yo_Robot | 0:958e1e10e536 | 131 | * @return 1x2 Matrix [Magnitude][Direction] in Degrees! |
Yo_Robot | 0:958e1e10e536 | 132 | */ |
Yo_Robot | 0:958e1e10e536 | 133 | static const Matrix Rect2Polar( const Matrix& Coord ); |
Yo_Robot | 0:958e1e10e536 | 134 | |
Yo_Robot | 0:958e1e10e536 | 135 | |
Yo_Robot | 0:958e1e10e536 | 136 | /**@brief Transforms from Polar to Rectangular coordenates. |
Yo_Robot | 0:958e1e10e536 | 137 | * @param 1x2 Matrix [Magnitude][Direction] in Degrees! |
Yo_Robot | 0:958e1e10e536 | 138 | * @return 1x2 Matrix [x][y] |
Yo_Robot | 0:958e1e10e536 | 139 | */ |
Yo_Robot | 0:958e1e10e536 | 140 | static const Matrix Polar2Rect( const Matrix& Coord ); |
Yo_Robot | 0:958e1e10e536 | 141 | |
Yo_Robot | 0:958e1e10e536 | 142 | |
Yo_Robot | 0:958e1e10e536 | 143 | private: |
Yo_Robot | 0:958e1e10e536 | 144 | |
Yo_Robot | 0:958e1e10e536 | 145 | Matrix _TrnsfM; |
Yo_Robot | 0:958e1e10e536 | 146 | Matrix _InvM; |
Yo_Robot | 0:958e1e10e536 | 147 | Matrix _RotM; |
Yo_Robot | 0:958e1e10e536 | 148 | vector< complex<float> > _Moves; |
Yo_Robot | 0:958e1e10e536 | 149 | |
Yo_Robot | 0:958e1e10e536 | 150 | int _nMoves; |
Yo_Robot | 0:958e1e10e536 | 151 | float _angRad; |
Yo_Robot | 0:958e1e10e536 | 152 | float _angDeg; |
Yo_Robot | 0:958e1e10e536 | 153 | float _distance; |
Yo_Robot | 0:958e1e10e536 | 154 | }; |
Yo_Robot | 0:958e1e10e536 | 155 | |
Yo_Robot | 0:958e1e10e536 | 156 | #endif /* TRACKVECTOR2D_H */ |