Diff: SpaceBall.cpp

Revision:

0:f67a8fffd94a

Child:

1:e6282b645d9b

--- /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 )
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////