Revision 0:958e1e10e536, committed 2011-12-03

Comitter:

Yo_Robot

Date:

Sat Dec 03 17:50:55 2011 +0000

Commit message:

version 1.0, but still under test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Log_tv2d.c	Sat Dec 03 17:50:55 2011 +0000
@@ -0,0 +1,22 @@
+/**
+ * @file  TrackVector2D Log.
+ *
+ * @author  e-mail: mecatronica.mid@gmail.com
+ *
+ * 
+ 
+
+ 1.0.0.0	01/11/2011
+ 			-->> TrackVector2D.cpp
+ 			*	Function  obj.moveto( float x, float y ) Tested OK
+ 			*   Function  obj.rotate( float degrees )    Tested OK
+ 			*   Fixed Bug in Rect2Polar( )
+ 			
+
+ 0.7.0.0    26/11/2011
+            -->> TrackVector2D_H
+            *  About 70% Completed
+	    	*  Tested fine for one move
+	    	*  Rotate still needs to be written
+	     
+ */

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TrackVector2D.h	Sat Dec 03 17:50:55 2011 +0000
@@ -0,0 +1,156 @@
+/**
+ * @file   TrackVector2D.h
+ * @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
+ */
+
+#ifndef TRACKVECTOR2D_H
+#define TRACKVECTOR2D_H
+
+#include "mbed.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
+
+#include <complex>
+
+/**@brief
+ * This class keeps track of a "vector" on a 2D plane.
+ * Vector is defined as any point on an XY plane, with a determine direction.
+ * Could be a mobile robot for instance. Distance is unit-less depends on the use.
+ * Angles are internally measured in radiands but the API uses degrees.
+ *
+ * Static Functions are provided for Angle and Coordenates transformations.
+ */
+class TrackVector2D {
+
+public:
+
+    /**@brief default Constructor.
+     * Initializes Matrices to default Values
+     */
+    TrackVector2D();
+
+
+    /**@brief Copies all elements.
+     * @param orig Original "Vector"
+     */
+    TrackVector2D(const TrackVector2D& orig);
+
+
+    /**@brief
+     * Move the "vector" to position x, y in the plane.
+     * Calculates the angle the vector should rotate, and the distance
+     * it should travel. Distance depeneds on metric units of coordenates.
+     * @param x  Coordenate in "X" axis.
+     * @param y  Coordenate in "Y" axis.
+     * @return   1x2 Matrix, [1][1] Distance to travel -no units.
+     *                       [1][2] Rotation Angle in degrees
+     */
+    const Matrix moveto( float x, float y );
+
+
+    /**@brief
+     * Changes the direction of the "vector" to point to a new direction.
+     * Keeps track of ratation. Doesn't return anything because does not travels
+     * any distance, and rotation is already known.
+     * @param degrees Angle in degrees to Rotate
+     */
+    void rotate( float degrees );
+
+
+    /**@brief Returns 3x3 Matrix Rotation
+     *
+     * @return 3x3 Matrix Rotation only
+     */
+    const Matrix GetRotM();
+
+
+    /**@brief Retirns 4x4 Full Transformation Matrix
+     *
+     * @return 4x4 Full Transfrmation Matrix.
+     */
+    const Matrix GetTrasfM();
+
+
+    /**@brief Returns 1x2 Matrix last [ X ][ Y ] coordenates
+     * @return 1x2 with last only Cordenates.
+     */
+    const Matrix GetLastMove();
+
+
+    /**@brief Retruns nx2 Matrix with all n-Moves.
+     * @return Large nx2 Matrix with all moves done by "vector"
+     */
+    const Matrix GetMove( int index );
+
+    /**@brief Cuerrent X position of Robot
+     * @return X position in Coordenate Sistem.
+     */
+    float getX();
+
+
+    /**@brief Current Y position of Robot
+     * @return Y position in Coordenate Sistem
+     */
+    float getY();
+
+
+    /**@brief Current direction with respect to "X" axis.
+     * @return Angle in radians
+     */
+    float getRad();
+
+
+    /**@brief Current direction with respect to "X" axis.
+     * @return Angle in Degrees
+     */
+    float getDeg();
+
+
+    /**@brief Transform from RADIANS to DEGREES.
+     * @param angle in radians
+     * @return angle in degrees
+     */
+    static float rad2deg( float angle );
+
+
+    /**@brief Transform from DEGREES to RADIANS.
+     * @param angle in degrees
+     * @return angle in radians
+     */
+    static float deg2rad( float angle );
+
+
+    /**@brief Transforms from Rectangular to Polar coordenates.
+     * @param 1x2 Matrix [x][y]
+     * @return 1x2 Matrix [Magnitude][Direction]  in Degrees!
+     */
+    static const Matrix Rect2Polar( const Matrix& Coord );
+
+
+    /**@brief Transforms from Polar to Rectangular coordenates.
+     * @param 1x2 Matrix [Magnitude][Direction] in Degrees!
+     * @return 1x2 Matrix [x][y]
+     */
+    static const Matrix Polar2Rect( const Matrix& Coord );
+
+
+private:
+
+    Matrix _TrnsfM;
+    Matrix _InvM;
+    Matrix _RotM;
+    vector< complex<float> > _Moves; 
+
+    int   _nMoves;
+    float _angRad;
+    float _angDeg;
+    float _distance;
+};
+
+#endif  /* TRACKVECTOR2D_H */