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
- Committer:
- Yo_Robot
- Date:
- 2011-12-03
- Revision:
- 0:958e1e10e536
File content as of revision 0:958e1e10e536:
/** * @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; }