Загрузить файлы в «/»
This commit is contained in:
380
musework.ino
Normal file
380
musework.ino
Normal file
@@ -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 - оптимально для плавности
|
||||
}
|
||||
Reference in New Issue
Block a user