OpenMoCo
Library to handle SpaceBall, SpaceMouse and SpaceOrb on serial port. Gets access to 3D rotation and translation vector as well as button status. (USB is not supported)
Library to handle SpaceBall, SpaceMouse and SpaceOrb on serial port. Gets access to 3D rotation and translation vector as well as button status. (USB is not supported)
All handling and decoding is done in the RX interrupt and the vector values can be read out asynchronously with different coordinate mappings.
Example:
#include "mbed.h"
#include "SpaceBall.h"
PwmOut led[] = {(LED1), (LED2), (LED3), (LED4) };
SpaceBall SBall(p9, p10); // tx, rx, bSOrb
int main() {
SBall.Init();
while(1) {
led[0] = abs( SBall[TX] ) + abs( SBall[TY] ) + abs( SBall[TZ] );
led[1] = abs( SBall[RX] );
led[2] = abs( SBall[RY] );
led[3] = abs( SBall[RZ] );
wait_us(500);
}
}
In this exaple the 4 LEDs are powered dependent on force at the Spaceball. LED1 shows the sum of all translation forces. LED2 to LED4 shows the rotation forces.
For more information about SpaceBall devices see manufactorers page http://www.3dconnexion.com
For connecting a SpaceBall (or SpaceMouse or SpaceOrb) to mbed see page wiki/Serial-Connection
Example: SpaceBall 4000
Revision 2:a7c0fcd157f7, committed 2012-12-01
- Comitter:
- jocis
- Date:
- Sat Dec 01 21:38:49 2012 +0000
- Parent:
- 1:e6282b645d9b
- Child:
- 3:7bacae57a30b
- Commit message:
- Addef mapping and cleanup
Changed in this revision
--- a/SB.cpp Sat Dec 01 18:23:50 2012 +0000
+++ b/SB.cpp Sat Dec 01 21:38:49 2012 +0000
@@ -1,13 +1,11 @@
#include "SpaceBall.h"
-#define Sleep(x) wait_ms(x*10)
+#define Sleep(x) wait_ms(x)
-extern Serial pc;
+//extern Serial pc;
///////////////////////////////////////////////////////////////////////////////
-
-
/*
Convert characters to nibbles
*/
@@ -42,16 +40,15 @@
void SpaceBall::InitSB()
{
- m_packlen=1;
m_resetoccured=0;
- m_spaceball4000 = 0; /* re-determine which type it is */
+ m_spaceball4000 = 0; /* re-determine which type it is */
m_leftymode4000 = 0; /* re-determine if its in lefty mode */
+ _fScaleT = 1.0 / 5000.0;
+ _fScaleR = 1.0 / 2000.0;
+
if (!m_resetoccured) {
-
- // pc.printf("Sending reset command to spaceball...\n");
-
m_resetoccured=1;
_serial.printf ( "@\r" ); /* force reset */
}
@@ -132,7 +129,7 @@
case 'D': /* Ball data */
if (_idx != 15) return;
for (i = 0; i < 6; i++) {
- m_axis[i] = (short)( (_data[2*i + 3] << 8) | _data[2*i + 2] );
+ _axis[i] = (short)( (_data[2*i + 3] << 8) | _data[2*i + 2] );
}
DoChangedAxis();
break;
@@ -141,7 +138,7 @@
//pc.printf("d%d",_idx);
if (_idx != 25) return;
for (i = 0; i < 6; i++) {
- m_axis[i] = (short)( (
+ _axis[i] = (short)( (
DecodeSM(_data[4*i+4]) | (DecodeSM(_data[4*i+3])<<4) | (DecodeSM(_data[4*i+2])<<8) | (DecodeSM(_data[4*i+1])<<12) )
^ 0x8000 );
}
@@ -166,7 +163,7 @@
if (m_spaceball4000)
break;
- m_buttons =
+ _buttons =
((_data[1] & 0x10) << 8) | /* 2003 pick button is ??? */
((_data[1] & 0x20) << 9) | /* 3003 rezero button */
((_data[1] & 0x08) << 11) | /* 2003 rezero button */
@@ -174,25 +171,14 @@
((_data[2] & 0x30) << 8) | /* 3003 Left/Right buttons */
((_data[2] & 0x0F)); /* 1,2,3,4 (2003/4000) */
- //bChanged = true;
DoChangedButtons();
- /*
- input_report_key(dev, BTN_1, (data[2] & 0x01) || (data[2] & 0x20));
- input_report_key(dev, BTN_2, data[2] & 0x02);
- input_report_key(dev, BTN_3, data[2] & 0x04);
- input_report_key(dev, BTN_4, data[2] & 0x08);
- input_report_key(dev, BTN_5, data[1] & 0x01);
- input_report_key(dev, BTN_6, data[1] & 0x02);
- input_report_key(dev, BTN_7, data[1] & 0x04);
- input_report_key(dev, BTN_8, data[1] & 0x10);
- */
break;
case 'k': /* Button data */
//pc.printf("k%d",_idx);
- pc.printf("<%s>\r\n",_data);
+ //pc.printf("<%s>\r\n",_data);
if (_idx != 4) return;
- m_buttons = DecodeSM(_data[1]) | (DecodeSM(_data[2])<<4) | (DecodeSM(_data[3])<<8);
+ _buttons = DecodeSM(_data[1]) | (DecodeSM(_data[2])<<4) | (DecodeSM(_data[3])<<8);
DoChangedButtons();
break;
@@ -210,7 +196,7 @@
/* Spaceball 4000 series "expanded" button press event */
/* includes data for 12 buttons, and left/right orientation */
- m_buttons =
+ _buttons =
(((~_data[1]) & 0x20) << 10) | /* "left handed" mode */
((_data[1] & 0x1F) << 7) | /* 8,9,10,11,12 */
((_data[2] & 0x3F) ) | /* 1,2,3,4,5,6 (4000) */
@@ -222,23 +208,7 @@
else
m_leftymode4000 = 0; /* right handed mode */
- //bChanged = true;
DoChangedButtons();
- /*
- input_report_key(dev, BTN_1, data[2] & 0x01);
- input_report_key(dev, BTN_2, data[2] & 0x02);
- input_report_key(dev, BTN_3, data[2] & 0x04);
- input_report_key(dev, BTN_4, data[2] & 0x08);
- input_report_key(dev, BTN_5, data[2] & 0x10);
- input_report_key(dev, BTN_6, data[2] & 0x20);
- input_report_key(dev, BTN_7, data[2] & 0x80);
- input_report_key(dev, BTN_8, data[1] & 0x01);
- input_report_key(dev, BTN_9, data[1] & 0x02);
- input_report_key(dev, BTN_A, data[1] & 0x04);
- input_report_key(dev, BTN_B, data[1] & 0x08);
- input_report_key(dev, BTN_C, data[1] & 0x10);
- input_report_key(dev, BTN_MODE, data[1] & 0x20);
- */
break;
case 'E': /* Device error */
@@ -266,7 +236,6 @@
/* 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;
@@ -281,11 +250,9 @@
break;
default:
- pc.printf("<%s>\r\n",_data);
+ //pc.printf("<%s>\r\n",_data);
break;
-
}
-
}
///////////////////////////////////////////////////////////////////////////////
--- a/SO.cpp Sat Dec 01 18:23:50 2012 +0000
+++ b/SO.cpp Sat Dec 01 21:38:49 2012 +0000
@@ -1,25 +1,20 @@
#include "SpaceBall.h"
-#define Sleep(x) wait_ms(x*10)
-
-extern Serial pc;
+//extern Serial pc;
///////////////////////////////////////////////////////////////////////////////
static unsigned char spaceorb_xor[] = "SpaceWare";
-
+/*
static unsigned char spaceorb_errors[][32] = {
"EEPROM storing 0 failed", "Receive queue overflow", "Transmit queue timeout",
"Bad packet", "Power brown-out", "EEPROM checksum error", "Hardware fault" };
-
+*/
void SpaceBall::InitSO()
{
-
- // pc.printf("Sending initialization sequence to spaceball...\n");
-
-// _serial.printf ( "CB\r" );
- Sleep(10);
+ _fScaleT = 1.0 / 512.0;
+ _fScaleR = 1.0 / 512.0;
}
///////////////////////////////////////////////////////////////////////////////
@@ -34,7 +29,6 @@
}
if (_idx < SPACEBALL_MAX_LENGTH)
_data[_idx++] = data & 0x7f;
-
}
///////////////////////////////////////////////////////////////////////////////
@@ -51,10 +45,6 @@
switch (_data[0]) {
case 'R': /* Reset packet */
- //spaceorb->data[spaceorb->idx - 1] = 0;
- //for (i = 1; i < spaceorb->idx && spaceorb->data[i] == ' '; i++);
- //printk(KERN_INFO "input: %s [%s] on %s\n",
- // spaceorb_name, spaceorb->data + i, spaceorb->serio->phys);
break;
case 'D': /* Ball + button data */
@@ -70,8 +60,8 @@
axes[4] = ((_data[7] & 0x03) << 8) | (_data[ 8] << 1) | (_data[7] >> 6);
axes[5] = ((_data[9] & 0x3f) << 4) | (_data[10] >> 3);
for (i = 0; i < 6; i++)
- m_axis[i] = axes[i] - ((axes[i] & 0x200) ? 1024 : 0);
- m_buttons = _data[1];
+ _axis[i] = axes[i] - ((axes[i] & 0x200) ? 1024 : 0);
+ _buttons = _data[1];
DoChangedAxis();
DoChangedButtons();
}
@@ -79,27 +69,22 @@
case 'K': /* Button data */
if (_idx != 5) return;
- m_buttons = _data[2];
+ _buttons = _data[2];
DoChangedButtons();
break;
case 'E': /* Error packet */
if (_idx != 4) return;
- //printk(KERN_ERR "joy-spaceorb: Device error. [ ");
- //for (i = 0; i < 7; i++) if (data[1] & (1 << i)) printk("%s ", spaceorb_errors[i]);
- //printk("]\n");
+ //printf(KERN_ERR "joy-spaceorb: Device error. [ ");
+ //for (i = 0; i < 7; i++) if (_data[1] & (1 << i)) printf("%s ", spaceorb_errors[i]);
+ //printf("]\n");
break;
- case '?': /* Version number follows */
+ default:
/* eat and ignore these packets */
+ //pc.printf("<%s>\r\n",_data);
break;
-
- default:
- pc.printf("<%s>\r\n",_data);
- break;
-
}
-
}
///////////////////////////////////////////////////////////////////////////////
--- a/SpaceBall.cpp Sat Dec 01 18:23:50 2012 +0000
+++ b/SpaceBall.cpp Sat Dec 01 21:38:49 2012 +0000
@@ -12,25 +12,19 @@
_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 */
+ _axis[0] = 0; /* last translational data received */
+ _axis[1] = 0;
+ _axis[2] = 0;
+ _axis[3] = 0; /* last rotational data received */
+ _axis[4] = 0;
+ _axis[5] = 0;
+ _buttons = 0; /* current button status */
_data[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 */
_escape = false;
@@ -62,8 +56,6 @@
{
c = _serial.getc();
- //printf("%c", c);
-
Process ( c );
}
}
@@ -79,5 +71,57 @@
}
///////////////////////////////////////////////////////////////////////////////
+
+float SpaceBall::GetAxis ( int nAxis ) const
+{
+ float fValue = 0;
+
+ if ( _mapping == CNC )
+ {
+ if ( nAxis == TX ) fValue = _axis[0];
+ else if ( nAxis == TY ) fValue = _axis[2];
+ else if ( nAxis == TZ ) fValue = _axis[1];
+ else if ( nAxis == RX ) fValue = -_axis[0];
+ else if ( nAxis == RY ) fValue = -_axis[2];
+ else if ( nAxis == RZ ) fValue = -_axis[1];
+ }
+ else if ( _mapping == CNCvert )
+ {
+ if ( nAxis == TX ) fValue = _axis[0];
+ else if ( nAxis == TY ) fValue = _axis[1];
+ else if ( nAxis == TZ ) fValue = -_axis[2];
+ else if ( nAxis == RX ) fValue = -_axis[0];
+ else if ( nAxis == RY ) fValue = -_axis[1];
+ else if ( nAxis == RZ ) fValue = -_axis[2];
+ }
+ else
+ {
+ if ( nAxis == TX ) fValue = _axis[0];
+ else if ( nAxis == TY ) fValue = _axis[1];
+ else if ( nAxis == TZ ) fValue = _axis[2];
+ else if ( nAxis == RX ) fValue = _axis[0];
+ else if ( nAxis == RY ) fValue = _axis[1];
+ else if ( nAxis == RZ ) fValue = _axis[2];
+ }
+
+ if ( nAxis <= TZ )
+ fValue *= _fScaleT * _fScale;
+ else
+ fValue *= _fScaleR * _fScale;
+
+ return fValue;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+int SpaceBall::GetAxisRaw ( int nAxis ) const
+ {
+ if ( nAxis<0 || nAxis>5 )
+ return 0;
+
+ return _axis[nAxis];
+ }
+
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
--- a/SpaceBall.h Sat Dec 01 18:23:50 2012 +0000
+++ b/SpaceBall.h Sat Dec 01 21:38:49 2012 +0000
@@ -42,6 +42,10 @@
RX=3, RY, RZ,
Pitch=3, Roll, Yaw };
+enum eSpaceBallMapping {
+ RAW=0, CNC, CNCvert };
+
+
class SpaceBall {
public:
@@ -50,38 +54,35 @@
void Init();
+ void SetMapping ( int mapping )
+ { _mapping = mapping; }
+
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 GetAxis ( int nAxis ) const;
+
+ int GetAxisRaw ( int nAxis ) const;
+
float operator[] (eSpaceBallAxis nAxis) const
{ return GetAxis ( nAxis ); }
float operator[] (int nAxis) const
{ return GetAxis ( nAxis ); }
int GetButtons() const
- {
- return m_buttons;
- }
+ { return _buttons; }
+
operator int (void) const
{ return GetButtons(); }
@@ -96,9 +97,8 @@
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 */
+ int _axis[6]; /* last translational data received */
+ int _buttons; /* current button status */
void SerialISR(void);
void Process ( char c );
@@ -107,21 +107,18 @@
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 */
bool _escape;
int _idx;
char _data[SPACEBALL_MAX_LENGTH];
+
+ float _fScaleR;
+ float _fScaleT;
+ int _mapping;
void InitSB();
void InitSO();