From 8f531dad38b877b46f2616b805c6c96832822512 Mon Sep 17 00:00:00 2001 From: "lyubishev.a" Date: Tue, 17 Feb 2026 15:35:10 +0300 Subject: [PATCH] =?UTF-8?q?=D0=97=D0=B0=D0=B3=D1=80=D1=83=D0=B7=D0=B8?= =?UTF-8?q?=D1=82=D1=8C=20=D1=84=D0=B0=D0=B9=D0=BB=D1=8B=20=D0=B2=20=C2=AB?= =?UTF-8?q?/=C2=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- musework.ino | 380 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 380 insertions(+) create mode 100644 musework.ino diff --git a/musework.ino b/musework.ino new file mode 100644 index 0000000..ac35ec8 --- /dev/null +++ b/musework.ino @@ -0,0 +1,380 @@ +#include +#include +#include +#include +#include + +#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 - оптимально для плавности +} \ No newline at end of file