mirror of
https://github.com/clockworkpi/DevTerm.git
synced 2025-12-12 18:28:50 +01:00
add new trackball handler
This commit is contained in:
Daniel Zanco
parent
569edf281e
commit
d4613afad7
@ -1,5 +1,5 @@
|
|||||||
/*
|
/*
|
||||||
* clockworkpi devterm trackball
|
* ClockworkPi DevTerm Trackball
|
||||||
*/
|
*/
|
||||||
#include "keys_io_map.h"
|
#include "keys_io_map.h"
|
||||||
|
|
||||||
@ -9,122 +9,213 @@
|
|||||||
|
|
||||||
#include <USBComposite.h>
|
#include <USBComposite.h>
|
||||||
|
|
||||||
|
|
||||||
#include "trackball.h"
|
#include "trackball.h"
|
||||||
|
|
||||||
#include "ratemeter.h"
|
|
||||||
#include "glider.h"
|
|
||||||
#include "math.h"
|
#include "math.h"
|
||||||
|
|
||||||
|
// Choose the type of filter (add a `_` to the #define you're not using):
|
||||||
|
// - FIR uses more memory and takes longer to run, but you can tweak the FILTER_SIZE
|
||||||
|
// - IIR is faster (has less coefficients), but the coefficients are pre-calculated (via scipy)
|
||||||
|
#define _USE_FIR
|
||||||
|
#define USE_IIR
|
||||||
|
|
||||||
|
// Enable debug messages via serial
|
||||||
enum Axis: uint8_t {
|
#define _DEBUG_TRACKBALL
|
||||||
AXIS_X,
|
|
||||||
AXIS_Y,
|
// Source: https://github.com/dangpzanco/dsp
|
||||||
AXIS_NUM,
|
#include <dsp_filters.h>
|
||||||
|
|
||||||
|
// Simple trackball pin direction counter
|
||||||
|
enum TrackballPin : uint8_t
|
||||||
|
{
|
||||||
|
PIN_LEFT,
|
||||||
|
PIN_RIGHT,
|
||||||
|
PIN_UP,
|
||||||
|
PIN_DOWN,
|
||||||
|
PIN_NUM,
|
||||||
};
|
};
|
||||||
static TrackballMode lastMode;
|
static uint8_t direction_counter[PIN_NUM] = {0};
|
||||||
static int8_t distances[AXIS_NUM];
|
|
||||||
static RateMeter rateMeter[AXIS_NUM];
|
|
||||||
static Glider glider[AXIS_NUM];
|
|
||||||
|
|
||||||
static const int8_t WHEEL_DENOM = 2;
|
// Mouse and Wheel sensitivity values
|
||||||
static int8_t wheelBuffer;
|
static const float MOUSE_SENSITIVITY = 10.0f;
|
||||||
|
static const float WHEEL_SENSITIVITY = 0.25f;
|
||||||
|
|
||||||
static float rateToVelocityCurve(float input) {
|
#ifdef USE_IIR
|
||||||
//return std::pow(std::abs(input) / 50, 1.4);
|
// Infinite Impulse Response (IIR) Filter
|
||||||
return std::abs(input) / 30;
|
// Filter design (https://docs.scipy.org/doc/scipy/reference/signal.html):
|
||||||
|
// Low-pass Butterworth filter [b, a = scipy.signal.butter(N=2, Wn=0.1)]
|
||||||
|
static const int8_t IIR_SIZE = 3;
|
||||||
|
static float iir_coeffs_b[IIR_SIZE] = {0.020083365564211232, 0.040166731128422464, 0.020083365564211232};
|
||||||
|
static float iir_coeffs_a[IIR_SIZE] = {1.0, -1.5610180758007182, 0.6413515380575631};
|
||||||
|
IIR iir_x, iir_y;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_FIR
|
||||||
|
// FIR Filter
|
||||||
|
static const int8_t FILTER_SIZE = 10;
|
||||||
|
static float fir_coeffs[FILTER_SIZE];
|
||||||
|
FIR fir_x, fir_y;
|
||||||
|
|
||||||
|
static void init_fir()
|
||||||
|
{
|
||||||
|
// Moving Average Finite Impulse Response (FIR) Filter:
|
||||||
|
// - Smooths out corners (the trackball normally only moves in 90 degree angles)
|
||||||
|
// - Filters out noisy data (avoids glitchy movements)
|
||||||
|
// - Adds delay to the movement. To tweak this:
|
||||||
|
// 1. Change the FILTER_SIZE (delay is proportional to the size, use millis() to measure time)
|
||||||
|
// 2. Redesign the filter with the desired delay
|
||||||
|
for (int8_t i = 0; i < FILTER_SIZE; i++)
|
||||||
|
fir_coeffs[i] = 1.0f / FILTER_SIZE;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
template<Axis AXIS, int8_t Direction >
|
#ifdef DEBUG_TRACKBALL
|
||||||
static void interrupt( ) {
|
static uint32_t time = millis();
|
||||||
distances[AXIS] += Direction;
|
|
||||||
rateMeter[AXIS].onInterrupt();
|
|
||||||
glider[AXIS].setDirection(Direction);
|
|
||||||
|
|
||||||
const auto rx = rateMeter[AXIS_X].rate();
|
// Useful debug function
|
||||||
const auto ry = rateMeter[AXIS_Y].rate();
|
template <typename T>
|
||||||
|
void print_vec(DEVTERM *dv, T *vec, uint32_t size, bool newline = true)
|
||||||
const auto rate = std::sqrt(rx * rx + ry * ry);
|
{
|
||||||
const auto ratio = rateToVelocityCurve(rate) / rate;
|
for (int8_t i = 0; i < size - 1; i++)
|
||||||
|
{
|
||||||
const auto vx = rx * ratio;
|
dv->_Serial->print(vec[i]);
|
||||||
const auto vy = ry * ratio;
|
dv->_Serial->print(",");
|
||||||
|
|
||||||
if (AXIS == AXIS_X) {
|
|
||||||
glider[AXIS_X].update(vx, std::sqrt(rateMeter[AXIS_X].delta()));
|
|
||||||
glider[AXIS_Y].updateSpeed(vy);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
glider[AXIS_X].updateSpeed(vx);
|
|
||||||
glider[AXIS_Y].update(vy, std::sqrt(rateMeter[AXIS_Y].delta()));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void trackball_task(DEVTERM*dv) {
|
|
||||||
int8_t x = 0, y = 0, w = 0;
|
|
||||||
noInterrupts();
|
|
||||||
const auto mode = dv->state->moveTrackball();
|
|
||||||
if (lastMode != mode) {
|
|
||||||
rateMeter[AXIS_X].expire();
|
|
||||||
rateMeter[AXIS_Y].expire();
|
|
||||||
wheelBuffer = 0;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
rateMeter[AXIS_X].tick(dv->delta);
|
|
||||||
rateMeter[AXIS_Y].tick(dv->delta);
|
|
||||||
}
|
|
||||||
lastMode = mode;
|
|
||||||
|
|
||||||
switch(mode){
|
|
||||||
case TrackballMode::Mouse: {
|
|
||||||
const auto rX = glider[AXIS_X].glide(dv->delta);
|
|
||||||
const auto rY = glider[AXIS_Y].glide(dv->delta);
|
|
||||||
x = rX.value;
|
|
||||||
y = rY.value;
|
|
||||||
if (rX.stopped) {
|
|
||||||
glider[AXIS_Y].stop();
|
|
||||||
}
|
|
||||||
if (rY.stopped) {
|
|
||||||
glider[AXIS_Y].stop();
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
case TrackballMode::Wheel: {
|
if (newline)
|
||||||
wheelBuffer += distances[AXIS_Y];
|
{
|
||||||
w = wheelBuffer / WHEEL_DENOM;
|
dv->_Serial->println(vec[size - 1]);
|
||||||
wheelBuffer -= w * WHEEL_DENOM;
|
|
||||||
if(w != 0){
|
|
||||||
dv->state->setScrolled();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
}
|
else
|
||||||
distances[AXIS_X] = 0;
|
{
|
||||||
distances[AXIS_Y] = 0;
|
dv->_Serial->print(vec[size - 1]);
|
||||||
interrupts();
|
dv->_Serial->print(",");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
if(x !=0 || y != 0 || -w!=0) {
|
template <TrackballPin PIN>
|
||||||
dv->Mouse->move(x, y, -w);
|
static void interrupt()
|
||||||
}
|
{
|
||||||
|
// Count the number of times the trackball rolls towards a certain direction
|
||||||
|
// (when the corresponding PIN changes its value). This part of the code should be minimal,
|
||||||
|
// so that the next interrupts are not blocked from happening.
|
||||||
|
direction_counter[PIN] += 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static float position_scale(float x)
|
||||||
void trackball_init(DEVTERM*dv){
|
{
|
||||||
|
// Exponential scaling of the mouse movement:
|
||||||
pinMode(LEFT_PIN, INPUT);
|
// - Small values remain small (precise movement)
|
||||||
pinMode(UP_PIN, INPUT);
|
// - Slightly larger values get much larger (fast movement)
|
||||||
pinMode(RIGHT_PIN, INPUT);
|
// This function may be tweaked further, but it's good enough for now.
|
||||||
pinMode(DOWN_PIN, INPUT);
|
return MOUSE_SENSITIVITY * sign(x) * std::exp(std::abs(x) / std::sqrt(MOUSE_SENSITIVITY));
|
||||||
|
}
|
||||||
attachInterrupt(LEFT_PIN, &interrupt<AXIS_X,-1> , ExtIntTriggerMode::CHANGE);
|
|
||||||
attachInterrupt(RIGHT_PIN, &interrupt<AXIS_X, 1>, ExtIntTriggerMode::CHANGE);
|
void trackball_task(DEVTERM *dv)
|
||||||
attachInterrupt(UP_PIN, &interrupt<AXIS_Y, -1>, ExtIntTriggerMode::CHANGE);
|
{
|
||||||
attachInterrupt(DOWN_PIN, &interrupt<AXIS_Y, 1>, ExtIntTriggerMode::CHANGE);
|
|
||||||
|
#ifdef DEBUG_TRACKBALL
|
||||||
|
// Measure elapsed time
|
||||||
|
uint32_t elapsed = millis() - time;
|
||||||
|
time += elapsed;
|
||||||
|
|
||||||
|
// Send raw data via serial (CSV format)
|
||||||
|
dv->_Serial->print(elapsed);
|
||||||
|
dv->_Serial->print(",");
|
||||||
|
print_vec(dv, direction_counter, PIN_NUM);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
// Stop interrupts from happening. Don't forget to re-enable them!
|
||||||
|
noInterrupts();
|
||||||
|
|
||||||
|
// Calculate x and y positions
|
||||||
|
float x = direction_counter[PIN_RIGHT] - direction_counter[PIN_LEFT];
|
||||||
|
float y = direction_counter[PIN_DOWN] - direction_counter[PIN_UP];
|
||||||
|
|
||||||
|
// Clear counters
|
||||||
|
// memset(direction_counter, 0, sizeof(direction_counter));
|
||||||
|
std::fill(std::begin(direction_counter), std::end(direction_counter), 0);
|
||||||
|
|
||||||
|
// Re-enable interrupts (Mouse.move needs interrupts)
|
||||||
|
interrupts();
|
||||||
|
|
||||||
|
// Non-linear scaling
|
||||||
|
x = position_scale(x);
|
||||||
|
y = position_scale(y);
|
||||||
|
|
||||||
|
// Wheel rolls with the (reverse) vertical axis (no filter needed)
|
||||||
|
int8_t w = clamp<int8_t>(-y * WHEEL_SENSITIVITY);
|
||||||
|
|
||||||
|
// Filter x and y
|
||||||
|
#ifdef USE_FIR
|
||||||
|
x = fir_filt(&fir_x, x);
|
||||||
|
y = fir_filt(&fir_y, y);
|
||||||
|
#endif
|
||||||
|
#ifdef USE_IIR
|
||||||
|
x = iir_filt(&iir_x, x);
|
||||||
|
y = iir_filt(&iir_y, y);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Move Trackball (either Mouse or Wheel)
|
||||||
|
switch (dv->state->moveTrackball())
|
||||||
|
{
|
||||||
|
case TrackballMode::Mouse:
|
||||||
|
{
|
||||||
|
// Move mouse
|
||||||
|
while ((int)x != 0 || (int)y != 0)
|
||||||
|
{
|
||||||
|
// Only 8bit values are allowed,
|
||||||
|
// so clamp and execute move() multiple times
|
||||||
|
int8_t x_byte = clamp<int8_t>(x);
|
||||||
|
int8_t y_byte = clamp<int8_t>(y);
|
||||||
|
|
||||||
|
// Move mouse with values in the range [-128, 127]
|
||||||
|
dv->Mouse->move(x_byte, y_byte, 0);
|
||||||
|
|
||||||
|
// Decrement the original value, stop if done
|
||||||
|
x -= x_byte;
|
||||||
|
y -= y_byte;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case TrackballMode::Wheel:
|
||||||
|
{
|
||||||
|
if (w != 0)
|
||||||
|
{
|
||||||
|
// Only scroll the wheel [move cursor by (0,0)]
|
||||||
|
dv->Mouse->move(0, 0, w);
|
||||||
|
dv->state->setScrolled();
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void trackball_init(DEVTERM *dv)
|
||||||
|
{
|
||||||
|
// Enable trackball pins
|
||||||
|
pinMode(LEFT_PIN, INPUT);
|
||||||
|
pinMode(UP_PIN, INPUT);
|
||||||
|
pinMode(RIGHT_PIN, INPUT);
|
||||||
|
pinMode(DOWN_PIN, INPUT);
|
||||||
|
|
||||||
|
// Initialize filters
|
||||||
|
#ifdef USE_FIR
|
||||||
|
init_fir();
|
||||||
|
fir_init(&fir_x, FILTER_SIZE, fir_coeffs);
|
||||||
|
fir_init(&fir_y, FILTER_SIZE, fir_coeffs);
|
||||||
|
#endif
|
||||||
|
#ifdef USE_IIR
|
||||||
|
iir_init(&iir_x, IIR_SIZE, iir_coeffs_b, IIR_SIZE, iir_coeffs_a);
|
||||||
|
iir_init(&iir_y, IIR_SIZE, iir_coeffs_b, IIR_SIZE, iir_coeffs_a);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Run interrupt function when corresponding PIN changes its value
|
||||||
|
attachInterrupt(LEFT_PIN, &interrupt<PIN_LEFT>, ExtIntTriggerMode::CHANGE);
|
||||||
|
attachInterrupt(RIGHT_PIN, &interrupt<PIN_RIGHT>, ExtIntTriggerMode::CHANGE);
|
||||||
|
attachInterrupt(UP_PIN, &interrupt<PIN_UP>, ExtIntTriggerMode::CHANGE);
|
||||||
|
attachInterrupt(DOWN_PIN, &interrupt<PIN_DOWN>, ExtIntTriggerMode::CHANGE);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user