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
Diff: TrackVector2D.cpp
- Revision:
- 0:958e1e10e536
diff -r 000000000000 -r 958e1e10e536 TrackVector2D.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TrackVector2D.cpp Sat Dec 03 17:50:55 2011 +0000 @@ -0,0 +1,215 @@ +/** + * @file TrackVector2D.cpp + * @author Ernesto Palacios + * + * @brief + * Develop Under GPL v3.0 License + * http://www.gnu.org/licenses/gpl-3.0.html . + * + * Created on 22 de octubre de 2011, 11:26 PM + */ + +#include "TrackVector2D.h" + +#include "mbed.h" +#include "Matrix.h" +#include "MatrixMath.h" + +#define PI 3.1416 + +TrackVector2D::TrackVector2D():_nMoves(0), _angRad(0), _angDeg(0), _distance(0) +{ + _TrnsfM = MatrixMath::Eye( 4 ); //Identity [4,4] Matrix + _InvM = MatrixMath::Eye( 4 ); + _RotM = MatrixMath::Eye( 4 ); + + _Moves.push_back( complex <float>( 0.0, 0.0) ); //First Position is Origin +} + + +///Copy Constructor +TrackVector2D::TrackVector2D(const TrackVector2D& orig) +{ + _TrnsfM = orig._TrnsfM; + _InvM = orig._InvM; + _RotM = orig._RotM; + _Moves = orig._Moves; + + _angRad = orig._angRad; + _angDeg = orig._angDeg; + _distance = orig._distance; + +} + + + +const Matrix TrackVector2D::moveto(float x, float y) +{ + _InvM = MatrixMath::Inv( _TrnsfM ); // Inverse of Last + // Transformation Matrix + + Matrix Position(4,1); // Position Matrix & + Position << x // Position Data + << y + << 0 + << 1; + + Matrix Pos_uv = _InvM * Position; // Target Position. + // P[x; y; z; 1 ] + Matrix Pos_rect( 1,2 ); + Pos_rect << Pos_uv(1,1) << Pos_uv( 2,1 ); + + printf( "\n\nPosition in Rect Coordenates is\n" ); + Pos_rect.print(); + + Matrix Pos_ma; + Pos_ma = TrackVector2D::Rect2Polar( Pos_rect ); //To Polar Coordenates + + printf( "\n\nPosition in Polar Coordenates is\n" ); + Pos_ma.print(); + + + + //*** SEPARATE ROTATION FROM TRANSLATION ***// + + // Origin is not Recursive. Only one origin + Matrix M_Transl = MatrixMath::Transl( x, y, 0 ); + + // Rotation is Recursive + float rotAngle = TrackVector2D::deg2rad( Pos_ma(1,2) ); + _RotM *= MatrixMath::RotZ( rotAngle ); + + // Updated Transformation Matrix + _TrnsfM = M_Transl * _RotM; + + + //*** VALUES FOR THE ROBOT TO TRAVEL ***// + + _angRad += Pos_ma(1,2); // Previous + New Angle + _angDeg = TrackVector2D::rad2deg(_angRad); + _distance = Pos_ma(1,1); + + Pos_ma(1,2) = TrackVector2D::rad2deg( Pos_ma(1,2) ); // To Degrees + + return Pos_ma; + // Matrix with the info for the "vector" to move. + // Pos_ma = [ magnitude, angle ] + +} + +// This function does not return anything because if you're callin' it +// you already know the angle the 'vector' rotated. +void TrackVector2D::rotate(float degrees) +{ + float rad = TrackVector2D::deg2rad( degrees ) ; + _RotM *= MatrixMath::RotZ( rad ); +} + +const Matrix TrackVector2D::GetRotM() { return this->_RotM; } + + +const Matrix TrackVector2D::GetTrasfM() { return this->_TrnsfM; } + + +const Matrix TrackVector2D::GetLastMove() +{ + Matrix last(1,2); + last( 1, 1 ) = _Moves[_Moves.size()].real(); // Parte Real "X" + last( 1, 2 ) = _Moves[_Moves.size()].imag(); // Parte Imaginaria "Y" + return last; +} + +const Matrix TrackVector2D::GetMove( int index) +{ + --index; + Matrix last(1,2); + last( 1, 1 ) = _Moves[ index ].real(); // Parte Real "X" + last( 1, 2 ) = _Moves[ index ].imag(); // Parte Imaginaria "Y" + return last; +} + + +float TrackVector2D::getX() +{ + return _Moves[_Moves.size()].real(); +} + +float TrackVector2D::getY() +{ + return _Moves[_Moves.size()].imag(); +} + +float TrackVector2D::getDeg() { return _angDeg; } + +float TrackVector2D::getRad() { return _angRad; } + + +float TrackVector2D::rad2deg(float angle) +{ + if( angle != 0 ) + return ( angle * 180 ) / PI; + else + return 0; +} + + +float TrackVector2D::deg2rad(float angle) +{ + if( angle != 0 ) + return ( angle / 180 ) * PI; + else + return 0; +} + + +const Matrix TrackVector2D::Rect2Polar(const Matrix& Coord) +{ + Matrix Polar( 1, 2 ); + + float ang, mag, X, Y; + + X = Coord(1,1); + Y = Coord(1,2); + + if( X < 0 && X > -0.01 ) // Discard error caused by PI + X = 0; + + if( Y < 0 && Y > -0.01 ) // Discard error + Y = 0; + + if( X == 0 && Y == 0 ) // Vector at Origin + ang = 0; + + else if( X < 0 && Y == 0) // Along "-X" axis + ang = PI; //180º + + else + ang = atan( Y / X ); // a = atan( y/x ) + + if( X < 0 && Y > 0 ) // Quad II + ang = PI + ang; // Correction for second Quadrant + + if( X < 0 && Y < 0 ) // Quad III + ang = -PI + ang; // Correction for Third Quadrant + + + mag = sqrt( pow(X, 2) + pow(Y,2) ); + ang = TrackVector2D::rad2deg( ang ); + + Polar(1,1) = mag; // Polar[mag, ang] + Polar(1,2) = ang; + + return Polar; +} + +const Matrix TrackVector2D::Polar2Rect(const Matrix& Coord) +{ + Matrix Rect( 1,2 ); + + float angle = TrackVector2D::deg2rad( Coord( 1,2 ) ); + + Rect(1,1) = cos( angle ) * Coord(1,1); + Rect(1,2) = sin( angle ) * Coord(1,1); + + return Rect; +} \ No newline at end of file