Katedra Transportu Kolejowego Wydział Transportu i Inżynierii Lotniczej

				
					// Adam Mańka (adam.manka(at)polsl.pl
// Rownowaznia aerodynamiczna - PID
// Arduino Uno + L298N + MPU6050
// Wersja dydaktyczna uproszczona
//
// Biblioteka: MPU6050 by Electronic Cats
//
// Komendy Serial Monitor 115200, Newline:
// a       - uzbroj silniki
// s       - stop
// p4.0    - Kp
// i0.01   - Ki
// d1.0    - Kd
// b100    - bazowy PWM
// m60     - max korekta PID
// sp0     - kat zadany
// sg1     - normalny znak sterowania
// sg-1    - odwroc znak sterowania
// z       - wyzeruj aktualny kat
// ?       - pokaz komendy

#include <Wire.h>
#include <MPU6050.h>
#include <math.h>

MPU6050 mpu;

// ===== Piny L298N =====
const byte ENA = 5;   // PWM - lewy silnik
const byte IN1 = 7;
const byte IN2 = 8;

const byte ENB = 6;   // PWM - prawy silnik
const byte IN3 = 9;
const byte IN4 = 10;

// ===== Skale MPU6050 =====
const float ACC_SCALE = 16384.0;   // +/-2g
const float GYRO_SCALE = 131.0;    // +/-250 deg/s
const float RAD_TO_DEG_LOCAL = 57.2957795;

// ===== PID =====
float Kp = 3.0;
float Ki = 0.0;
float Kd = 0.8;

float setAngle = 0.0;       // kat zadany [deg]
float basePWM = 90.0;       // podstawowa predkosc silnikow
float maxCorr = 50.0;       // maksymalna korekta PID
int controlSign = 1;        // jesli ucieka, wpisz sg-1

// ===== Pomiar kata =====
float angle = 0.0;
float lastAngle = 0.0;
float zeroRoll = 0.0;
float gyroOffsetX = 0.0;

const float alpha = 0.96;   // filtr komplementarny
const int LOOP_MS = 10;     // petla 100 Hz

// ===== Zmienne PID =====
float integral = 0.0;
float pidOut = 0.0;

bool armed = false;

unsigned long lastLoopMs = 0;
unsigned long lastMicros = 0;
unsigned long lastPrintMs = 0;

// ===== Komendy z Serial Monitor =====
char cmd[20];
byte cmdIndex = 0;

// ------------------------------------------------------------

void printCommands()
{
  Serial.println();
  Serial.println("=== KOMENDY ===");
  Serial.println("a       - uzbroj silniki");
  Serial.println("s       - stop");
  Serial.println("p4.0    - ustaw Kp");
  Serial.println("i0.01   - ustaw Ki");
  Serial.println("d1.0    - ustaw Kd");
  Serial.println("b100    - bazowy PWM silnikow");
  Serial.println("m60     - maksymalna korekta PID");
  Serial.println("sp0     - kat zadany");
  Serial.println("sg1     - normalny znak sterowania");
  Serial.println("sg-1    - odwroc znak sterowania");
  Serial.println("z       - aktualny kat jako zero");
  Serial.println("?       - pokaz komendy");
  Serial.println();
}

// ------------------------------------------------------------

byte readReg(byte reg)
{
  Wire.beginTransmission(0x68);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 1);

  if (Wire.available()) return Wire.read();
  return 0xFF;
}

// ------------------------------------------------------------

float calcRollDeg(int16_t ayRaw, int16_t azRaw)
{
  float ay = ayRaw / ACC_SCALE;
  float az = azRaw / ACC_SCALE;

  return atan2(ay, az) * RAD_TO_DEG_LOCAL;
}

// ------------------------------------------------------------

void setMotor(byte en, byte inA, byte inB, int pwm)
{
  pwm = constrain(pwm, 0, 255);

  if (pwm == 0)
  {
    digitalWrite(inA, LOW);
    digitalWrite(inB, LOW);
    analogWrite(en, 0);
  }
  else
  {
    digitalWrite(inA, HIGH);
    digitalWrite(inB, LOW);
    analogWrite(en, pwm);
  }
}

// ------------------------------------------------------------

void setMotors(int leftPWM, int rightPWM)
{
  setMotor(ENA, IN1, IN2, leftPWM);
  setMotor(ENB, IN3, IN4, rightPWM);
}

// ------------------------------------------------------------

void stopMotors()
{
  setMotors(0, 0);
}

// ------------------------------------------------------------

void countdownBeforeCalibration()
{
  Serial.println("Ustaw rownowaznie poziomo.");
  Serial.println("Kalibracja rozpocznie sie za:");

  for (int s = 5; s > 0; s--)
  {
    Serial.print(s);
    Serial.println(" s");
    delay(1000);
  }

  Serial.println("Start kalibracji...");
}

// ------------------------------------------------------------

void calibrateMPU()
{
  const int calibTimeSec = 5;

  long sumGX = 0;
  float sumRoll = 0.0;
  int samples = 0;

  int16_t axRaw, ayRaw, azRaw;
  int16_t gxRaw, gyRaw, gzRaw;

  unsigned long startTime = millis();
  unsigned long endTime = startTime + calibTimeSec * 1000UL;

  int lastShownSecond = -1;

  while (millis() < endTime)
  {
    mpu.getMotion6(&axRaw, &ayRaw, &azRaw, &gxRaw, &gyRaw, &gzRaw);

    sumGX += gxRaw;
    sumRoll += calcRollDeg(ayRaw, azRaw);
    samples++;

    int secondsLeft = (endTime - millis() + 999) / 1000;

    if (secondsLeft != lastShownSecond)
    {
      lastShownSecond = secondsLeft;
      Serial.print("Do konca kalibracji: ");
      Serial.print(secondsLeft);
      Serial.println(" s");
    }

    delay(5);
  }

  gyroOffsetX = (sumGX / (float)samples) / GYRO_SCALE;
  zeroRoll = sumRoll / (float)samples;

  angle = 0.0;
  lastAngle = 0.0;
  integral = 0.0;

  Serial.println("Kalibracja zakonczona.");
  Serial.print("Liczba probek: ");
  Serial.println(samples);
  Serial.print("gyroOffsetX = ");
  Serial.println(gyroOffsetX, 4);
  Serial.print("zeroRoll = ");
  Serial.println(zeroRoll, 2);
  Serial.println();
}

// ------------------------------------------------------------

void printSettings()
{
  Serial.print("Kp=");
  Serial.print(Kp);
  Serial.print(" Ki=");
  Serial.print(Ki);
  Serial.print(" Kd=");
  Serial.print(Kd);
  Serial.print(" basePWM=");
  Serial.print(basePWM);
  Serial.print(" maxCorr=");
  Serial.print(maxCorr);
  Serial.print(" setAngle=");
  Serial.print(setAngle);
  Serial.print(" sign=");
  Serial.println(controlSign);
}

// ------------------------------------------------------------

void parseCommand(char *c)
{
  if (strcmp(c, "a") == 0)
  {
    armed = true;
    integral = 0.0;
    lastAngle = angle;
    Serial.println("UZBROJONO");
  }
  else if (strcmp(c, "s") == 0)
  {
    armed = false;
    stopMotors();
    integral = 0.0;
    Serial.println("STOP");
  }
  else if (strcmp(c, "z") == 0)
  {
    zeroRoll += angle;
    angle = 0.0;
    lastAngle = 0.0;
    integral = 0.0;
    Serial.println("Aktualny kat ustawiony jako zero.");
  }
  else if (strncmp(c, "sp", 2) == 0)
  {
    setAngle = atof(c + 2);
  }
  else if (strncmp(c, "sg", 2) == 0)
  {
    int v = atoi(c + 2);
    controlSign = (v < 0) ? -1 : 1;
  }
  else if (c[0] == 'p')
  {
    Kp = atof(c + 1);
  }
  else if (c[0] == 'i')
  {
    Ki = atof(c + 1);
  }
  else if (c[0] == 'd')
  {
    Kd = atof(c + 1);
  }
  else if (c[0] == 'b')
  {
    basePWM = constrain(atof(c + 1), 0, 255);
  }
  else if (c[0] == 'm')
  {
    maxCorr = constrain(atof(c + 1), 0, 255);
  }
  else if (c[0] == '?')
  {
    printCommands();
  }

  printSettings();
}

// ------------------------------------------------------------

void handleSerial()
{
  while (Serial.available())
  {
    char ch = Serial.read();

    if (ch == '\n' || ch == '\r')
    {
      if (cmdIndex > 0)
      {
        cmd[cmdIndex] = '\0';
        parseCommand(cmd);
        cmdIndex = 0;
      }
    }
    else
    {
      if (cmdIndex < sizeof(cmd) - 1)
      {
        cmd[cmdIndex++] = ch;
      }
    }
  }
}

// ------------------------------------------------------------

void setup()
{
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  stopMotors();

  Serial.begin(115200);
  delay(1000);

  Serial.println("Rownowaznia aerodynamiczna - PID");
  Serial.println("Arduino Uno + L298N + MPU6050");

  Wire.begin();
  Wire.setClock(100000);

  byte who = readReg(0x75);
  Serial.print("WHO_AM_I = 0x");
  if (who < 16) Serial.print("0");
  Serial.println(who, HEX);

  mpu.initialize();

  Serial.print("testConnection(): ");
  if (mpu.testConnection())
  {
    Serial.println("OK");
  }
  else
  {
    Serial.println("FAIL - ale probujemy dalej");
  }

  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  mpu.setDLPFMode(MPU6050_DLPF_BW_20);

  printCommands();

  countdownBeforeCalibration();
  calibrateMPU();

  Serial.println("Gotowe.");
  Serial.println("Najpierw sprawdz wykres kata bez silnikow.");
  Serial.println("Potem wpisz: a");
  Serial.println();

  lastLoopMs = millis();
  lastMicros = micros();
}

// ------------------------------------------------------------

void loop()
{
  handleSerial();

  unsigned long nowMs = millis();

  if (nowMs - lastLoopMs < LOOP_MS) return;
  lastLoopMs = nowMs;

  unsigned long nowUs = micros();
  float dt = (nowUs - lastMicros) / 1000000.0;
  lastMicros = nowUs;

  if (dt <= 0.0 || dt > 0.1)
  {
    dt = LOOP_MS / 1000.0;
  }

  int16_t axRaw, ayRaw, azRaw;
  int16_t gxRaw, gyRaw, gzRaw;

  mpu.getMotion6(&axRaw, &ayRaw, &azRaw, &gxRaw, &gyRaw, &gzRaw);

  float accRoll = calcRollDeg(ayRaw, azRaw) - zeroRoll;
  float gyroX = gxRaw / GYRO_SCALE - gyroOffsetX;

  // Filtr komplementarny
  angle = alpha * (angle + gyroX * dt) + (1.0 - alpha) * accRoll;

  float error = setAngle - angle;
  float dAngle = (angle - lastAngle) / dt;

  if (!armed)
  {
    pidOut = 0.0;
    integral = 0.0;
    stopMotors();
  }
  else
  {
    if (abs(angle) > 45.0)
    {
      armed = false;
      stopMotors();
      Serial.println("STOP: zbyt duzy kat");
      return;
    }

    integral += error * dt;
    integral = constrain(integral, -100.0, 100.0);

    pidOut = Kp * error + Ki * integral - Kd * dAngle;
    pidOut = constrain(pidOut, -maxCorr, maxCorr);

    int leftPWM  = constrain((int)(basePWM + controlSign * pidOut), 0, 255);
    int rightPWM = constrain((int)(basePWM - controlSign * pidOut), 0, 255);

    setMotors(leftPWM, rightPWM);
  }

  lastAngle = angle;

  if (nowMs - lastPrintMs >= 100)
  {
    lastPrintMs = nowMs;

    Serial.print("kat:");
    Serial.print(angle, 2);

    Serial.print(" zad:");
    Serial.print(setAngle, 2);

    Serial.print(" pid:");
    Serial.print(pidOut, 2);

    Serial.print(" gx:");
    Serial.print(gyroX, 2);

    Serial.print(" armed:");
    Serial.println(armed ? 1 : 0);
  }
}