trackball.ino 6.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221
  1. /*
  2. * ClockworkPi DevTerm Trackball
  3. */
  4. #include "keys_io_map.h"
  5. #include <stdint.h>
  6. #include <stdbool.h>
  7. #include <string.h>
  8. #include <USBComposite.h>
  9. #include "trackball.h"
  10. #include "math.h"
  11. // Choose the type of filter (add a `_` to the #define you're not using):
  12. // - FIR uses more memory and takes longer to run, but you can tweak the FILTER_SIZE
  13. // - IIR is faster (has less coefficients), but the coefficients are pre-calculated (via scipy)
  14. #define _USE_FIR
  15. #define USE_IIR
  16. // Enable debug messages via serial
  17. #define _DEBUG_TRACKBALL
  18. // Source: https://github.com/dangpzanco/dsp
  19. #include <dsp_filters.h>
  20. // Simple trackball pin direction counter
  21. enum TrackballPin : uint8_t
  22. {
  23. PIN_LEFT,
  24. PIN_RIGHT,
  25. PIN_UP,
  26. PIN_DOWN,
  27. PIN_NUM,
  28. };
  29. static uint8_t direction_counter[PIN_NUM] = {0};
  30. // Mouse and Wheel sensitivity values
  31. static const float MOUSE_SENSITIVITY = 10.0f;
  32. static const float WHEEL_SENSITIVITY = 0.25f;
  33. #ifdef USE_IIR
  34. // Infinite Impulse Response (IIR) Filter
  35. // Filter design (https://docs.scipy.org/doc/scipy/reference/signal.html):
  36. // Low-pass Butterworth filter [b, a = scipy.signal.butter(N=2, Wn=0.1)]
  37. static const int8_t IIR_SIZE = 3;
  38. static float iir_coeffs_b[IIR_SIZE] = {0.020083365564211232, 0.040166731128422464, 0.020083365564211232};
  39. static float iir_coeffs_a[IIR_SIZE] = {1.0, -1.5610180758007182, 0.6413515380575631};
  40. IIR iir_x, iir_y;
  41. #endif
  42. #ifdef USE_FIR
  43. // FIR Filter
  44. static const int8_t FILTER_SIZE = 10;
  45. static float fir_coeffs[FILTER_SIZE];
  46. FIR fir_x, fir_y;
  47. static void init_fir()
  48. {
  49. // Moving Average Finite Impulse Response (FIR) Filter:
  50. // - Smooths out corners (the trackball normally only moves in 90 degree angles)
  51. // - Filters out noisy data (avoids glitchy movements)
  52. // - Adds delay to the movement. To tweak this:
  53. // 1. Change the FILTER_SIZE (delay is proportional to the size, use millis() to measure time)
  54. // 2. Redesign the filter with the desired delay
  55. for (int8_t i = 0; i < FILTER_SIZE; i++)
  56. fir_coeffs[i] = 1.0f / FILTER_SIZE;
  57. }
  58. #endif
  59. #ifdef DEBUG_TRACKBALL
  60. static uint32_t time = millis();
  61. // Useful debug function
  62. template <typename T>
  63. void print_vec(DEVTERM *dv, T *vec, uint32_t size, bool newline = true)
  64. {
  65. for (int8_t i = 0; i < size - 1; i++)
  66. {
  67. dv->_Serial->print(vec[i]);
  68. dv->_Serial->print(",");
  69. }
  70. if (newline)
  71. {
  72. dv->_Serial->println(vec[size - 1]);
  73. }
  74. else
  75. {
  76. dv->_Serial->print(vec[size - 1]);
  77. dv->_Serial->print(",");
  78. }
  79. }
  80. #endif
  81. template <TrackballPin PIN>
  82. static void interrupt()
  83. {
  84. // Count the number of times the trackball rolls towards a certain direction
  85. // (when the corresponding PIN changes its value). This part of the code should be minimal,
  86. // so that the next interrupts are not blocked from happening.
  87. direction_counter[PIN] += 1;
  88. }
  89. static float position_scale(float x)
  90. {
  91. // Exponential scaling of the mouse movement:
  92. // - Small values remain small (precise movement)
  93. // - Slightly larger values get much larger (fast movement)
  94. // This function may be tweaked further, but it's good enough for now.
  95. return MOUSE_SENSITIVITY * sign(x) * std::exp(std::abs(x) / std::sqrt(MOUSE_SENSITIVITY));
  96. }
  97. void trackball_task(DEVTERM *dv)
  98. {
  99. #ifdef DEBUG_TRACKBALL
  100. // Measure elapsed time
  101. uint32_t elapsed = millis() - time;
  102. time += elapsed;
  103. // Send raw data via serial (CSV format)
  104. dv->_Serial->print(elapsed);
  105. dv->_Serial->print(",");
  106. print_vec(dv, direction_counter, PIN_NUM);
  107. #endif
  108. // Stop interrupts from happening. Don't forget to re-enable them!
  109. noInterrupts();
  110. // Calculate x and y positions
  111. float x = direction_counter[PIN_RIGHT] - direction_counter[PIN_LEFT];
  112. float y = direction_counter[PIN_DOWN] - direction_counter[PIN_UP];
  113. // Clear counters
  114. // memset(direction_counter, 0, sizeof(direction_counter));
  115. std::fill(std::begin(direction_counter), std::end(direction_counter), 0);
  116. // Re-enable interrupts (Mouse.move needs interrupts)
  117. interrupts();
  118. // Non-linear scaling
  119. x = position_scale(x);
  120. y = position_scale(y);
  121. // Wheel rolls with the (reverse) vertical axis (no filter needed)
  122. int8_t w = clamp<int8_t>(-y * WHEEL_SENSITIVITY);
  123. // Filter x and y
  124. #ifdef USE_FIR
  125. x = fir_filt(&fir_x, x);
  126. y = fir_filt(&fir_y, y);
  127. #endif
  128. #ifdef USE_IIR
  129. x = iir_filt(&iir_x, x);
  130. y = iir_filt(&iir_y, y);
  131. #endif
  132. // Move Trackball (either Mouse or Wheel)
  133. switch (dv->state->moveTrackball())
  134. {
  135. case TrackballMode::Mouse:
  136. {
  137. // Move mouse
  138. while ((int)x != 0 || (int)y != 0)
  139. {
  140. // Only 8bit values are allowed,
  141. // so clamp and execute move() multiple times
  142. int8_t x_byte = clamp<int8_t>(x);
  143. int8_t y_byte = clamp<int8_t>(y);
  144. // Move mouse with values in the range [-128, 127]
  145. dv->Mouse->move(x_byte, y_byte, 0);
  146. // Decrement the original value, stop if done
  147. x -= x_byte;
  148. y -= y_byte;
  149. }
  150. break;
  151. }
  152. case TrackballMode::Wheel:
  153. {
  154. if (w != 0)
  155. {
  156. // Only scroll the wheel [move cursor by (0,0)]
  157. dv->Mouse->move(0, 0, w);
  158. dv->state->setScrolled();
  159. }
  160. break;
  161. }
  162. }
  163. }
  164. void trackball_init(DEVTERM *dv)
  165. {
  166. // Enable trackball pins
  167. pinMode(LEFT_PIN, INPUT);
  168. pinMode(UP_PIN, INPUT);
  169. pinMode(RIGHT_PIN, INPUT);
  170. pinMode(DOWN_PIN, INPUT);
  171. // Initialize filters
  172. #ifdef USE_FIR
  173. init_fir();
  174. fir_init(&fir_x, FILTER_SIZE, fir_coeffs);
  175. fir_init(&fir_y, FILTER_SIZE, fir_coeffs);
  176. #endif
  177. #ifdef USE_IIR
  178. iir_init(&iir_x, IIR_SIZE, iir_coeffs_b, IIR_SIZE, iir_coeffs_a);
  179. iir_init(&iir_y, IIR_SIZE, iir_coeffs_b, IIR_SIZE, iir_coeffs_a);
  180. #endif
  181. // Run interrupt function when corresponding PIN changes its value
  182. attachInterrupt(LEFT_PIN, &interrupt<PIN_LEFT>, ExtIntTriggerMode::CHANGE);
  183. attachInterrupt(RIGHT_PIN, &interrupt<PIN_RIGHT>, ExtIntTriggerMode::CHANGE);
  184. attachInterrupt(UP_PIN, &interrupt<PIN_UP>, ExtIntTriggerMode::CHANGE);
  185. attachInterrupt(DOWN_PIN, &interrupt<PIN_DOWN>, ExtIntTriggerMode::CHANGE);
  186. }