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.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?

UserRevisionLine numberNew 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 }