trackball.ino 2.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123
  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 "ratemeter.h"
  11. #include "glider.h"
  12. #include "math.h"
  13. enum Axis: uint8_t {
  14. AXIS_X,
  15. AXIS_Y,
  16. AXIS_NUM,
  17. };
  18. static TrackballMode lastMode;
  19. static int8_t distances[AXIS_NUM];
  20. static RateMeter rateMeter[AXIS_NUM];
  21. static Glider glider[AXIS_NUM];
  22. static const int8_t WHEEL_DENOM = 2;
  23. static int8_t wheelBuffer;
  24. static float rateToVelocityCurve(float input) {
  25. return std::pow(std::abs(input) / 50, 1.4);
  26. }
  27. template<Axis AXIS, int8_t Direction>
  28. static void interrupt() {
  29. distances[AXIS] += Direction;
  30. rateMeter[AXIS].onInterrupt();
  31. glider[AXIS].setDirection(Direction);
  32. const auto rx = rateMeter[AXIS_X].rate();
  33. const auto ry = rateMeter[AXIS_Y].rate();
  34. const auto rate = std::sqrt(rx * rx + ry * ry);
  35. const auto ratio = rateToVelocityCurve(rate) / rate;
  36. const auto vx = rx * ratio;
  37. const auto vy = ry * ratio;
  38. if (AXIS == AXIS_X) {
  39. glider[AXIS_X].update(vx, std::sqrt(rateMeter[AXIS_X].delta()));
  40. glider[AXIS_Y].updateSpeed(vy);
  41. } else {
  42. glider[AXIS_X].updateSpeed(vx);
  43. glider[AXIS_Y].update(vy, std::sqrt(rateMeter[AXIS_Y].delta()));
  44. }
  45. }
  46. void trackball_task(DEVTERM*dv) {
  47. int8_t x = 0, y = 0, w = 0;
  48. noInterrupts();
  49. const auto mode = dv->state->moveTrackball();
  50. if (lastMode != mode) {
  51. rateMeter[AXIS_X].expire();
  52. rateMeter[AXIS_Y].expire();
  53. wheelBuffer = 0;
  54. }
  55. else {
  56. rateMeter[AXIS_X].tick(dv->delta);
  57. rateMeter[AXIS_Y].tick(dv->delta);
  58. }
  59. lastMode = mode;
  60. switch(mode){
  61. case TrackballMode::Mouse: {
  62. const auto rX = glider[AXIS_X].glide(dv->delta);
  63. const auto rY = glider[AXIS_Y].glide(dv->delta);
  64. x = rX.value;
  65. y = rY.value;
  66. if (rX.stopped) {
  67. glider[AXIS_Y].stop();
  68. }
  69. if (rY.stopped) {
  70. glider[AXIS_Y].stop();
  71. }
  72. break;
  73. }
  74. case TrackballMode::Wheel: {
  75. wheelBuffer += distances[AXIS_Y];
  76. w = wheelBuffer / WHEEL_DENOM;
  77. wheelBuffer -= w * WHEEL_DENOM;
  78. break;
  79. }
  80. }
  81. distances[AXIS_X] = 0;
  82. distances[AXIS_Y] = 0;
  83. interrupts();
  84. if(x !=0 || y != 0 || -w!=0) {
  85. dv->Mouse->move(x, y, -w);
  86. }
  87. }
  88. void trackball_init(DEVTERM*){
  89. pinMode(LEFT_PIN, INPUT);
  90. pinMode(UP_PIN, INPUT);
  91. pinMode(RIGHT_PIN, INPUT);
  92. pinMode(DOWN_PIN, INPUT);
  93. attachInterrupt(LEFT_PIN, &interrupt<AXIS_X, -1>, ExtIntTriggerMode::CHANGE);
  94. attachInterrupt(RIGHT_PIN, &interrupt<AXIS_X, 1>, ExtIntTriggerMode::CHANGE);
  95. attachInterrupt(UP_PIN, &interrupt<AXIS_Y, -1>, ExtIntTriggerMode::CHANGE);
  96. attachInterrupt(DOWN_PIN, &interrupt<AXIS_Y, 1>, ExtIntTriggerMode::CHANGE);
  97. }