380 lines
13 KiB
C++
380 lines
13 KiB
C++
#include <Wire.h>
|
||
#include <MPU6050.h>
|
||
#include <BleCompositeHID.h>
|
||
#include <MouseDevice.h>
|
||
#include <KeyboardDevice.h>
|
||
|
||
#define BUTTON_PIN 4
|
||
#define PAUSE_BUTTON_PIN 5
|
||
#define LED_PIN 2
|
||
|
||
MPU6050 mpu;
|
||
KeyboardDevice* keyboard;
|
||
MouseDevice* mouse;
|
||
BleCompositeHID compositeHID("fire_extinguisher_1", "Misfit", 100);
|
||
|
||
// =================== ПЕРЕМЕННЫЕ ===================
|
||
float current_yaw_angle = 0.0;
|
||
float current_pitch_angle = 0.0;
|
||
float gyroZ_offset = 0.0;
|
||
float accelX_offset = 0.0;
|
||
bool is_calibrated = false;
|
||
unsigned long last_micros = 0;
|
||
const float GYRO_SCALE = 65.5;
|
||
|
||
// =================== НАСТРОЙКИ СКОРОСТИ ===================
|
||
float YAW_SPEED_GAIN = 1.4; // x2 от 0.8 (горизонталь)
|
||
float PITCH_SPEED_GAIN = 1.6; // x1.2 от 0.6 (вертикаль)
|
||
const int MAX_SPEED = 50;
|
||
|
||
// Для вычисления изменения угла
|
||
float prev_yaw_angle = 0.0;
|
||
float prev_pitch_angle = 0.0;
|
||
|
||
// Улучшенное сглаживание против дрожания
|
||
float smoothed_speedX = 0;
|
||
float smoothed_speedY = 0;
|
||
float smoothing = 0.4; // Более сильное сглаживание
|
||
|
||
// Дополнительный фильтр для мелких движений
|
||
const float MIN_MOVEMENT_THRESHOLD = 0.8; // Игнорировать очень мелкие изменения
|
||
|
||
// =================== ИСПРАВЛЕНИЯ ДЛЯ НАКЛОНА ===================
|
||
const float PITCH_DEADZONE = 1.2; // Мертвая зона для наклона (градусы)
|
||
const float PITCH_RESET_THRESHOLD = 1.0; // Порог сброса накопленной ошибки
|
||
float last_valid_pitch = 0.0;
|
||
bool pitch_was_reset = false;
|
||
|
||
// =================== КАЛИБРОВКА ===================
|
||
const int CALIB_SAMPLES = 250;
|
||
|
||
void setup() {
|
||
Serial.begin(115200);
|
||
pinMode(LED_PIN, OUTPUT);
|
||
pinMode(BUTTON_PIN, INPUT_PULLUP);
|
||
pinMode(PAUSE_BUTTON_PIN, INPUT_PULLUP);
|
||
|
||
Wire.begin(21, 22);
|
||
Wire.setClock(400000);
|
||
|
||
mpu.initialize();
|
||
|
||
// Оптимальные настройки для уменьшения шума
|
||
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //
|
||
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
|
||
mpu.setDLPFMode(MPU6050_DLPF_BW_20); // фильтр против дрожания
|
||
|
||
KeyboardConfiguration keyboardConfig;
|
||
keyboardConfig.setUseMediaKeys(true);
|
||
keyboard = new KeyboardDevice(keyboardConfig);
|
||
|
||
MouseConfiguration mouseConfig;
|
||
mouseConfig.setAutoReport(false);
|
||
mouse = new MouseDevice(mouseConfig);
|
||
|
||
compositeHID.addDevice(keyboard);
|
||
compositeHID.addDevice(mouse);
|
||
compositeHID.begin();
|
||
|
||
for(int i = 0; i < 3; i++) {
|
||
digitalWrite(LED_PIN, HIGH);
|
||
delay(100);
|
||
digitalWrite(LED_PIN, LOW);
|
||
delay(100);
|
||
}
|
||
|
||
delay(1000);
|
||
calibrateSensors();
|
||
|
||
last_micros = micros();
|
||
prev_yaw_angle = current_yaw_angle;
|
||
prev_pitch_angle = current_pitch_angle;
|
||
|
||
Serial.println("=== УЛУЧШЕННАЯ ВЕРСИЯ С ИСПРАВЛЕНИЯМИ ===");
|
||
Serial.println("1. Инверсия осей (лево-право)");
|
||
Serial.println("2. Убрано дрожание");
|
||
Serial.println("3. Исправлено 'залипание' наклона");
|
||
Serial.println("4. Скорость X: x2 | Y: x1.2");
|
||
}
|
||
|
||
void calibrateSensors() {
|
||
Serial.println("Точная калибровка...");
|
||
digitalWrite(LED_PIN, LOW);
|
||
|
||
long gz_sum = 0;
|
||
long ax_sum = 0;
|
||
|
||
for(int i = 0; i < CALIB_SAMPLES; i++) {
|
||
int16_t ax, ay, az, gx, gy, gz;
|
||
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
||
|
||
// Отбрасываем выбросы
|
||
if (abs(gz) < 30000 && abs(ax) < 30000) {
|
||
gz_sum += gz;
|
||
ax_sum += ax;
|
||
}
|
||
|
||
delay(4);
|
||
}
|
||
|
||
gyroZ_offset = gz_sum / CALIB_SAMPLES;
|
||
accelX_offset = ax_sum / CALIB_SAMPLES;
|
||
|
||
// Сброс
|
||
current_yaw_angle = 0;
|
||
current_pitch_angle = 0;
|
||
prev_yaw_angle = 0;
|
||
prev_pitch_angle = 0;
|
||
smoothed_speedX = 0;
|
||
smoothed_speedY = 0;
|
||
last_valid_pitch = 0.0;
|
||
pitch_was_reset = true;
|
||
|
||
is_calibrated = true;
|
||
|
||
digitalWrite(LED_PIN, HIGH);
|
||
Serial.print("Калибровка OK. Offsets: GZ=");
|
||
Serial.print(gyroZ_offset);
|
||
Serial.print(" AX=");
|
||
Serial.println(accelX_offset);
|
||
}
|
||
|
||
// Функция для сильного подавления шума
|
||
float applyDeadzoneAndFilter(float value, float &filtered, float threshold) {
|
||
// Игнорируем очень маленькие значения
|
||
if (abs(value) < threshold) {
|
||
return 0;
|
||
}
|
||
|
||
// Экспоненциальное сглаживание
|
||
filtered = filtered * 0.7 + value * 0.3;
|
||
return filtered;
|
||
}
|
||
|
||
// Новая функция для обработки наклона с исправлением "залипания"
|
||
float processPitchWithStuckFix(float raw_pitch) {
|
||
static float last_stable_pitch = 0.0;
|
||
static unsigned long last_reset_time = 0;
|
||
|
||
// Если изменение слишком маленькое - игнорируем (мертвая зона)
|
||
if (abs(raw_pitch - last_stable_pitch) < PITCH_DEADZONE) {
|
||
return last_stable_pitch;
|
||
}
|
||
|
||
// Если наклонились достаточно сильно - обновляем значение
|
||
last_stable_pitch = raw_pitch;
|
||
last_reset_time = millis();
|
||
|
||
return raw_pitch;
|
||
}
|
||
|
||
void loop() {
|
||
if (Serial.available() > 0) {
|
||
String cmd = Serial.readStringUntil('\n');
|
||
cmd.trim();
|
||
|
||
if (cmd.startsWith("y")) {
|
||
float val = cmd.substring(1).toFloat();
|
||
if (val >= 0.5 && val <= 3.0) {
|
||
YAW_SPEED_GAIN = val;
|
||
Serial.print("Скорость вращения: ");
|
||
Serial.println(YAW_SPEED_GAIN);
|
||
}
|
||
} else if (cmd.startsWith("p")) {
|
||
float val = cmd.substring(1).toFloat();
|
||
if (val >= 0.5 && val <= 3.0) {
|
||
PITCH_SPEED_GAIN = val;
|
||
Serial.print("Скорость наклона: ");
|
||
Serial.println(PITCH_SPEED_GAIN);
|
||
}
|
||
} else if (cmd == "cal") {
|
||
calibrateSensors();
|
||
} else if (cmd == "reset") {
|
||
current_yaw_angle = 0;
|
||
current_pitch_angle = 0;
|
||
prev_yaw_angle = 0;
|
||
prev_pitch_angle = 0;
|
||
smoothed_speedX = 0;
|
||
smoothed_speedY = 0;
|
||
last_valid_pitch = 0.0;
|
||
pitch_was_reset = true;
|
||
Serial.println("Полный сброс");
|
||
}
|
||
}
|
||
|
||
if (!compositeHID.isConnected()) {
|
||
static unsigned long lastBlink = 0;
|
||
static bool ledState = LOW;
|
||
if (millis() - lastBlink > 500) {
|
||
lastBlink = millis();
|
||
ledState = !ledState;
|
||
digitalWrite(LED_PIN, ledState);
|
||
}
|
||
delay(50);
|
||
return;
|
||
}
|
||
|
||
digitalWrite(LED_PIN, HIGH);
|
||
|
||
// Пауза
|
||
bool currentPause = digitalRead(PAUSE_BUTTON_PIN);
|
||
static bool lastPauseState = HIGH;
|
||
static bool mousePaused = false;
|
||
|
||
if (currentPause != lastPauseState) {
|
||
delay(20);
|
||
if (currentPause == LOW) {
|
||
mousePaused = !mousePaused;
|
||
Serial.println(mousePaused ? "Пауза ВКЛ" : "Пауза ВЫКЛ");
|
||
}
|
||
lastPauseState = currentPause;
|
||
}
|
||
|
||
if (mousePaused || !is_calibrated) {
|
||
delay(50);
|
||
return;
|
||
}
|
||
|
||
// === ОБРАБОТКА ===
|
||
|
||
unsigned long current_micros = micros();
|
||
float dt = (current_micros - last_micros) / 1000000.0;
|
||
last_micros = current_micros;
|
||
|
||
if (dt > 0.05) dt = 0.05;
|
||
if (dt < 0.001) dt = 0.001;
|
||
|
||
// Чтение данных
|
||
int16_t ax, ay, az, gx, gy, gz;
|
||
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
||
|
||
// 1. YAW из гироскопа Z
|
||
float gyroZ = (gz - gyroZ_offset) / GYRO_SCALE;
|
||
current_yaw_angle += gyroZ * dt;
|
||
|
||
// 2. PITCH из акселерометра X (наклон вперед/назад) - ИСПРАВЛЕННАЯ ВЕРСИЯ
|
||
float accelX = (ax - accelX_offset) / 16384.0;
|
||
|
||
// Защита от переполнения
|
||
if (accelX > 0.99) accelX = 0.99;
|
||
if (accelX < -0.99) accelX = -0.99;
|
||
|
||
float new_pitch = asin(accelX) * 180.0 / PI;
|
||
|
||
// Применяем исправление от "залипания"
|
||
new_pitch = processPitchWithStuckFix(new_pitch);
|
||
|
||
// Сильная фильтрация pitch против дрожания
|
||
static float filtered_pitch = 0;
|
||
static float pitch_filter_alpha = 0.15; // Оптимальный баланс
|
||
filtered_pitch = filtered_pitch * (1-pitch_filter_alpha) + new_pitch * pitch_filter_alpha;
|
||
current_pitch_angle = filtered_pitch;
|
||
|
||
// Автоматический сброс наклона при возвращении в нейтральное положение
|
||
if (abs(current_pitch_angle) < PITCH_RESET_THRESHOLD && !pitch_was_reset) {
|
||
current_pitch_angle = 0;
|
||
filtered_pitch = 0;
|
||
last_valid_pitch = 0;
|
||
pitch_was_reset = true;
|
||
} else if (abs(current_pitch_angle) >= PITCH_RESET_THRESHOLD) {
|
||
pitch_was_reset = false;
|
||
}
|
||
|
||
// 3. ВЫЧИСЛЕНИЕ ИЗМЕНЕНИЯ УГЛОВ
|
||
float yaw_change = current_yaw_angle - prev_yaw_angle;
|
||
float pitch_change = current_pitch_angle - prev_pitch_angle;
|
||
|
||
// Сохраняем для следующего цикла
|
||
prev_yaw_angle = current_yaw_angle;
|
||
prev_pitch_angle = current_pitch_angle;
|
||
|
||
// 4. ИНВЕРСИЯ ОСЕЙ И ПРИМЕНЕНИЕ СКОРОСТНЫХ КОЭФФИЦИЕНТОВ
|
||
// ИНВЕРСИЯ ЛЕВО-ПРАВО: меняем знак для оси X (поворот влево = курсор вправо)
|
||
float raw_speedX = yaw_change * YAW_SPEED_GAIN * 12.0; // Инверсия лево-право
|
||
|
||
// ИНВЕРСИЯ ВПЕРЕД-НАЗАД: меняем знак для оси Y (наклон вперед = курсор вверх)
|
||
float raw_speedY = -pitch_change * PITCH_SPEED_GAIN * 12.0; // Инверсия вперед-назад
|
||
|
||
// 5. ПОДАВЛЕНИЕ ДРОЖАНИЯ
|
||
// Фильтрация очень мелких движений
|
||
static float filtered_speedX = 0;
|
||
static float filtered_speedY = 0;
|
||
|
||
raw_speedX = applyDeadzoneAndFilter(raw_speedX, filtered_speedX, MIN_MOVEMENT_THRESHOLD);
|
||
raw_speedY = applyDeadzoneAndFilter(raw_speedY, filtered_speedY, MIN_MOVEMENT_THRESHOLD);
|
||
|
||
// Дополнительное экспоненциальное сглаживание
|
||
smoothed_speedX = smoothed_speedX * smoothing + raw_speedX * (1-smoothing);
|
||
smoothed_speedY = smoothed_speedY * smoothing + raw_speedY * (1-smoothing);
|
||
|
||
// Округление до целых с порогом
|
||
int mouseX = 0;
|
||
int mouseY = 0;
|
||
|
||
if (abs(smoothed_speedX) > 0.5) mouseX = round(smoothed_speedX);
|
||
if (abs(smoothed_speedY) > 0.5) mouseY = round(smoothed_speedY);
|
||
|
||
// Ограничение максимальной скорости
|
||
mouseX = constrain(mouseX, -MAX_SPEED, MAX_SPEED);
|
||
mouseY = constrain(mouseY, -MAX_SPEED, MAX_SPEED);
|
||
|
||
// 6. ДИАГНОСТИКА
|
||
static unsigned long lastDebug = 0;
|
||
if (millis() - lastDebug > 1200) {
|
||
lastDebug = millis();
|
||
|
||
Serial.print("ΔX: ");
|
||
Serial.print(yaw_change, 2);
|
||
Serial.print("° -> speed: ");
|
||
Serial.print(raw_speedX, 1);
|
||
Serial.print(" -> mouseX: ");
|
||
Serial.print(mouseX);
|
||
|
||
Serial.print(" | ΔY: ");
|
||
Serial.print(pitch_change, 2);
|
||
Serial.print("° -> speed: ");
|
||
Serial.print(raw_speedY, 1);
|
||
Serial.print(" -> mouseY: ");
|
||
Serial.print(mouseY);
|
||
|
||
Serial.print(" | Pitch: ");
|
||
Serial.print(current_pitch_angle, 1);
|
||
Serial.print("°");
|
||
|
||
Serial.println();
|
||
}
|
||
|
||
// 7. ДВИЖЕНИЕ МЫШИ
|
||
if (mouseX != 0 || mouseY != 0) {
|
||
mouse->mouseMove(mouseX, mouseY);
|
||
mouse->sendMouseReport();
|
||
}
|
||
|
||
// 8. ОБРАБОТКА КНОПКИ
|
||
bool currentButton = digitalRead(BUTTON_PIN);
|
||
static bool lastButtonState = HIGH;
|
||
static bool mousePressed = false;
|
||
|
||
if (currentButton != lastButtonState) {
|
||
delay(25);
|
||
if (currentButton == LOW && !mousePressed) {
|
||
mouse->mousePress(MOUSE_LOGICAL_LEFT_BUTTON);
|
||
mouse->sendMouseReport();
|
||
mousePressed = true;
|
||
|
||
keyboard->keyPress(KEY_1);
|
||
keyboard->sendKeyReport();
|
||
delay(40);
|
||
keyboard->keyRelease(KEY_1);
|
||
keyboard->sendKeyReport();
|
||
|
||
} else if (currentButton == HIGH && mousePressed) {
|
||
mouse->mouseRelease(MOUSE_LOGICAL_LEFT_BUTTON);
|
||
mouse->sendMouseReport();
|
||
mousePressed = false;
|
||
}
|
||
lastButtonState = currentButton;
|
||
}
|
||
|
||
delay(8); // ~83Hz - оптимально для плавности
|
||
} |