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.cpp@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.cpp |
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 | #include "TrackVector2D.h" |
Yo_Robot | 0:958e1e10e536 | 13 | |
Yo_Robot | 0:958e1e10e536 | 14 | #include "mbed.h" |
Yo_Robot | 0:958e1e10e536 | 15 | #include "Matrix.h" |
Yo_Robot | 0:958e1e10e536 | 16 | #include "MatrixMath.h" |
Yo_Robot | 0:958e1e10e536 | 17 | |
Yo_Robot | 0:958e1e10e536 | 18 | #define PI 3.1416 |
Yo_Robot | 0:958e1e10e536 | 19 | |
Yo_Robot | 0:958e1e10e536 | 20 | TrackVector2D::TrackVector2D():_nMoves(0), _angRad(0), _angDeg(0), _distance(0) |
Yo_Robot | 0:958e1e10e536 | 21 | { |
Yo_Robot | 0:958e1e10e536 | 22 | _TrnsfM = MatrixMath::Eye( 4 ); //Identity [4,4] Matrix |
Yo_Robot | 0:958e1e10e536 | 23 | _InvM = MatrixMath::Eye( 4 ); |
Yo_Robot | 0:958e1e10e536 | 24 | _RotM = MatrixMath::Eye( 4 ); |
Yo_Robot | 0:958e1e10e536 | 25 | |
Yo_Robot | 0:958e1e10e536 | 26 | _Moves.push_back( complex <float>( 0.0, 0.0) ); //First Position is Origin |
Yo_Robot | 0:958e1e10e536 | 27 | } |
Yo_Robot | 0:958e1e10e536 | 28 | |
Yo_Robot | 0:958e1e10e536 | 29 | |
Yo_Robot | 0:958e1e10e536 | 30 | ///Copy Constructor |
Yo_Robot | 0:958e1e10e536 | 31 | TrackVector2D::TrackVector2D(const TrackVector2D& orig) |
Yo_Robot | 0:958e1e10e536 | 32 | { |
Yo_Robot | 0:958e1e10e536 | 33 | _TrnsfM = orig._TrnsfM; |
Yo_Robot | 0:958e1e10e536 | 34 | _InvM = orig._InvM; |
Yo_Robot | 0:958e1e10e536 | 35 | _RotM = orig._RotM; |
Yo_Robot | 0:958e1e10e536 | 36 | _Moves = orig._Moves; |
Yo_Robot | 0:958e1e10e536 | 37 | |
Yo_Robot | 0:958e1e10e536 | 38 | _angRad = orig._angRad; |
Yo_Robot | 0:958e1e10e536 | 39 | _angDeg = orig._angDeg; |
Yo_Robot | 0:958e1e10e536 | 40 | _distance = orig._distance; |
Yo_Robot | 0:958e1e10e536 | 41 | |
Yo_Robot | 0:958e1e10e536 | 42 | } |
Yo_Robot | 0:958e1e10e536 | 43 | |
Yo_Robot | 0:958e1e10e536 | 44 | |
Yo_Robot | 0:958e1e10e536 | 45 | |
Yo_Robot | 0:958e1e10e536 | 46 | const Matrix TrackVector2D::moveto(float x, float y) |
Yo_Robot | 0:958e1e10e536 | 47 | { |
Yo_Robot | 0:958e1e10e536 | 48 | _InvM = MatrixMath::Inv( _TrnsfM ); // Inverse of Last |
Yo_Robot | 0:958e1e10e536 | 49 | // Transformation Matrix |
Yo_Robot | 0:958e1e10e536 | 50 | |
Yo_Robot | 0:958e1e10e536 | 51 | Matrix Position(4,1); // Position Matrix & |
Yo_Robot | 0:958e1e10e536 | 52 | Position << x // Position Data |
Yo_Robot | 0:958e1e10e536 | 53 | << y |
Yo_Robot | 0:958e1e10e536 | 54 | << 0 |
Yo_Robot | 0:958e1e10e536 | 55 | << 1; |
Yo_Robot | 0:958e1e10e536 | 56 | |
Yo_Robot | 0:958e1e10e536 | 57 | Matrix Pos_uv = _InvM * Position; // Target Position. |
Yo_Robot | 0:958e1e10e536 | 58 | // P[x; y; z; 1 ] |
Yo_Robot | 0:958e1e10e536 | 59 | Matrix Pos_rect( 1,2 ); |
Yo_Robot | 0:958e1e10e536 | 60 | Pos_rect << Pos_uv(1,1) << Pos_uv( 2,1 ); |
Yo_Robot | 0:958e1e10e536 | 61 | |
Yo_Robot | 0:958e1e10e536 | 62 | printf( "\n\nPosition in Rect Coordenates is\n" ); |
Yo_Robot | 0:958e1e10e536 | 63 | Pos_rect.print(); |
Yo_Robot | 0:958e1e10e536 | 64 | |
Yo_Robot | 0:958e1e10e536 | 65 | Matrix Pos_ma; |
Yo_Robot | 0:958e1e10e536 | 66 | Pos_ma = TrackVector2D::Rect2Polar( Pos_rect ); //To Polar Coordenates |
Yo_Robot | 0:958e1e10e536 | 67 | |
Yo_Robot | 0:958e1e10e536 | 68 | printf( "\n\nPosition in Polar Coordenates is\n" ); |
Yo_Robot | 0:958e1e10e536 | 69 | Pos_ma.print(); |
Yo_Robot | 0:958e1e10e536 | 70 | |
Yo_Robot | 0:958e1e10e536 | 71 | |
Yo_Robot | 0:958e1e10e536 | 72 | |
Yo_Robot | 0:958e1e10e536 | 73 | //*** SEPARATE ROTATION FROM TRANSLATION ***// |
Yo_Robot | 0:958e1e10e536 | 74 | |
Yo_Robot | 0:958e1e10e536 | 75 | // Origin is not Recursive. Only one origin |
Yo_Robot | 0:958e1e10e536 | 76 | Matrix M_Transl = MatrixMath::Transl( x, y, 0 ); |
Yo_Robot | 0:958e1e10e536 | 77 | |
Yo_Robot | 0:958e1e10e536 | 78 | // Rotation is Recursive |
Yo_Robot | 0:958e1e10e536 | 79 | float rotAngle = TrackVector2D::deg2rad( Pos_ma(1,2) ); |
Yo_Robot | 0:958e1e10e536 | 80 | _RotM *= MatrixMath::RotZ( rotAngle ); |
Yo_Robot | 0:958e1e10e536 | 81 | |
Yo_Robot | 0:958e1e10e536 | 82 | // Updated Transformation Matrix |
Yo_Robot | 0:958e1e10e536 | 83 | _TrnsfM = M_Transl * _RotM; |
Yo_Robot | 0:958e1e10e536 | 84 | |
Yo_Robot | 0:958e1e10e536 | 85 | |
Yo_Robot | 0:958e1e10e536 | 86 | //*** VALUES FOR THE ROBOT TO TRAVEL ***// |
Yo_Robot | 0:958e1e10e536 | 87 | |
Yo_Robot | 0:958e1e10e536 | 88 | _angRad += Pos_ma(1,2); // Previous + New Angle |
Yo_Robot | 0:958e1e10e536 | 89 | _angDeg = TrackVector2D::rad2deg(_angRad); |
Yo_Robot | 0:958e1e10e536 | 90 | _distance = Pos_ma(1,1); |
Yo_Robot | 0:958e1e10e536 | 91 | |
Yo_Robot | 0:958e1e10e536 | 92 | Pos_ma(1,2) = TrackVector2D::rad2deg( Pos_ma(1,2) ); // To Degrees |
Yo_Robot | 0:958e1e10e536 | 93 | |
Yo_Robot | 0:958e1e10e536 | 94 | return Pos_ma; |
Yo_Robot | 0:958e1e10e536 | 95 | // Matrix with the info for the "vector" to move. |
Yo_Robot | 0:958e1e10e536 | 96 | // Pos_ma = [ magnitude, angle ] |
Yo_Robot | 0:958e1e10e536 | 97 | |
Yo_Robot | 0:958e1e10e536 | 98 | } |
Yo_Robot | 0:958e1e10e536 | 99 | |
Yo_Robot | 0:958e1e10e536 | 100 | // This function does not return anything because if you're callin' it |
Yo_Robot | 0:958e1e10e536 | 101 | // you already know the angle the 'vector' rotated. |
Yo_Robot | 0:958e1e10e536 | 102 | void TrackVector2D::rotate(float degrees) |
Yo_Robot | 0:958e1e10e536 | 103 | { |
Yo_Robot | 0:958e1e10e536 | 104 | float rad = TrackVector2D::deg2rad( degrees ) ; |
Yo_Robot | 0:958e1e10e536 | 105 | _RotM *= MatrixMath::RotZ( rad ); |
Yo_Robot | 0:958e1e10e536 | 106 | } |
Yo_Robot | 0:958e1e10e536 | 107 | |
Yo_Robot | 0:958e1e10e536 | 108 | const Matrix TrackVector2D::GetRotM() { return this->_RotM; } |
Yo_Robot | 0:958e1e10e536 | 109 | |
Yo_Robot | 0:958e1e10e536 | 110 | |
Yo_Robot | 0:958e1e10e536 | 111 | const Matrix TrackVector2D::GetTrasfM() { return this->_TrnsfM; } |
Yo_Robot | 0:958e1e10e536 | 112 | |
Yo_Robot | 0:958e1e10e536 | 113 | |
Yo_Robot | 0:958e1e10e536 | 114 | const Matrix TrackVector2D::GetLastMove() |
Yo_Robot | 0:958e1e10e536 | 115 | { |
Yo_Robot | 0:958e1e10e536 | 116 | Matrix last(1,2); |
Yo_Robot | 0:958e1e10e536 | 117 | last( 1, 1 ) = _Moves[_Moves.size()].real(); // Parte Real "X" |
Yo_Robot | 0:958e1e10e536 | 118 | last( 1, 2 ) = _Moves[_Moves.size()].imag(); // Parte Imaginaria "Y" |
Yo_Robot | 0:958e1e10e536 | 119 | return last; |
Yo_Robot | 0:958e1e10e536 | 120 | } |
Yo_Robot | 0:958e1e10e536 | 121 | |
Yo_Robot | 0:958e1e10e536 | 122 | const Matrix TrackVector2D::GetMove( int index) |
Yo_Robot | 0:958e1e10e536 | 123 | { |
Yo_Robot | 0:958e1e10e536 | 124 | --index; |
Yo_Robot | 0:958e1e10e536 | 125 | Matrix last(1,2); |
Yo_Robot | 0:958e1e10e536 | 126 | last( 1, 1 ) = _Moves[ index ].real(); // Parte Real "X" |
Yo_Robot | 0:958e1e10e536 | 127 | last( 1, 2 ) = _Moves[ index ].imag(); // Parte Imaginaria "Y" |
Yo_Robot | 0:958e1e10e536 | 128 | return last; |
Yo_Robot | 0:958e1e10e536 | 129 | } |
Yo_Robot | 0:958e1e10e536 | 130 | |
Yo_Robot | 0:958e1e10e536 | 131 | |
Yo_Robot | 0:958e1e10e536 | 132 | float TrackVector2D::getX() |
Yo_Robot | 0:958e1e10e536 | 133 | { |
Yo_Robot | 0:958e1e10e536 | 134 | return _Moves[_Moves.size()].real(); |
Yo_Robot | 0:958e1e10e536 | 135 | } |
Yo_Robot | 0:958e1e10e536 | 136 | |
Yo_Robot | 0:958e1e10e536 | 137 | float TrackVector2D::getY() |
Yo_Robot | 0:958e1e10e536 | 138 | { |
Yo_Robot | 0:958e1e10e536 | 139 | return _Moves[_Moves.size()].imag(); |
Yo_Robot | 0:958e1e10e536 | 140 | } |
Yo_Robot | 0:958e1e10e536 | 141 | |
Yo_Robot | 0:958e1e10e536 | 142 | float TrackVector2D::getDeg() { return _angDeg; } |
Yo_Robot | 0:958e1e10e536 | 143 | |
Yo_Robot | 0:958e1e10e536 | 144 | float TrackVector2D::getRad() { return _angRad; } |
Yo_Robot | 0:958e1e10e536 | 145 | |
Yo_Robot | 0:958e1e10e536 | 146 | |
Yo_Robot | 0:958e1e10e536 | 147 | float TrackVector2D::rad2deg(float angle) |
Yo_Robot | 0:958e1e10e536 | 148 | { |
Yo_Robot | 0:958e1e10e536 | 149 | if( angle != 0 ) |
Yo_Robot | 0:958e1e10e536 | 150 | return ( angle * 180 ) / PI; |
Yo_Robot | 0:958e1e10e536 | 151 | else |
Yo_Robot | 0:958e1e10e536 | 152 | return 0; |
Yo_Robot | 0:958e1e10e536 | 153 | } |
Yo_Robot | 0:958e1e10e536 | 154 | |
Yo_Robot | 0:958e1e10e536 | 155 | |
Yo_Robot | 0:958e1e10e536 | 156 | float TrackVector2D::deg2rad(float angle) |
Yo_Robot | 0:958e1e10e536 | 157 | { |
Yo_Robot | 0:958e1e10e536 | 158 | if( angle != 0 ) |
Yo_Robot | 0:958e1e10e536 | 159 | return ( angle / 180 ) * PI; |
Yo_Robot | 0:958e1e10e536 | 160 | else |
Yo_Robot | 0:958e1e10e536 | 161 | return 0; |
Yo_Robot | 0:958e1e10e536 | 162 | } |
Yo_Robot | 0:958e1e10e536 | 163 | |
Yo_Robot | 0:958e1e10e536 | 164 | |
Yo_Robot | 0:958e1e10e536 | 165 | const Matrix TrackVector2D::Rect2Polar(const Matrix& Coord) |
Yo_Robot | 0:958e1e10e536 | 166 | { |
Yo_Robot | 0:958e1e10e536 | 167 | Matrix Polar( 1, 2 ); |
Yo_Robot | 0:958e1e10e536 | 168 | |
Yo_Robot | 0:958e1e10e536 | 169 | float ang, mag, X, Y; |
Yo_Robot | 0:958e1e10e536 | 170 | |
Yo_Robot | 0:958e1e10e536 | 171 | X = Coord(1,1); |
Yo_Robot | 0:958e1e10e536 | 172 | Y = Coord(1,2); |
Yo_Robot | 0:958e1e10e536 | 173 | |
Yo_Robot | 0:958e1e10e536 | 174 | if( X < 0 && X > -0.01 ) // Discard error caused by PI |
Yo_Robot | 0:958e1e10e536 | 175 | X = 0; |
Yo_Robot | 0:958e1e10e536 | 176 | |
Yo_Robot | 0:958e1e10e536 | 177 | if( Y < 0 && Y > -0.01 ) // Discard error |
Yo_Robot | 0:958e1e10e536 | 178 | Y = 0; |
Yo_Robot | 0:958e1e10e536 | 179 | |
Yo_Robot | 0:958e1e10e536 | 180 | if( X == 0 && Y == 0 ) // Vector at Origin |
Yo_Robot | 0:958e1e10e536 | 181 | ang = 0; |
Yo_Robot | 0:958e1e10e536 | 182 | |
Yo_Robot | 0:958e1e10e536 | 183 | else if( X < 0 && Y == 0) // Along "-X" axis |
Yo_Robot | 0:958e1e10e536 | 184 | ang = PI; //180º |
Yo_Robot | 0:958e1e10e536 | 185 | |
Yo_Robot | 0:958e1e10e536 | 186 | else |
Yo_Robot | 0:958e1e10e536 | 187 | ang = atan( Y / X ); // a = atan( y/x ) |
Yo_Robot | 0:958e1e10e536 | 188 | |
Yo_Robot | 0:958e1e10e536 | 189 | if( X < 0 && Y > 0 ) // Quad II |
Yo_Robot | 0:958e1e10e536 | 190 | ang = PI + ang; // Correction for second Quadrant |
Yo_Robot | 0:958e1e10e536 | 191 | |
Yo_Robot | 0:958e1e10e536 | 192 | if( X < 0 && Y < 0 ) // Quad III |
Yo_Robot | 0:958e1e10e536 | 193 | ang = -PI + ang; // Correction for Third Quadrant |
Yo_Robot | 0:958e1e10e536 | 194 | |
Yo_Robot | 0:958e1e10e536 | 195 | |
Yo_Robot | 0:958e1e10e536 | 196 | mag = sqrt( pow(X, 2) + pow(Y,2) ); |
Yo_Robot | 0:958e1e10e536 | 197 | ang = TrackVector2D::rad2deg( ang ); |
Yo_Robot | 0:958e1e10e536 | 198 | |
Yo_Robot | 0:958e1e10e536 | 199 | Polar(1,1) = mag; // Polar[mag, ang] |
Yo_Robot | 0:958e1e10e536 | 200 | Polar(1,2) = ang; |
Yo_Robot | 0:958e1e10e536 | 201 | |
Yo_Robot | 0:958e1e10e536 | 202 | return Polar; |
Yo_Robot | 0:958e1e10e536 | 203 | } |
Yo_Robot | 0:958e1e10e536 | 204 | |
Yo_Robot | 0:958e1e10e536 | 205 | const Matrix TrackVector2D::Polar2Rect(const Matrix& Coord) |
Yo_Robot | 0:958e1e10e536 | 206 | { |
Yo_Robot | 0:958e1e10e536 | 207 | Matrix Rect( 1,2 ); |
Yo_Robot | 0:958e1e10e536 | 208 | |
Yo_Robot | 0:958e1e10e536 | 209 | float angle = TrackVector2D::deg2rad( Coord( 1,2 ) ); |
Yo_Robot | 0:958e1e10e536 | 210 | |
Yo_Robot | 0:958e1e10e536 | 211 | Rect(1,1) = cos( angle ) * Coord(1,1); |
Yo_Robot | 0:958e1e10e536 | 212 | Rect(1,2) = sin( angle ) * Coord(1,1); |
Yo_Robot | 0:958e1e10e536 | 213 | |
Yo_Robot | 0:958e1e10e536 | 214 | return Rect; |
Yo_Robot | 0:958e1e10e536 | 215 | } |