1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171

/* Teensy 4.1 — SpaceMouse (USB Host via JoystickController) → USB-MIDI (Serial+MIDI)
 * Fixed full-scale, change-gated 14-bit CC. Buttons → two CCs (127/0).
 * NEW: sendAxisPair() sends LSB whenever MSB changes (prevents stale LSB pairing in Pd).
 * Optional SNAP_TO_CENTER to force perfect 8192 (MSB=64, LSB=0) near rest.
 */

#include <USBHost_t36.h>

// -------- Host stack --------
USBHost            usb;
USBHub             hub1(usb);             // optional hub support
USBHIDParser       hid1(usb), hid2(usb);
JoystickController joy1(usb);             // first joystick / multi-axis device

// -------- User config --------
const uint8_t MIDI_CH = 1;                // MIDI channel 1..16

// CC map (MSB/LSB) for X,Y,Z,Rx,Ry,Rz
const uint8_t CC_X_MSB=16, CC_X_LSB=48;
const uint8_t CC_Y_MSB=17, CC_Y_LSB=49;
const uint8_t CC_Z_MSB=18, CC_Z_LSB=50;
const uint8_t CC_RX_MSB=19, CC_RX_LSB=51;
const uint8_t CC_RY_MSB=20, CC_RY_LSB=52;
const uint8_t CC_RZ_MSB=21, CC_RZ_LSB=53;

// Buttons (on/off 127/0)
const uint8_t CC_BTN_L = 22;
const uint8_t CC_BTN_R = 23;

// Axis inversion (+1 or -1) — flip to -1 for any axis that feels reversed
int AX_INV[6] = { +1, +1, +1, +1, +1, +1 };

// Fixed full-scale per axis (tune if your device tops out differently)
const int FS_X  = 200;
const int FS_Y  = 200;
const int FS_Z  = 200;
const int FS_RX = 200;
const int FS_RY = 200;
const int FS_RZ = 200;

// Poll/send throttle (ms)
const uint32_t SEND_PERIOD_MS = 8;

// Optional: snap to exact 8192 when very close to center
const bool  SNAP_TO_CENTER    = true;
const float SNAP_THRESHOLD    = 0.004f;   // ~0.4% of full scale (~±65 in 14-bit)

// -------- State --------
uint8_t lastMSB[6] = {255,255,255,255,255,255};
uint8_t lastLSB[6] = {255,255,255,255,255,255};
bool    lastBtnL=false, lastBtnR=false;
uint32_t lastPollMs = 0;

// -------- Helpers --------
static inline float normAxisFixed(int v, int fullScale){
  if (v < -fullScale) v = -fullScale;
  if (v >  fullScale) v =  fullScale;
  return (float)v / (float)fullScale; // -1..+1
}

static inline void to14(float norm, uint8_t &msb, uint8_t &lsb){
  if (norm < -1.f) norm = -1.f; else if (norm > 1.f) norm = 1.f;
  int v14;
  if (SNAP_TO_CENTER && fabsf(norm) < SNAP_THRESHOLD) {
    v14 = 8192;                       // force perfect center
  } else {
    v14 = (int)lroundf((norm * 0.5f + 0.5f) * 16383.0f);
  }
  if (v14 < 0) v14 = 0; else if (v14 > 16383) v14 = 16383;
  msb = (v14 >> 7) & 0x7F;
  lsb = v14 & 0x7F;
}

// NEW: send LSB whenever MSB changes so Pd always pairs a fresh LSB with the new MSB.
static inline void sendAxisPair(uint8_t ccMSB, uint8_t ccLSB,
                                uint8_t &last_msb, uint8_t &last_lsb,
                                float norm){
  uint8_t msb, lsb; to14(norm, msb, lsb);

  if (msb != last_msb) {
    usbMIDI.sendControlChange(ccMSB, msb, MIDI_CH);
    usbMIDI.sendControlChange(ccLSB, lsb, MIDI_CH);  // force-send LSB with new MSB
    last_msb = msb;
    last_lsb = lsb;
    return;
  }
  if (lsb != last_lsb) {
    usbMIDI.sendControlChange(ccLSB, lsb, MIDI_CH);
    last_lsb = lsb;
  }
}

static inline void sendBtn(uint8_t cc, bool down){
  usbMIDI.sendControlChange(cc, down ? 127 : 0, MIDI_CH);
}

// -------- Setup / Loop --------
void setup(){
  Serial.begin(115200);
  usb.begin();
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.println("SpaceMouse → USB-MIDI (paired LSB/MSB) ready.");
}

void loop(){
  usb.Task();
  while (usbMIDI.read()) {} // drain incoming MIDI if any

  uint32_t now = millis();
  if (now - lastPollMs < SEND_PERIOD_MS) return;
  lastPollMs = now;

  if (!joy1){
    static uint32_t t0=0; 
    if (now - t0 > 1000){ t0 = now; Serial.println("Waiting for joystick/multi-axis..."); }
    return;
  }

  // Read raw axes (0..5). Swap these if your unit orders them differently.
  int axX  = joy1.getAxis(0);
  int axY  = joy1.getAxis(1);
  int axZ  = joy1.getAxis(2);
  int axRx = joy1.getAxis(3);
  int axRy = joy1.getAxis(4);
  int axRz = joy1.getAxis(5);

  // Normalize with fixed scales & optional inversion
  float X  = AX_INV[0] * normAxisFixed(axX,  FS_X);
  float Y  = AX_INV[1] * normAxisFixed(axY,  FS_Y);
  float Z  = AX_INV[2] * normAxisFixed(axZ,  FS_Z);
  float RX = AX_INV[3] * normAxisFixed(axRx, FS_RX);
  float RY = AX_INV[4] * normAxisFixed(axRy, FS_RY);
  float RZ = AX_INV[5] * normAxisFixed(axRz, FS_RZ);

  // Send CC pairs (paired LSB/MSB behavior)
  sendAxisPair(CC_X_MSB,  CC_X_LSB,  lastMSB[0], lastLSB[0], X);
  sendAxisPair(CC_Y_MSB,  CC_Y_LSB,  lastMSB[1], lastLSB[1], Y);
  sendAxisPair(CC_Z_MSB,  CC_Z_LSB,  lastMSB[2], lastLSB[2], Z);
  sendAxisPair(CC_RX_MSB, CC_RX_LSB, lastMSB[3], lastLSB[3], RX);
  sendAxisPair(CC_RY_MSB, CC_RY_LSB, lastMSB[4], lastLSB[4], RY);
  sendAxisPair(CC_RZ_MSB, CC_RZ_LSB, lastMSB[5], lastLSB[5], RZ);

  // Buttons (bit0, bit1 typical)
  uint32_t btns = joy1.getButtons();
  bool L = (btns & 0x01);
  bool R = (btns & 0x02);
  if (L != lastBtnL){ sendBtn(CC_BTN_L, L); lastBtnL = L; }
  if (R != lastBtnR){ sendBtn(CC_BTN_R, R); lastBtnR = R; }

  // Serial debug for X (prints whenever X's MSB/LSB changed)
  {
    uint8_t msbX = lastMSB[0], lsbX = lastLSB[0];
    static uint8_t prevM=255, prevL=255;
    if (msbX != prevM || lsbX != prevL) {
      int val14 = ((int)msbX << 7) | lsbX;
      Serial.print("X norm=");
      Serial.print(X, 3);
      Serial.print("  MSB/LSB=");
      Serial.print(msbX);
      Serial.print("/");
      Serial.print(lsbX);
      Serial.print("  val14=");
      Serial.println(val14);
      prevM = msbX; prevL = lsbX;
    }
  }

  // Heartbeat (device present)
  static uint32_t t1=0; 
  if (now - t1 > 500){ t1 = now; digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); }
}