Revision 0:f67a8fffd94a, committed 2012-12-01

Comitter:

jocis

Date:

Sat Dec 01 05:30:34 2012 +0000

Child:

1:e6282b645d9b

Commit message:

.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpaceBall.cpp	Sat Dec 01 05:30:34 2012 +0000
@@ -0,0 +1,385 @@
+#include "SpaceBall.h"
+
+#define Sleep(x) wait_ms(x*10)
+
+///////////////////////////////////////////////////////////////////////////////
+
+SpaceBall::SpaceBall ( PinName tx, PinName rx, bool bSpaceOrb ) : _serial ( tx, rx )
+{
+    _bSpaceOrb = bSpaceOrb;
+    _serial.baud ( 9600 );
+    
+    _fScale = 1.0f;
+
+    m_axis[0]       = 0;      /* last translational data received */
+    m_axis[1]       = 0;
+    m_axis[2]       = 0;
+    m_axis[3]       = 0;        /* last rotational data received */
+    m_axis[4]       = 0;
+    m_axis[5]       = 0;
+    m_buttons       = 0;       /* current button status */
+
+    m_buf[0]        = 0;
+    m_resetstring[0]= 0; 
+    m_bufpos        = 0;        /* current char position in packet buffer */
+    m_packtype      = 0;      /* what kind of packet is it */
+    m_packlen       = 0;       /* how many bytes do we ultimately expect? */
+    m_escapedchar   = 0;   /* if set, we're processing an escape sequence */
+    m_erroroccured  = 0;  /* if set, we've received an error packet or packets */
+    m_resetoccured  = 0;  /* if set, ball was reset, so have to reinitialize it */
+    m_spaceball4000 = 0; /* if set, its a Spaceball 4000 */
+    m_leftymode4000 = 0; /* if set, Spaceball 4000 in "lefty" orientation */
+    m_timer         = 0;         /* time since last packet was received */
+  
+    _serial.attach ( this, &SpaceBall::SerialISR );
+
+    Init();  
+    
+};
+
+///////////////////////////////////////////////////////////////////////////////
+
+void SpaceBall::Init()
+{
+    m_packlen=1;
+    m_resetoccured=0;
+
+   m_spaceball4000 = 0; /* re-determine which type it is     */
+    m_leftymode4000 = 0; /* re-determine if its in lefty mode */
+
+    if (!m_resetoccured) {
+#if defined(DEBUG)
+        printf("Sending reset command to spaceball...\n");
+#endif
+        m_resetoccured=1;
+        _serial.printf ( "@\r" ); /* force reset */
+        }
+
+    Sleep(10);
+
+#if defined(DEBUG)
+    printf("Sending initialization sequence to spaceball...\n");
+#endif
+
+    _serial.printf ( "CB\r" );
+    Sleep(10);
+    _serial.printf ( "NT\r" );
+    Sleep(10);
+    _serial.printf ( "FTp\r" );
+    Sleep(10);
+    _serial.printf ( "FRp\r" );
+    Sleep(10);
+    _serial.printf ( "P@r@r\r" );
+    Sleep(10);
+    _serial.printf ( "MSSV\r" );
+    Sleep(10);
+    _serial.printf ( "Z\r" );
+    Sleep(10);
+    _serial.printf ( "BcCcC\r" );
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void SpaceBall::SerialISR(void)
+{
+    char c;
+    
+    while ( _serial.readable() )
+    {
+        c = _serial.getc();
+        
+        //printf("%c", c);
+        
+        Process ( c );
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void SpaceBall::Process ( char c )
+{
+    if ( _bSpaceOrb )
+        ProcessSO ( c );
+    else
+        ProcessSB ( c );
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void SpaceBall::ProcessSB ( char c )
+{
+    int i, num, packs;
+    bool bChanged = false;
+
+    packs = 0; /* no packs received yet */
+
+    /* process potentially occuring escaped character sequences */
+    if (c == '^') 
+        {
+        if (!m_escapedchar) 
+            {
+            m_escapedchar = 1;
+            return; /* eat the escape character from buffer */
+            } 
+        }
+
+    if (m_escapedchar) 
+        {
+        m_escapedchar = 0;
+
+        switch (c) 
+            {
+            case '^': /* leave char in buffer unchanged */
+                break;
+
+            case 'Q':
+            case 'S':
+            case 'M':
+                c &= 0x1F; /* convert character to unescaped form */
+                break;
+
+            default:
+                break;
+            }
+        } 
+
+
+    /* figure out what kind of packet we received */
+    if (m_bufpos == 0) 
+        {
+        switch(c) 
+            {
+            case 'D':  /* Displacement packet */
+                m_packtype = 'D'; 
+                m_packlen = 16;    /* D packets are 15 bytes long */
+                break;
+
+            case 'K':  /* Button/Key packet */
+                m_packtype = 'K';
+                m_packlen = 4;     /* K packets are 3 bytes long */
+                break;
+
+            case '.': /* Spaceball 4000 FLX "advanced" button press event */
+                m_packtype = '.';
+                m_packlen = 4;     /* . packets are 3 bytes long */
+                break;
+
+            case 'C': /* Communications mode packet */
+                m_packtype = 'C';
+                m_packlen = 4;
+                break;
+
+            case 'F': /* Spaceball sensitization mode packet */
+                m_packtype = 'F';
+                m_packlen = 4;
+                break;
+
+            case 'M': /* Movement mode packet */
+                m_packtype = 'M';
+                m_packlen = 5;
+                break;
+
+            case 'N': /* Null region packet */
+                m_packtype = 'N';
+                m_packlen = 3;
+                break;
+
+            case 'P': /* Update rate packet */ 
+                m_packtype = 'P';
+                m_packlen = 6;
+                break;
+
+            case '\v': /* XON at poweron */
+                m_packtype = '\v';
+                m_packlen = 1;
+                break;
+
+            case '\n': /* carriage return at poweron */
+            case '\r': /* carriage return at poweron */
+                m_packtype = '\r';
+                m_packlen = 1;
+                break;
+
+            case '@': /* Spaceball Hard/Soft Reset packet */
+                m_resetoccured=1;
+                m_packtype = '@';
+                m_packlen = 62;    /* Resets aren't longer than 62 chars */
+                break;
+
+            case 'E': /* Error packet */
+                m_packtype = 'E';
+                m_packlen = 8;     /* E packets are up to 7 bytes long */
+                break;
+
+            case 'Z': /* Zero packet (Spaceball 2003/3003/4000 FLX) */
+                m_packtype = 'Z';
+                m_packlen = 14;    /* Z packets are hardware dependent */
+                break;
+
+            default:  /* Unknown packet! */
+                return;
+            }
+        }
+
+
+    m_buf[m_bufpos] = c;
+    m_bufpos++;
+
+    /* Reset packet processing */
+    if (m_packtype == '@') 
+        {
+        if (c != '\r')
+            return;
+        else 
+            m_packlen = m_bufpos;
+        }   
+
+    /* Error packet processing */
+    if (m_packtype == 'E') 
+        {
+        if (c != '\r')
+            return;
+        else 
+            m_packlen = m_bufpos;
+        } 
+    else if (m_bufpos != m_packlen)
+        return;
+
+    switch (m_packtype) 
+        {
+        case 'D':  /* ball displacement event */
+            {
+            unsigned int tx, ty, tz, rx, ry, rz;
+
+            /* number of 1/16ths of milliseconds since last */
+            /* ball displacement packet */ 
+            m_timer = ((m_buf[1]) << 8) | (m_buf[2]);
+
+            tx = ((m_buf[ 3]) << 8) | ((m_buf[ 4]));
+            tz = ((m_buf[ 5]) << 8) | ((m_buf[ 6]));
+            ty = ((m_buf[ 7]) << 8) | ((m_buf[ 8]));
+            rx = ((m_buf[ 9]) << 8) | ((m_buf[10]));
+            rz = ((m_buf[11]) << 8) | ((m_buf[12]));
+            ry = ((m_buf[13]) << 8) | ((m_buf[14]));
+
+            m_axis[0] = (((int) tx) << 16) >> 16;
+            m_axis[1] = (((int) ty) << 16) >> 16;
+            m_axis[2] = (((int) tz) << 16) >> 16;
+            m_axis[3] = -(((int) rx) << 16) >> 16;
+            m_axis[4] = -(((int) ry) << 16) >> 16;
+            m_axis[5] = -(((int) rz) << 16) >> 16;
+
+            bChanged = true;
+            DoChangedAxis();
+            }
+            break;
+
+        case 'K': /* button press event */
+            /* Spaceball 2003A, 2003B, 2003 FLX, 3003 FLX, 4000 FLX       */
+            /* button packet. (4000 only for backwards compatibility)     */
+            /* The lowest 5 bits of the first byte are buttons 5-9        */
+            /* Button '8' on a Spaceball 2003 is the rezero button        */
+            /* The lowest 4 bits of the second byte are buttons 1-4       */
+            /* For Spaceball 2003, we'll map the buttons 1-7 normally     */   
+            /* skip 8, as its a hardware "rezero button" on that device   */
+            /* and call the "pick" button "8".                            */
+            /* On the Spaceball 3003, the "right" button also triggers    */
+            /* the "pick" bit.  We OR the 2003/3003 rezero bits together  */
+
+            /* if we have found a Spaceball 4000, then we ignore the 'K'  */
+            /* packets entirely, and only use the '.' packets.            */ 
+            if (m_spaceball4000)
+                break;
+
+            m_buttons = 
+                ((m_buf[1] & 0x10) <<  8) | /* 2003 pick button is ??? */
+                ((m_buf[1] & 0x20) <<  9) | /* 3003 rezero button      */
+                ((m_buf[1] & 0x08) << 11) | /* 2003 rezero button      */
+                ((m_buf[1] & 0x07) <<  4) | /* 5,6,7    (2003/4000)    */
+                ((m_buf[2] & 0x30) <<  8) | /* 3003 Left/Right buttons */ 
+                ((m_buf[2] & 0x0F));        /* 1,2,3,4  (2003/4000)    */ 
+
+            bChanged = true;
+            DoChangedButtons();
+            break;
+
+        case '.': /* button press event (4000) */
+            /* Spaceball 4000 FLX "expanded" button packet, with 12 buttons */
+
+            /* extra packet validity check, since we use this packet type */
+            /* to override the 'K' button packets, and determine if its a */
+            /* Spaceball 4000 or not...                                   */
+            if (m_buf[3] != '\r') {
+                break; /* if not terminated with a '\r', probably garbage */
+                }
+
+            /* if we got a valid '.' packet, this must be a Spaceball 4000 */
+            m_spaceball4000 = 1; /* Must be talking to a Spaceball 4000 */
+
+            /* Spaceball 4000 series "expanded" button press event      */
+            /* includes data for 12 buttons, and left/right orientation */
+            m_buttons = 
+                (((~m_buf[1]) & 0x20) << 10) |  /* "left handed" mode  */
+                ((m_buf[1] & 0x1F) <<  7)    |  /* 8,9,10,11,12        */
+                ((m_buf[2] & 0x3F)      )    |  /* 1,2,3,4,5,6 (4000)  */
+                ((m_buf[2] & 0x80) >>  1);      /* 7           (4000)  */
+
+            /* set "lefty" orientation mode if "lefty bit" is _clear_ */
+            if ((m_buf[1] & 0x20) == 0) 
+                m_leftymode4000 = 1; /* left handed mode */
+            else 
+                m_leftymode4000 = 0; /* right handed mode */
+
+            bChanged = true;
+            DoChangedButtons();
+            break;
+
+        case 'C': /* Communications mode packet */
+        case 'F': /* Spaceball sensitization packet */
+        case 'P': /* Spaceball update rate packet */
+        case 'M': /* Spaceball movement mode packet */
+        case 'N': /* Null region packet */
+        case '\r': /* carriage return at poweron */
+        case '\v': /* XON at poweron */
+            /* eat and ignore these packets */
+            break;
+
+        case '@': /* Reset packet */
+            /* if we get a reset packet, we have to re-initialize       */
+            /* the device, and assume that its completely schizophrenic */
+            /* at this moment, we must reset it again at this point     */
+            m_resetoccured=1;
+            //JKReset();
+            Init();
+            break;
+
+
+        case 'E': /* Error packet, hardware/software problem */
+            m_erroroccured++;
+            break;
+
+        case 'Z': /* Zero packet (Spaceball 2003/3003/4000 FLX) */
+            /* We just ignore these... */
+            break;          
+
+        default: 
+            break;
+        }   
+
+    /* reset */ 
+    m_bufpos = 0;   
+    m_packtype = 0; 
+    m_packlen = 1;
+    packs++;
+
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void SpaceBall::ProcessSO ( char c )
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpaceBall.h	Sat Dec 01 05:30:34 2012 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+
+/* Spaceball Button bit-masks */
+#define SBALL_BUTTON_1        0x0001   /* bit  0 */
+#define SBALL_BUTTON_2        0x0002   /* bit  1 */
+#define SBALL_BUTTON_3        0x0004   /* bit  2 */
+#define SBALL_BUTTON_4        0x0008   /* bit  3 */
+#define SBALL_BUTTON_5        0x0010   /* bit  4 */
+#define SBALL_BUTTON_6        0x0020   /* bit  5 */
+#define SBALL_BUTTON_7        0x0040   /* bit  6 */
+#define SBALL_BUTTON_8        0x0080   /* bit  7 */
+#define SBALL_BUTTON_9        0x0100   /* bit  8 */
+#define SBALL_BUTTON_10       0x0200   /* bit  9 */
+#define SBALL_BUTTON_11       0x0400   /* bit 10 */
+#define SBALL_BUTTON_12       0x0800   /* bit 11 */
+
+/* The Spaceball 3003 and 3003 FLX only have "left" and "right" buttons */
+#define SBALL_BUTTON_RIGHT    0x1000   /* bit 12 */
+#define SBALL_BUTTON_LEFT     0x2000   /* bit 13 */
+
+/* The Spaceball 2003A and 2003B have a dedicated pick button on the ball */
+/* The Spaceball 2003 FLX uses "button 9" as the pick button.             */
+/* All of them return this as "button 9" in their encoded button data     */
+#define SBALL_BUTTON_PICK      SBALL_BUTTON_RIGHT   /* bit  12 */
+
+/* On Spaceball 2003A and 2003B, the Rezero is "button 8" on the device */
+/* On the newer devices, there are dedicated rezero buttons */
+#define SBALL_BUTTON_REZERO  0x4000   /* bit 14 */
+
+/* The Spaceball 4000 FLX has a configurable palm rest which can be in    */
+/* either "left" or "right" handed mode.  When it is configured in "left" */
+/* handed mode, the "lefty" bit is set, and coordinate systems need to be */
+/* inverted on one axis.                                                  */
+#define SBALL_MODE_LEFTY     0x8000   /* bit 15 */
+
+
+enum eSpaceBallAxis { 
+    TX=0, TY, TZ, 
+    Right=0, Forward, Up, 
+    RX=3, RY, RZ,
+    Pitch=3, Roll, Yaw };
+
+class SpaceBall {
+public:
+    
+    SpaceBall ( PinName tx, PinName rx, bool bSpaceOrb=false );
+    ~SpaceBall() {};
+
+    void Init();
+    
+    void GetTranslation ( float* fValue[3] ) const
+    {
+        *fValue[0] = GetAxis ( TX );
+        *fValue[1] = GetAxis ( TY );
+        *fValue[2] = GetAxis ( TZ );
+    }
+    void GetRotation ( float* fValue[3] ) const
+    {
+        *fValue[0] = GetAxis ( RX );
+        *fValue[1] = GetAxis ( RY );
+        *fValue[2] = GetAxis ( RZ );
+    }
+    float GetAxis ( int nAxis ) const
+    {
+        return GetAxisRaw ( nAxis ) * 0.00005f * _fScale;
+    }
+    int GetAxisRaw ( int nAxis ) const
+    {
+        if ( nAxis<0 || nAxis>5 )
+            return 0;
+        
+        return m_axis[nAxis];
+    }
+    float operator[] (eSpaceBallAxis nAxis) const
+    {   return GetAxis ( nAxis ); }
+    float operator[] (int nAxis) const
+    {   return GetAxis ( nAxis ); }
+    
+    int GetButtons() const
+    {
+        return m_buttons;
+    }
+    operator int (void) const
+    {   return GetButtons(); }
+    
+    void SetScale ( float fScale=1.0f )
+    {   _fScale = fScale; }
+    float GetScale ( void )
+    {   return _fScale; }
+
+
+protected:
+    Serial _serial;
+    bool _bSpaceOrb;
+    float _fScale;
+    
+    int m_axis[6];      /* last translational data received */
+    //int m_rot[3];        /* last rotational data received */
+    int m_buttons;       /* current button status */
+    
+    void SerialISR(void);
+    void Process ( char c );
+    
+    virtual void DoChangedAxis (void) {};
+    virtual void DoChangedButtons (void) {};
+    
+private:
+   unsigned char m_buf[256];
+    char m_resetstring[256]; 
+    int m_bufpos;        /* current char position in packet buffer */
+    int m_packtype;      /* what kind of packet is it */
+    int m_packlen;       /* how many bytes do we ultimately expect? */
+    int m_escapedchar;   /* if set, we're processing an escape sequence */
+    int m_erroroccured;  /* if set, we've received an error packet or packets */
+    int m_resetoccured;  /* if set, ball was reset, so have to reinitialize it */
+    int m_spaceball4000; /* if set, its a Spaceball 4000 */
+    int m_leftymode4000; /* if set, Spaceball 4000 in "lefty" orientation */
+    int m_timer;         /* time since last packet was received */
+
+    void InitSB() {};
+    void InitSO() {};
+
+    void ProcessSB ( char c );
+    void ProcessSO ( char c );
+};
\ No newline at end of file