#include <Servo.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <MPU6050.h>
#include <EEPROM.h>
// Піни для двигунів
#define MOTOR_LEFT_EN 5
#define MOTOR_RIGHT_EN 6
#define MOTOR_LEFT_FORWARD 7
#define MOTOR_LEFT_BACKWARD 8
#define MOTOR_RIGHT_FORWARD 3
#define MOTOR_RIGHT_BACKWARD 4
#define GYROSCOPE_SDA A4
#define GYROSCOPE_SCL A5
#define SERVO_PIN 9
// Піни для Bluetooth
#define BT_RX 10
#define BT_TX 11
// Пін для датчика відстані
#define ECHO_PIN A0
#define TRIG_PIN 2
Servo servo;
MPU6050 gyro;
SoftwareSerial bluetooth(BT_RX, BT_TX);
float Kp = 1.0, Ki = 0, Kd = 0;
int controlMode = 0;
// Змінні для зберігання станів
bool isRunning = false;
String inputString = "";
bool stringComplete = false;
// Таймери
unsigned long previousMillis = 0;
const long interval = 50; // Інтервал часу для оновлення руху
unsigned long servoMoveStartTime = 0;
bool servoTurning = false;
const long servoTurnDuration = 500; // Час на поворот серво
// PID контролер змінні
float setPoint = 0; // Цільовий кут
float input = 0; // Поточний кут
float output = 0; // Вихідний сигнал контролера
float integral = 0;
float previousError = 0;
void setup() {
pinMode(MOTOR_LEFT_EN, OUTPUT);
pinMode(MOTOR_RIGHT_EN, OUTPUT);
pinMode(MOTOR_LEFT_FORWARD, OUTPUT);
pinMode(MOTOR_LEFT_BACKWARD, OUTPUT);
pinMode(MOTOR_RIGHT_FORWARD, OUTPUT);
pinMode(MOTOR_RIGHT_BACKWARD, OUTPUT);
servo.attach(SERVO_PIN);
bluetooth.begin(9600);
Wire.begin();
gyro.initialize();
// Зчитування PID параметрів з EEPROM
EEPROM.get(0, Kp);
EEPROM.get(4, Ki);
EEPROM.get(8, Kd);
}
void loop() {
unsigned long currentMillis = millis();
if (isRunning) {
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
long distance = getDistance();
if (distance < 20 && !servoTurning) {
// Поворот серво
servo.write(90);
servoTurning = true;
// Рух в новому напрямку
servoMoveStartTime = currentMillis;
} else if (servoTurning) {
if (currentMillis - servoMoveStartTime >= servoTurnDuration) {
servoTurning = false;
// Рух в новому напрямку
moveRobot(200, 200);
}
} else {
// Основний рух прямо
moveRobot(255, 255);
}
} else {
moveRobot(0, 0);
}
processBluetoothCommand();
}
}
// Функція для вимірювання відстані
long getDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
long duration = pulseIn(ECHO_PIN, HIGH);
long distance = (duration / 2) / 29.1;
return distance;
}
// Функція для керування рухом робота
void moveRobot(int leftSpeed, int rightSpeed) {
if (leftSpeed > 0) {
analogWrite(MOTOR_LEFT_FORWARD, leftSpeed);
analogWrite(MOTOR_LEFT_BACKWARD, 0);
} else {
analogWrite(MOTOR_LEFT_FORWARD, 0);
analogWrite(MOTOR_LEFT_BACKWARD, -leftSpeed);
}
if (rightSpeed > 0) {
analogWrite(MOTOR_RIGHT_FORWARD, rightSpeed);
analogWrite(MOTOR_RIGHT_BACKWARD, 0);
} else {
analogWrite(MOTOR_RIGHT_FORWARD, 0);
analogWrite(MOTOR_RIGHT_BACKWARD, -rightSpeed);
}
}
// Функція для зчитування даних з Bluetooth
void serialEvent() {
while (bluetooth.available()) {
char inChar = (char)bluetooth.read();
inputString += inChar;
if (inChar == '\n') {
stringComplete = true;
}
}
}
// Функція для обробки Bluetooth команд
void processBluetoothCommand() {
if (stringComplete) {
if (inputString.startsWith("start")) {
isRunning = true;
} else if (inputString.startsWith("stop")) {
isRunning = false;
} else if (inputString.startsWith("pControl")) {
controlMode = 0;
} else if (inputString.startsWith("pdControl")) {
controlMode = 1;
} else if (inputString.startsWith("piControl")) {
controlMode = 2;
} else if (inputString.startsWith("pidControl")) {
controlMode = 3;
} else if (inputString.startsWith("Kp=")) {
Kp = inputString.substring(3).toFloat();
EEPROM.put(0, Kp);
} else if (inputString.startsWith("Ki=")) {
Ki = inputString.substring(3).toFloat();
EEPROM.put(4, Ki);
} else if (inputString.startsWith("Kd=")) {
Kd = inputString.substring(3).toFloat();
EEPROM.put(8, Kd);
}
inputString = "";
stringComplete = false;
}
}
float computePID(float input, float setPoint) {
float error = setPoint - input;
integral += error * interval / 1000.0;
float derivative = (error - previousError) / (interval / 1000.0);
previousError = error;
float output;
switch (controlMode) {
case 0: // P
output = Kp * error;
break;
case 1: // PD
output = Kp * error + Kd * derivative;
break;
case 2: // PI
output = Kp * error + Ki * integral;
break;
case 3: // PID
output = Kp * error + Ki * integral + Kd * derivative;
break;
}
return output;
}