Загрузить файлы в «/»

musework.ino

@@ -0,0 +1,380 @@
+#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 - оптимально для плавности
+}