Ernesto Palacios / TrackVector2D

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?

UserRevisionLine numberNew 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 */