Arduino.ru

Здравствуйте ,

Подскажите пожалуйста, почему не выполняется функция "pid" в 254 строке кода.

Все данные вхлдные есть. Единственное я заметил, что коэффициент "K" всегда ноль. Хотя в настройках равен 1.0.

#include <Kalman.h>

#include <I2Cdev.h>



  // -------------------------------------------------------------------------------------------------------------------------------------------------------------------------
// ------------  Программа для расчета угла наклона по данным гироскопа и ускорения с использованием фильтра Калмана ----------------------------------
// --------------------------  и PID-регулирование для последовательного управления двумя двигателями драйверами Sabertooth -----------------------------------------------
// -------------------------------------------------------------------------------------------------------------------------------------------------------------------------


#include "Wire.h"
#include <TroykaIMU.h>   // ЭТО Я УЖЕ ОТ СЕБЯ БУДУ ДОБАВЛЯТЬ 
#include <math.h>
#include <SoftwareSerial.h>         // ХЗ Я ЕЩЕ НЕ ПОРЕДЕЛИЛСЯ С ДРАЙВЕРОМ
#include <SabertoothSimplified.h>   // С БИБЛИОТЕКОЙ СООТВЕТСТВЕННО ТОЖЕ

SoftwareSerial SWSerial(NOT_A_PIN, 11);          // RX on no pin (unused), TX on pin 11 (to S1). ЭТО ВСЕ К ТОЙ ЖЕ БИБЛИОТЕКЕ ОТНОСИТСЯ
SabertoothSimplified ST(SWSerial);               // Use SWSerial as the serial port.

Gyroscope gyro;            // СОЗДАЛИ ОБЬЕКТ ДЛЯ РАБОТЫ С ГИРИКОМ
                           // создаём объект для работы с акселерометром
Accelerometer accel;


#define Pin_Lenkung_rechts      12               // Штыревой разъем для команды управления Вправо
#define Pin_Lenkung_links       13               // Штыревой разъем для команды управления ВЛЕВО

#define Pin_Schalter_Box        3                // ПИН КОРОБКИ ВЫКЛЮЧАТЕЛЕЙ ДЛЯ ПЕРЕКЛЮЧЕНИЯ МЕЖДУ СИНХРОНИЗАЦИЕЙ МОТОРОВ И I-УПРАВЛЕНИЕМ

#define Pin_PID_Regelung_P      A1               // Контактный разъем для потенциометра для изменения P-компонента
#define Pin_PID_Regelung_I      A2               // Контактный разъем для потенциометра для изменения I-компонента
#define Pin_PID_Regelung_D      A3               // Контактный разъем для потенциометра для изменения D-компонента


int LoopTime_Soll = 9;                           // Желаемое время измельчения в мс для достижения 100 Гц      
int LoopTime_Angepasst = LoopTime_Soll;          // Последнее время цикла с принудительной паузой
int LoopTime_Bisher = LoopTime_Soll;             // Последнее время цикла без принудительного перерыва
unsigned long LoopTime_Start = 0;		             // ВРЕМЯ НАЧАЛА ЦИКЛА

float Winkel;					                           // Текущий угол наклона
float Winkel_Soll;                               // Уставка угла наклона, т. Е. 0 °
float Winkel_Grenze;                             // Максимально разрешенный угол наклона, над которым Segway выключен   
float Winkel1;


float ACC_angledraga;                            // Угол датчика ускорения ТРОЙКА
float GYRO_ratedraga;                            // Угол датчика Гироскопа Тройка
                           // Угол датчика Гироскопа Тройка

float Kp,Ki,Kd,K;                                // Константы для ПИД-регулирования, Разная деталь, Интегральная часть, Дифференциальная часть, Общая часть
int Motor;                                       // От ПИД-регулятора до управления двигателем
int Motor_rechts, Motor_links;                   // Значения для двух двигателей
float K_Motor_links, K_Motor_rechts;             // Коэффициенты коррекции синхронной работы двух двигателей

int Schalter_Box;                                // Переменная, которая запрашивает положение переключателя на коробке

int Lenkung_Eingang_rechts = 0;                  // Переменная для определения команды управления вправо
int Lenkung_Eingang_links = 0;                   // Переменная для определения команды управления ВЛЕВО
float Lenkung_max;                               // Значение, с помощью которого управление двигателем должно изменяться как максимальное значение для команды управления
float Lenkung_rechts, Lenkung_links;             // Текущее и ступенчатое увеличение контрольного значения при управлении вправо или влево

// ****************************************************************************
// ****************************** SETUP ***************************************
// ****************************************************************************


void setup()
   {
    Wire.begin(); 
     
    //SWSerial.begin(9600);     // This is the baud rate you chose with the DIP switches.
     
    Serial.begin(9600);    // baud rate für den seriellen Monitor, um die Werte zu überprüfen
    
    // initialize device
       //Подпрограмма для однократной калибровки датчиков
         // инициализация гироскопа
  gyro.begin();
  // инициализация акселерометра
  accel.begin();
   }





// ***********************************************************************************
// ****************************** Kalibrierung ***************************************
// ***********************************************************************************


void calibrateSensors()              //Одноразовое определение значений нуля датчика (среднее значение от 50 измерений в каждом случае)
   {
    
    // =================================================
    // ======== Изменение разрешения датчика ==========
    // =================================================
    
    // ===========================================================================
    // read raw accel/gyro measurements from device
    // Выходной сигнал: Разрешение 2g: 16384 / g Разрешение 4g: 8192 / g
    //  Выходной гироскоп: Разрешение 250 ° / с: 131 / ° / с Разрешение 500 ° / с: 65,5 / ° / с
    // ===========================================================================
    
     
    
    // Попытка избежать «погремушки» двух двигателей в начале
    
    ST.motor(1, 0);             
    ST.motor(2, 0);
    
   
        
    Winkel_Soll = 0.0;                 // Уставка для угла наклона
    Winkel_Grenze = 30.0;              // Максимально допустимый угол наклона
    
    
    // *******************************************************
    // ********** K - Werte für die PID-Regelung *************
    // *******************************************************
    
    
    Kp = analogRead(Pin_PID_Regelung_P) * 25.0 / 1023.0;        // Разница с фиксированным Поти
    Ki = 0.1;                                                   // Встроенная часть с потенциометром (переключатель, но, возможно, для коррекции двигателя, поэтому сначала установите 0,1)   
    Kd = analogRead(Pin_PID_Regelung_D) * 100.0 / 1023.0;       // Дифференциальная часть с потенциометром
    K = 1.0;                                                    // общая доля


    // **************************************************
    // ********** K - значения для ПИД-регулятора *************
    // **************************************************


   
         
    pinMode(Pin_Schalter_Box, INPUT); // Pin для выбора между I-управлением и синхронизацией двигателя

    
    K_Motor_rechts = 1.0;                  // Korrekturfaktor für den rechten Motor
    K_Motor_links = 0.8;                   // Korrekturfaktor für den linken Motor
      
    
    // **********************************************
    // ********** Рулевые значения *************
    // **********************************************
    
    
    Lenkung_max = 25.0;                      // Значение, с помощью которого управление двигателем должно измениться с помощью команды управления MAXIMAL
    Lenkung_rechts = 0.0;                    // Текущее дополнительное значение в процессе рулевого управления вправо
    Lenkung_links = 0.0;                     // Текущее дополнительное значение в процессе рулевого управления влево
    pinMode(Pin_Lenkung_rechts, INPUT);      // Вывод для рулевого управления ВПРАВО объявляется как вход
    pinMode(Pin_Lenkung_links, INPUT);       // Вывод для рулевого управления ВЛЕВО объявляется как вход
      
   }






// ***************************************************************************************************************************************************
// ***************************************************************************************************************************************************
// ********************************************************************** HAUPTSCHLEIFE **************************************************************
// ***************************************************************************************************************************************************
// ***************************************************************************************************************************************************

void loop()
   {

   // *******************************************************
   // ********************* Sensorabfrage *******************
   // *******************************************************
   
    // ===========================================================================
    // read raw accel/gyro measurements from device
    // Ausgabewerte-Acc: Auflösung 2g:  16384/g    Auflösung 4g:  8192/g
    // Ausgabewerte-Gyro: Auflösung 250°/s:  131/°/s   Auflösung 500°/s:  65.5/°/s
    // ===========================================================================
    Serial.print("    ACC_angledraga = ");
    Serial.print(ACC_angledraga);
    Serial.print("    motor : ");
    Serial.print(Motor);
    Serial.print("    Winkel: ");
    Serial.print(Winkel,0);       
    Serial.print("    Winkel_Soll : ");
    Serial.print(Winkel_Soll);       
    Serial.print("    GYRO : ");
    Serial.print(GYRO_ratedraga) ;
    Serial.print("    Kp : ");
    Serial.print(Kp) ;   
    Serial.print("    Kd : ");
    Serial.print(Kd) ;   
    Serial.print("    Ki : ");
    Serial.print(Ki) ; 
    Serial.print("    Schalter : ");
    Serial.print(Schalter_Box) ;
    Serial.print("    K : ");
    Serial.println(K);
                          // ЕБАННАЯ БИБЛИОТЕКА ДРУГОГО ДАТЧКИА
 
    
    ACC_angledraga = atan2(accel.readAX(),accel.readAZ()) * 180.0 / 3.141592654;      // МОЯ КРАСАВИЦА
    GYRO_ratedraga = gyro.readDegPerSecY();

     
     
   // *******************************************************
   // ********** K - значения для ПИД-регулятора *************
   // *******************************************************
    
    
    Kp = analogRead(Pin_PID_Regelung_P) * 25.0 / 1023.0;                     // Разница с Поти фиксирована; Максимум = 25. КОРОЧЕ МАКСИМАЛЬНЫЕ ЗНАЧЕНИЯ ДЛЯ НАСТРОЕК НЕ БОЛЬШЕ 25 И 100
    Kd = analogRead(Pin_PID_Regelung_D) * 100.0 / 1023.0;                    // Дифференциальное соотношение с потенциометром; Максимум = 100

    Schalter_Box = digitalRead(Pin_Schalter_Box);                            // Запросить контакт для состояния переключателя на коробке
    
    if (Schalter_Box == HIGH)                                                // С помощью переключателя на активированном ящике я управляю
       {
        Ki = analogRead(Pin_PID_Regelung_I) * 2.0 / 1023.0;                  // Интегральная часть с потенциометром; Максимум = 2        
       }
    else                                                                     // С помощью переключателя включено управление двигателем коробки
        {
         K_Motor_rechts = analogRead(Pin_PID_Regelung_I) * 2.0 / 1023.0;     // Коэффициент коррекции для синхронной работы двух двигателей; Максимум = 2
        }

      
     
     
     // ********************************************************************************
     // ****************** Фильтр Калмана, расчет PWM и значения двигателя *****************
     // ********************************************************************************
     
     
     Winkel = kalmanCalculate(ACC_angledraga, GYRO_ratedraga, LoopTime_Angepasst);	// Рассчитано с помощью фильтра Калмана
                                                                                    

     if (Winkel > Winkel_Grenze || Winkel < (Winkel_Grenze * (-1)))
        {
         // ===============================================
         // Снос из-за слишком большого угла наклона!
         // ===============================================
         
         ST.motor(1, 0);          //ОТКЛЮЧИТЬ НАХЕР ОБА МОТОРА
         ST.motor(2, 0);
        }
     else
        {
         // =========================
         // УГОЛ НАКЛОНА В ПОРЯДОСЕ 
         // =========================
 
         Motor = pid(Winkel, Winkel_Soll, GYRO_ratedraga);  // Расчет значения ШИМ для управления двигателями // НЕЗАБУДЬ ЧТО ЭТО ДЛЯ ТРОЙКИ УДАЛИ DRAGA 
     
         Motor_rechts = K_Motor_rechts * Motor;            // Расчет скорости двигателя, синхронизированный коэффициентом K, для правого двигателя

         Motor_links = K_Motor_links * Motor;              // Расчет скорости двигателя, синхронизированный коэффициентом K, для левого двигателя
     
         
          
         // **************************************************************************************
         // ***** Проверьте, активировано ли рулевое управление и измените управление двигателем *****
         // **************************************************************************************
     
     
         Lenkung_Eingang_rechts = digitalRead(Pin_Lenkung_rechts);   // Abfrage des Pins für Lenkung nach Rechts

         if (Lenkung_Eingang_rechts == HIGH)
            {     
              // ******************************************
              // *** Lenkung nach Rechts wurde gedrückt ***
              // ******************************************
          
              if (Motor_rechts >= 0)    // segway fährt gerade vorwärts oder steht. Welcher Motor abgefragt wird, ist egal.
                 {
                  Motor_rechts = Motor_rechts - (int)Lenkung_rechts;   // Возможно, также попробуйте с умножением вектора (например, * (1 - 0,1)
                  Motor_links = Motor_links + (int)Lenkung_rechts;     // Возможно, попробуйте также умножить вектор (например, * (1 + 0,1))
                 }
              else                      // segway fährt gerade rückwärts
                 {
                  Motor_rechts = Motor_rechts + (int)Lenkung_rechts;   // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1) versuchen
                  Motor_links = Motor_links - (int)Lenkung_rechts;     // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
                 }
                 
             Lenkung_rechts = Lenkung_rechts + 0.05;                                 // Лучше только для, например, Увеличьте 0,1 на запрос, чтобы рулевое управление не было слишком резким!
             
             if (Lenkung_rechts > Lenkung_max) Lenkung_rechts = Lenkung_max;        // Максимальное значение рулевого управления не должно превышаться!
             
             //Lenkung_rechts = constrain(Lenkung_rechts, 0, Lenkung_max);          // Правое значение рулевого управления используется в интервале [0, рулевое управление]
            } 
         else
            {
             Lenkung_rechts = 0.0;
            }
    
    
         Lenkung_Eingang_links = digitalRead(Pin_Lenkung_links);    // Abfrage des Pins für Lenkung nach Links

         if (Lenkung_Eingang_links == HIGH)
            {     
              // *****************************************
              // *** Lenkung nach Links wurde gedrückt ***
              // *****************************************
          
              if (Motor_links >= 0)    // segway fährt gerade vorwärts oder steht. Welcher Motor abgefragt wird ist egal.
                 {
                  Motor_rechts = Motor_rechts + (int)Lenkung_links;   // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1) versuchen
                  Motor_links = Motor_links - (int)Lenkung_links;     // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
                 }
              else                      // segway fährt gerade rückwärts
                 {
                  Motor_rechts = Motor_rechts - (int)Lenkung_links;   // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 - 0.1) versuchen
                  Motor_links = Motor_links + (int)Lenkung_links;     // Vielleicht auch mit Multiplikation eines Faktors (z.B. * (1 + 0.1) versuchen
                 }
                 
             Lenkung_links = Lenkung_links + 0.05;                                 // Besser nur um z.B. 0.1 pro Abfrage erhöhen, damit Lenkung nicht zu abrupt erfolgt!
             
             if (Lenkung_links > Lenkung_max) Lenkung_links = Lenkung_max;        // Maximaler Lenkungswert darf nicht überschritten werden!
             
             //Lenkung_links = constrain(Lenkung_links, 0, Lenkung_max);          // linker Lenkungswert ins Intervall [0,Lenkung_max] gebracht
            } 
         else
            {
             Lenkung_links = 0.0;
            }
       
        
        
     
         // *******************************************************************************************
         // ******************************** Управление двигателями  ***********************************
         // *******************************************************************************************
        
         
         Motor_rechts = constrain(Motor_rechts, -127, 127);          // Значение правого двигателя в интервале [-127,127]
         Motor_links = constrain(Motor_links, -127, 127);            // Значение левого двигателя в интервале [-127,127]
         
                      
     /*
         // Использование функции корня вместо функции линейного привода для улучшения реакции при низких значениях двигателя
         // ======================================================================================================================================
         
         if (Motor_rechts >= 0)     // rechter Motor dreht vorwärts
            { 
             Motor_rechts = sqrt(127 * Motor_rechts);             // Чтобы улучшить реакцию при низких значениях двигателя
              
             ST.motor(2, Motor_rechts);      
            }
         else                       // rechter Motor dreht rückwärts
            {
             Motor_rechts = -sqrt(127 * -Motor_rechts);           // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten
             
             ST.motor(2, Motor_rechts);               
            }


         if (Motor_links >= 0)      // linker Motor dreht vorwärts
            {
             Motor_links = sqrt(127 * Motor_links);               // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten 
             
             ST.motor(1, Motor_links);               
            }
         else                       // linker Motor dreht rückwärts
            {
             Motor_links = -sqrt(127 * -Motor_links);             // zur Verbesserung des Ansprechverhaltens bei niedrigen Motorwerten 
             
             ST.motor(1, Motor_links);  
            }
         */
         
         ST.motor(1, Motor_links);
         ST.motor(2, Motor_rechts);
         
        } 


   // ************************************************************************ 
   // ***********************   Выход измеряемых величин **************************
   // ************************************************************************

    Werteausgabe();


   // ******************************************************************
   // *********************** Tastaturabfrage **************************
   // ******************************************************************

   // Tastatureingabe();



   // **********************************************************************
   // *********************** loop timing control **************************
   // **********************************************************************

     LoopTime_Bisher = millis() - LoopTime_Start;        // Время с момента последнего цикла
     
     if(LoopTime_Bisher < LoopTime_Soll)
        {
         delay(LoopTime_Soll - LoopTime_Bisher);         // Задержка для получения того же времени цикла
        }
     
     LoopTime_Angepasst = millis() - LoopTime_Start;     // Обновленная длительность последнего цикла должна равняться LoopTime_Soll = например. 10 мсек!
     LoopTime_Start = millis();                          //Новый запуск цикла
   
 }


// ********************************************************************************************
// ****************** Вывод на последовательный интерфейс ******************************
// ********************************************************************************************

void Werteausgabe()
   {
    /*
    Serial.print(Winkel);
    Serial.print("     ");
    Serial.println(Motor);
    Serial.print("     ");
    */
    

    
    /*
    Serial.print("   Motor: ");
    Serial.print(Motor);
    Serial.print("    Motor_rechts: ");
    Serial.print(Motor_rechts);
    Serial.print("    Motor_links: ");
    Serial.println(Motor_links);
    */
    
   }


// ******************************************************************************************************
// ***************************************** PID-Steuerung **********************************************
// ******************************************************************************************************

float error;
float last_error = 0;
float pTerm;
float iTerm;
float dTerm;
float integrated_error = 0;
int GUARD_GAIN = 40;           // Максимальная угловая погрешность

   int pid(float Winkel_aktuell, float Winkel_Vorgabe, float Winkelgeschwindigkeit)
      {
       error = Winkel_Vorgabe - Winkel_aktuell;
       
       pTerm = Kp * error;                                                         // Differenzenanteil
       
       integrated_error = integrated_error + error;
   
       iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);          // Integralanteil
  
       
       dTerm = Kd * Winkelgeschwindigkeit / 100.0;                                 // Дифференциальный компонент; : 100 для достижения полезных значений!  
  
       last_error = error;
       
       return constrain(K*(pTerm + iTerm + dTerm), -127, 127);                     // Выходное значение двигателя для двух двигателей в пределах [-127,127]
      } 

 
 

// ******************************************************************************************************
// ************************************** Kalman filter module ******************************************
// ******************************************************************************************************


    float Q_angle  =  0.001;		// E(alpha2) = 0.001
    float Q_gyro   =  0.003;  		// E(bias2) = 0.003
    float R_angle  =  0.001;  		// Sz = 0.03   !!! je größer die Zahl, desto unempfindlicher reagiert der Winkel auf Veränderungen !!!
    float x_angle = 0;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float dt, y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate, int looptime)
     {
      dt = float(looptime)/1000;    // dt in Sekunden
      x_angle = x_angle + dt * (newRate - x_bias);
      P_00 = P_00 - dt * (P_10 + P_01) + Q_angle * dt;
      P_01 = P_01 - dt * P_11;
      P_10 = P_10 - dt * P_11;
      P_11 = P_11 + Q_gyro * dt;

      y = newAngle - x_angle;
      S = P_00 + R_angle;
      K_0 = P_00 / S;
      K_1 = P_10 / S;

      x_angle +=  K_0 * y;
      x_bias  +=  K_1 * y;
      P_00 -= K_0 * P_00;
      P_01 -= K_0 * P_01;
      P_10 -= K_1 * P_00;
      P_11 -= K_1 * P_01;

      return x_angle;
     }



// ********************************************************************
// ******** Запрос клавиатуры для изменения параметров PUI *********
// ********************************************************************

int Tastatureingabe()
   {
    if(!Serial.available())    return 0;
   
    char param = Serial.read();                            // get parameter byte
  
    if(!Serial.available())    return 0;
  
    char cmd = Serial.read();                              // get command byte
  
    Serial.flush();
  
    switch (param)
       {
        case 'p':
           if(cmd=='+')    Kp++;
           if(cmd=='-')    Kp--;
           break;
        case 'i':
           if(cmd=='+')    Ki += 0.1;
           if(cmd=='-')    Ki -= 0.1;
           break;
        case 'd':
           if(cmd=='+')    Kd++;
           if(cmd=='-')    Kd--;
           break;
       case 'k':
           if(cmd=='+')    K += 0.2;
           if(cmd=='-')    K -= 0.2;
           break;
       case 'l':
           if(cmd=='+')    K_Motor_links += 0.1;
           if(cmd=='-')    K_Motor_links -= 0.1;
           break;
       case 'r':
           if(cmd=='+')    K_Motor_rechts += 0.1;
           if(cmd=='-')    K_Motor_rechts -= 0.1;
           break;
     
       default:
           Serial.print("?");           Serial.print(param);
           Serial.print(" ?");          Serial.println(cmd);
      }
  
    Serial.println();
    Serial.print("K:");                      Serial.print(K);
    Serial.print("   Kp:");                  Serial.print(Kp);
    Serial.print("   Ki:");                  Serial.print(Ki);
    Serial.print("   Kd:");                  Serial.print(Kd);
    Serial.print("   K_Motor_links:");       Serial.print(K_Motor_links);
    Serial.print("   K_Motor_rechts:");      Serial.println(K_Motor_rechts);
   }