diff --git a/include/gyro.h b/include/gyro.h new file mode 100644 index 0000000..184522c --- /dev/null +++ b/include/gyro.h @@ -0,0 +1,90 @@ +#ifndef GYRO_H +#define GYRO_H + +#include + +#include + +class Gyro +{ + private: + class Filter + { + private: + uint8_t size; + int16_t *buffer; + uint8_t pointer; + int32_t sum; + + public: + + void init(uint8_t size); + + ~Filter(); + + int16_t filter(int16_t x); + }; + + Filter x_filter, y_filter, z_filter; + + int32_t x, y, z; + uint8_t mapped_id; + + int8_t invert_x, invert_y, invert_z; + + float sensitivity; + int16_t deadzone; + int16_t min_delta; + + public: + enum BindToX : uint8_t + { + BIND_X, + BIND_Z, + BIND_XZ, + }; + private: + + BindToX bind_to_x; + + MPU6050 mpu; + + uint32_t time0; + uint32_t delay; + bool (*_Enabled)(); + + public: + + Gyro(); + + void init(); + + void setEnabledCallback(bool (*_Enabled)()) { this->_Enabled = _Enabled; } + bool Enabled() { return _Enabled(); } + + void setMappedId(uint8_t mapped_id) { this->mapped_id = mapped_id; } + uint8_t getMappedId() { return mapped_id; } + + void setInvertX(bool invert_x = true) { this->invert_x = invert_x? -1 : 1; } + void setInvertY(bool invert_y = true) { this->invert_y = invert_y? -1 : 1; } + void setInvertZ(bool invert_z = true) { this->invert_z = invert_z? -1 : 1; } + + void setSensitivity(float sensitivity) { this->sensitivity = sensitivity; } + void setDeadzone(int16_t deadzone) { this->deadzone = deadzone; } + void setMinDelta(int16_t min_delta) { this->min_delta = min_delta; } + + void setBindToX(BindToX bind_to_x) { this->bind_to_x = bind_to_x; } + + void setDelay(uint32_t delay) { this->delay = delay; } + + void update(uint32_t time); + + int16_t getX() { return x; } + int16_t getY() { return y; } + int16_t getZ() { return z; } + + int16_t getDX(); + int16_t getDY(); +}; + +#endif \ No newline at end of file diff --git a/include/input_mapper.h b/include/input_mapper.h index b63fef8..fe60fb0 100644 --- a/include/input_mapper.h +++ b/include/input_mapper.h @@ -32,8 +32,6 @@ namespace InputMapper bool mapButton(HardwareButtons button, bool value); - // void mapGyro(); - void sendReport(); } diff --git a/src/gyro.cpp b/src/gyro.cpp new file mode 100644 index 0000000..6a7a4a7 --- /dev/null +++ b/src/gyro.cpp @@ -0,0 +1,160 @@ +#include "gyro.h" + +#include + +#include "util_func.h" + +void Gyro::Filter::init(uint8_t size) +{ + this->size = size; + buffer = new int16_t[size]; + for (uint8_t i = 0; i < size; i++) + { + buffer[i] = 0; + } + pointer = 0; + sum = 0; +} + +Gyro::Filter::~Filter() +{ + delete[] buffer; +} + +int16_t Gyro::Filter::filter(int16_t x) +{ + sum -= buffer[pointer]; + sum += x; + buffer[pointer] = x; + + pointer = (pointer + 1) % size; + + return sum / size; +} + +Gyro::Gyro() +{ + invert_x = 1; + invert_y = 1; + invert_z = 1; + + sensitivity = 1.0f; + + time0 = 0; + delay = 0; + + bind_to_x = BIND_X; + + _Enabled = [] { return false; }; +} + +void Gyro::init() +{ + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + mpu.initialize(); + Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + + /* + mpu.setXGyroOffset(0); + mpu.setYGyroOffset(0); + mpu.setZGyroOffset(0); + */ + mpu.CalibrateGyro(6); + + uint8_t filter_size = 5; + x_filter.init(filter_size); + y_filter.init(filter_size); + z_filter.init(filter_size); + + //mpu.setIntDataReadyEnabled(1); + + //mpu.setDLPFMode(MPU6050_DLPF_BW_5); +} + +void Gyro::update(uint32_t time) +{ + if (Enabled()) + { + float dt = time - time0; + if (dt > delay) + { + time0 = time; + + int16_t x, y, z; + + mpu.getRotation(&x, &y, &z); + + //x = x_filter.filter(x) * invert_x; + //y = y_filter.filter(y) * invert_y; + //z = z_filter.filter(z) * invert_z; + + dt /= 1000.0f; + + this->x = x * sensitivity * invert_x * dt; + this->y = y * sensitivity * invert_y * dt; + this->z = z * sensitivity * invert_z * dt; + } + } +} + +int16_t Gyro::getDX() +{ + int32_t dx; + + switch (bind_to_x) + { + case BIND_X: + dx = x; + break; + + case BIND_Z: + dx = z; + break; + + case BIND_XZ: + dx = x + z; + } + + if (dx > -deadzone && dx < deadzone) + { + dx = 0; + } + else + if (dx >= deadzone && dx < min_delta) + { + dx = min_delta; + } + else + if (dx <= -deadzone && dx > -min_delta) + { + dx = -min_delta; + } + + return clamp(dx, -32767, 32767); +} + +int16_t Gyro::getDY() +{ + + if (y > -deadzone && y < deadzone) + { + y = 0; + } + else + if (y >= deadzone && y < min_delta) + { + y = min_delta; + } + else + if (y <= -deadzone && y > -min_delta) + { + y = -min_delta; + } + + return clamp(y, -32767, 32767); +} diff --git a/src/input_mapper.cpp b/src/input_mapper.cpp index 145a5f5..9166421 100644 --- a/src/input_mapper.cpp +++ b/src/input_mapper.cpp @@ -2,6 +2,7 @@ #include "usb_device.h" #include "touch_controls_all.h" +#include "gyro.h" #include @@ -29,6 +30,9 @@ namespace InputMapper }; const uint8_t num_controls = sizeof(tcontrols) / sizeof(TouchControl*[2]); + + Gyro gyro; + /* uint16_t button_map[] = { @@ -170,6 +174,18 @@ namespace InputMapper } } + gyro.init(); + gyro.setEnabledCallback([]{ return tjoystick_right.getTouching() > TouchControl::CT_NONE; }); + //gyro.setEnabledCallback([]{ return xinput_counter[USB_Device::BUMPER_RIGHT] > 0; }); + gyro.setMappedId(1); + //gyro.setInvertX(); + gyro.setInvertY(); + gyro.setSensitivity(1.0f); + gyro.setDeadzone(0); + gyro.setMinDelta(1000); + gyro.setBindToX(Gyro::BIND_XZ); + gyro.setDelay(1000); + device.begin(); } @@ -268,6 +284,8 @@ namespace InputMapper } } + + gyro.update(time); } void mapTriggers(uint32_t value[2]) @@ -383,22 +401,29 @@ namespace InputMapper } } + if (gyro.Enabled()) + { + dx[gyro.getMappedId()] += gyro.getDX(); + dy[gyro.getMappedId()] += gyro.getDY(); + } + for (int j = 0; j < 2; ++j) { if (count[j] > 0) { x[j] = x[j] / count[j] + dx[j]; y[j] = y[j] / count[j] + dy[j]; - - x[j] = clamp(x[j], USB_Device::usb_joystick_x - USB_Device::usb_joystick_r, USB_Device::usb_joystick_x + USB_Device::usb_joystick_r); - y[j] = clamp(y[j], USB_Device::usb_joystick_y - USB_Device::usb_joystick_r, USB_Device::usb_joystick_y + USB_Device::usb_joystick_r); - - device.joystick(j, x[j], y[j]); } else { - device.joystick(j, USB_Device::usb_joystick_x + dx[j], USB_Device::usb_joystick_y + dy[j]); + x[j] = USB_Device::usb_joystick_x + dx[j]; + y[j] = USB_Device::usb_joystick_y + dy[j]; } + + x[j] = clamp(x[j], USB_Device::usb_joystick_x - USB_Device::usb_joystick_r, USB_Device::usb_joystick_x + USB_Device::usb_joystick_r); + y[j] = clamp(y[j], USB_Device::usb_joystick_y - USB_Device::usb_joystick_r, USB_Device::usb_joystick_y + USB_Device::usb_joystick_r); + + device.joystick(j, x[j], y[j]); } device.sendReport(); diff --git a/src/main.cpp b/src/main.cpp index d3be764..a532a9d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -25,6 +25,8 @@ uint8_t button_state[sizeof(pin_button) / 2] = {0}; const uint8_t pin_trackpad_data[2] = {PB5, PB9}; const uint8_t pin_trackpad_clock[2] = {PB4, PB8}; +const uint8_t gyro_int = PC14; + TrackPad trackpad[2]; // 0 - left, 1 - right int32_t trackpad_maxX, trackpad_maxY; @@ -55,7 +57,7 @@ void setup() trackpad_maxX = trackpad[0].getMaxX(); trackpad_maxY = trackpad[0].getMaxY(); - + InputMapper::begin(); // Turn off LED @@ -114,8 +116,6 @@ void loop() } } } - - InputMapper::update(micros()); uint32_t triggers[] = {analogRead(pin_trigger[0]), analogRead(pin_trigger[1])}; InputMapper::mapTriggers(triggers); @@ -132,5 +132,7 @@ void loop() } } + InputMapper::update(micros()); + InputMapper::sendReport(); }