SpaceBall.cpp

00001 #include "SpaceBall.h"
00002 
00003 #define Sleep(x) wait_ms(x*10)
00004 //extern Serial pc;
00005 
00006 ///////////////////////////////////////////////////////////////////////////////
00007 
00008 SpaceBall::SpaceBall ( PinName tx, PinName rx, bool bSpaceOrb ) : _serial ( tx, rx )
00009 {
00010     _bSpaceOrb = bSpaceOrb;
00011     _serial.baud ( 9600 );
00012     
00013     _fScale = 1.0f;
00014 
00015     _axis[0]       = 0;      /* last translational data received */
00016     _axis[1]       = 0;
00017     _axis[2]       = 0;
00018     _axis[3]       = 0;        /* last rotational data received */
00019     _axis[4]       = 0;
00020     _axis[5]       = 0;
00021     _buttons       = 0;       /* current button status */
00022 
00023     _data[0]        = 0;
00024     m_erroroccured  = 0;  /* if set, we've received an error packet or packets */
00025     m_resetoccured  = 0;  /* if set, ball was reset, so have to reinitialize it */
00026     m_spaceball4000 = 0; /* if set, its a Spaceball 4000 */
00027     m_leftymode4000 = 0; /* if set, Spaceball 4000 in "lefty" orientation */
00028 
00029     _mapping = 1;
00030     _escape = false;
00031     _idx = 0;
00032     
00033     _serial.attach ( this, &SpaceBall::SerialISR );
00034 
00035     //Init();  
00036     
00037 };
00038 
00039 ///////////////////////////////////////////////////////////////////////////////
00040 
00041 void SpaceBall::Init()
00042 {
00043     if ( _bSpaceOrb )
00044         InitSO ();
00045     else
00046         InitSB ();
00047 }
00048 
00049 ///////////////////////////////////////////////////////////////////////////////
00050 
00051 void SpaceBall::SerialISR(void)
00052 {
00053     char c;
00054     
00055     while ( _serial.readable() )
00056     {
00057         c = _serial.getc();
00058         
00059         Process ( c );
00060     }
00061 }
00062 
00063 ///////////////////////////////////////////////////////////////////////////////
00064 
00065 void SpaceBall::Process ( char c )
00066 {
00067     if ( _bSpaceOrb )
00068         ProcessSO ( c );
00069     else
00070         ProcessSB ( c );
00071 }
00072 
00073 ///////////////////////////////////////////////////////////////////////////////
00074 
00075 float SpaceBall::GetAxis ( int nAxis ) const
00076 {
00077     float fValue = 0;
00078     
00079     if ( _mapping == CNC )
00080     {
00081         if      ( nAxis == TX ) fValue = _axis[0];
00082         else if ( nAxis == TY ) fValue = _axis[2];
00083         else if ( nAxis == TZ ) fValue = _axis[1];
00084         else if ( nAxis == RX ) fValue = -_axis[0];
00085         else if ( nAxis == RY ) fValue = -_axis[2];
00086         else if ( nAxis == RZ ) fValue = -_axis[1];
00087     }
00088     else if ( _mapping == CNCvert )
00089     {
00090         if      ( nAxis == TX ) fValue = _axis[0];
00091         else if ( nAxis == TY ) fValue = _axis[1];
00092         else if ( nAxis == TZ ) fValue = -_axis[2];
00093         else if ( nAxis == RX ) fValue = -_axis[0];
00094         else if ( nAxis == RY ) fValue = -_axis[1];
00095         else if ( nAxis == RZ ) fValue = -_axis[2];
00096     }
00097     else
00098     {
00099         if      ( nAxis == TX ) fValue = _axis[0];
00100         else if ( nAxis == TY ) fValue = _axis[1];
00101         else if ( nAxis == TZ ) fValue = _axis[2];
00102         else if ( nAxis == RX ) fValue = _axis[0];
00103         else if ( nAxis == RY ) fValue = _axis[1];
00104         else if ( nAxis == RZ ) fValue = _axis[2];
00105     }
00106     
00107     if ( nAxis <= TZ )
00108         fValue *= _fScaleT * _fScale;
00109     else
00110         fValue *= _fScaleR * _fScale;
00111         
00112     return fValue;
00113 }
00114 
00115 ///////////////////////////////////////////////////////////////////////////////
00116 
00117 int SpaceBall::GetAxisRaw ( int nAxis ) const
00118     {
00119         if ( nAxis<0 || nAxis>5 )
00120             return 0;
00121         
00122         return _axis[nAxis];
00123     }
00124     
00125 ///////////////////////////////////////////////////////////////////////////////
00126 ///////////////////////////////////////////////////////////////////////////////
00127 ///////////////////////////////////////////////////////////////////////////////