Arduino.ru

Привет. Задачат такая. Сигнализация на ардуино нано + GY 521 + DFPlayer лай собаки для авто. Нашел проэкт с лающей собакой но на датчиек движения PIR. Нужно заменить PIR на GY 521. Нужно еще и функция для настойки чувствительности.  По электронике шарю а вот по програмированию тормоз.

скетч с датчиком движения

#include "Arduino.h"
#include "SoftwareSerial.h"
#include "DFRobotDFPlayerMini.h"
#include<Wire.h>

SoftwareSerial mySoftwareSerial(10, 11);                // RX, TX
DFRobotDFPlayerMini myDFPlayer;
void printDetail(uint8_t type, int value);
int count;
const int pirPIN = 2;                                   // контакт  подключения датчика
int state = 0;
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,GyX,GyY,GyZ;

void setup() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  mySoftwareSerial.begin(9600);
  Wire.begin();
  Serial.begin(115200);  
  if (!myDFPlayer.begin(mySoftwareSerial)) {            //Проверка
    Serial.println(F("Unable to begin:"));
    Serial.println(F("1.Please recheck the connection!"));
    Serial.println(F("2.Please insert the SD card!"));
    while(true);
  }
  Serial.println(F("Всё работает!!!"));
    
  myDFPlayer.volume(25);                                //Установка громкости (0~30).
    delay(40000);                                       // Задержка 40 секунд на настройку датчика.
    myDFPlayer.play(4);
    count = myDFPlayer.readFileCounts();                //Количество файлов
}

void loop()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
   state = digitalRead(pirPIN);
   if(state == 1) {
    Serial.println("Тревога "); 
    if(myDFPlayer.readState() == 512) {
    myDFPlayer.play(random(1,count));
    }
    Serial.print("Номер трека ");
    Serial.println(myDFPlayer.readCurrentFileNumber());
    delay(5000);  
   }   
   if(state == 0) {
    Serial.println("Отбой Тревоги ");
//    myDFPlayer.pause();
   }
   if (myDFPlayer.available()) {
      printDetail(myDFPlayer.readType(), myDFPlayer.read()); //Распечатка ошибок и статусов принтера
    }
}

void printDetail(uint8_t type, int value){
  switch (type) {
    case TimeOut:
      Serial.println(F("Time Out!"));
      break;
    case WrongStack:
      Serial.println(F("Stack Wrong!"));
      break;
    case DFPlayerCardInserted:
      Serial.println(F("Card Inserted!"));
      break;
    case DFPlayerCardRemoved:
      Serial.println(F("Card Removed!"));
      break;
    case DFPlayerCardOnline:
      Serial.println(F("Card Online!"));
      break;
    case DFPlayerUSBInserted:
      Serial.println("USB Inserted!");
      break;
    case DFPlayerUSBRemoved:
      Serial.println("USB Removed!");
      break;
    case DFPlayerPlayFinished:
      Serial.print(F("Number:"));
      Serial.print(value);
      Serial.println(F(" Play Finished!"));
      break;
    case DFPlayerError:
      Serial.print(F("DFPlayerError:"));
      switch (value) {
        case Busy:
          Serial.println(F("Card not found"));
          break;
        case Sleeping:
          Serial.println(F("Sleeping"));
          break;
        case SerialWrongStack:
          Serial.println(F("Get Wrong Stack"));
          break;
        case CheckSumNotMatch:
          Serial.println(F("Check Sum Not Match"));
          break;
        case FileIndexOut:
          Serial.println(F("File Index Out of Bound"));
          break;
        case FileMismatch:
          Serial.println(F("Cannot Find File"));
          break;
        case Advertise:
          Serial.println(F("In Advertise"));
          break;
        default:
          break;
      }
      break;
    default:
      break;
  }
  
}

скетч полной сигнализации с использованием датчика GY 521. тут человек полностью имеет все функции, чувствительность, от фальшивых срабатываний, подсчетов датчика и как бы авто не стояло криво, датчик запоминает положение и ведет наблюдение за отклонениями и многе другое связаное с GY 521. Вот скетч

#include "Kalman.h"
#include <Wire.h>
#include <SoftwareSerial.h>
#define Module_Start "Call Ready"
#define GSM_TYPE_SIM800
SoftwareSerial Serial_S(2 ,  3);
bool GSM_In_sms;
String GSM_Phone;
String GSM_text;
bool GSM_Reject_call;
bool GSM_start;
bool GSM_Net;
int GSM_dBm;
bool GSM_Received_SMS;
String GSM_Text_sms;
String GSM_T_Nomer;
bool GSM_Sent_SMS;
bool GSM_Call;
int GSM_Number_calls;
int GSM_Error_Code;
bool GSM_start_N; //Модем прислал сообщение о том, что он стартовал 
bool GSM_final; // закончена обработка запроса
bool GSM_wait; // идет ожидание ответа на команду
bool GSM_past_SMS;
bool GSM_command_SMS;
bool GSM_past_Reject_call;
bool GSM_command_Reject_call;
//bool GSM_Call_P1;
char GSM_ch;
   //символ пришетший в ком порт
byte GSM_errorCounter; // счетчик ошибок
byte GSM_numberRepeats;
//byte GSM_Dial_response_code;
byte GSM_team_room; //номер текущей функции 
byte GSM_count; // счетчик последовальности команд
unsigned long GSM_time; //Время посылки команды
unsigned long GSM_T_millis; //Время посылки команды
unsigned long GSM_time_n; //Время последнего опроса связи с сетью.
String GSM_line; // сообщение от модуля
String GSM_reply; //Ответ модуля на команду
String GSM_Phone_T=""; //Телефон во время подачи команд на звонок или отправку смс
int GSM_DTMF;
#define GSM_INCLUSION_DTMF
bool GSM_Outgoing_Call;
bool GSM_Take_Call;
bool GSM_Connect;
#define GSM_INCLUSION_CALLS
float gyro_axisX;
float gyro_axisY;
Kalman kalmanX;
Kalman kalmanY;
uint8_t IMUAddress= 0x68;
int var;
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
float accXangle; // Angle calculate using the accelerometer
float accYangle;
float temp;
float gyroXangle= 180; // Angle calculate using the gyro
float gyroYangle= 180;
float compAngleX= 180; // Calculate the angle using a Kalman filter
float compAngleY= 180;
float kalAngleX; // Calculate the angle using a Kalman filter
float kalAngleY;
uint32_t timer;
int _gtv1;
int _gtv2;
int _gtv3;
bool _gtv4 = 0;
bool _gtv5 = 0;
bool _gtv6 = 0;
bool _gtv7 = 0;
String _gtv8;
bool _gtv10;
int _gtv11;
bool _gtv12 = 0;
bool _gtv14 = 0;
bool _gtv15 = 0;
bool _trgrt13 = 0;
bool _trgrt13I = 0;
bool _tim2I = 0;
bool _tim2O = 0;
unsigned long _tim2P = 0UL;
bool _tim9I = 0;
bool _tim9O = 0;
unsigned long _tim9P = 0UL;
bool _tim1I = 0;
bool _tim1O = 0;
unsigned long _tim1P = 0UL;
bool _trgrt8 = 0;
bool _trgrt8I = 0;
bool _trgrt6 = 0;
bool _trgrt6I = 0;
bool _trgr1 = 0;
bool _trgrt2 = 0;
bool _trgrt2I = 0;
bool _trgrt14 = 0;
bool _trgrt14I = 0;
bool _trgrt9 = 0;
bool _trgrt9I = 0;
bool _gen1I = 0;
bool _gen1O = 0;
unsigned long _gen1P = 0UL;
bool _trgrt12 = 0;
bool _trgrt12I = 0;
bool _trgrt5 = 0;
bool _trgrt5I = 0;
bool _tim7I = 0;
bool _tim7O = 0;
unsigned long _tim7P = 0UL;
bool _tim8I = 0;
bool _tim8O = 0;
unsigned long _tim8P = 0UL;
bool _tim4I = 0;
bool _tim4O = 0;
unsigned long _tim4P = 0UL;
bool _tim6I = 0;
bool _tim6O = 0;
unsigned long _tim6P = 0UL;
bool _tim5I = 0;
bool _tim5O = 0;
unsigned long _tim5P = 0UL;
bool _trgrt4 = 0;
bool _trgrt4I = 0;
bool _trgrt7 = 0;
bool _trgrt7I = 0;
bool _bounseInputD3S = 0;
bool _bounseInputD3O = 0;
unsigned long _bounseInputD3P = 0UL;
bool _tim3I = 0;
bool _tim3O = 0;
unsigned long _tim3P = 0UL;
bool _trgs1 = 0;
bool _trgrt11 = 0;
bool _trgrt11I = 0;
bool _trgrt10 = 0;
bool _trgrt10I = 0;
bool _trgrt3 = 0;
bool _trgrt3I = 0;
bool _bounseInputD11S = 0;
bool _bounseInputD11O = 0;
unsigned long _bounseInputD11P = 0UL;
void setup()
{
    pinMode(3, INPUT_PULLUP);
    pinMode(11, INPUT);
    pinMode(13, OUTPUT);
    digitalWrite(13, 0);
    pinMode(12, OUTPUT);
    digitalWrite(12, 0);
    pinMode(2, OUTPUT);
    digitalWrite(2, 0);
    pinMode(9, OUTPUT);
    digitalWrite(9, 0);
    pinMode(8, OUTPUT);
    digitalWrite(8, 0);
    pinMode(7, OUTPUT);
    digitalWrite(7, 0);
    pinMode(10, OUTPUT);
    digitalWrite(10, 0);
    pinMode(6, OUTPUT);
    digitalWrite(6, 0);
    _bounseInputD3O =  digitalRead(3);
    _bounseInputD11O =  digitalRead(11);
    Serial_S.println("AT+IPR=9600");
    Serial_S.println("AT&W");
    Serial_S.begin(9600);
    Wire.begin();
    i2cWrite(0x6B,0x00); // Disable sleep mode
    kalmanX.setAngle(180); // Set starting angle
    kalmanY.setAngle(180);
    timer = micros();
    var=0;
}
void loop()
{
    bool  _bounceInputTmpD3 =  (digitalRead (3));
    if (_bounseInputD3S)
    {
        if (millis() >= (_bounseInputD3P + 40))
        {
            _bounseInputD3O= _bounceInputTmpD3;
            _bounseInputD3S=0;
        }
    }
    else
    {
        if (_bounceInputTmpD3 != _bounseInputD3O)
        {
            _bounseInputD3S=1;
            _bounseInputD3P = millis();
        }
    }
    bool  _bounceInputTmpD11 =  (digitalRead (11));
    if (_bounseInputD11S)
    {
        if (millis() >= (_bounseInputD11P + 40))
        {
            _bounseInputD11O= _bounceInputTmpD11;
            _bounseInputD11S=0;
        }
    }
    else
    {
        if (_bounceInputTmpD11 != _bounseInputD11O)
        {
            _bounseInputD11S=1;
            _bounseInputD11P = millis();
        }
    }
    //Плата:1
//Наименование:SIM800
    if (1) 
    {
         if (_trgrt9I) 
        {
             _trgrt9 = 0;
        }
         else 
        {
            _trgrt9 = 1;
            _trgrt9I = 1;
        }
    }
     else 
    {
        _trgrt9 = 0;
        _trgrt9I = 0;
    }
    ;
    digitalWrite(12, _trgrt9);
    _gtv11 = GSM_DTMF;
    if (_gtv10)
    {
         if (_tim9I) 
        {
             if (_isTimer(_tim9P, 3000)) 
            {
                _tim9O = 1;
            }
        }
         else 
        {
            _tim9I =1;
            _tim9P = millis();
        }
    }
     else 
    {
        _tim9O = 0;
        _tim9I = 0;
    }
    if (_tim9O) 
    {
         if (_trgrt14I) 
        {
             _trgrt14 = 0;
        }
         else 
        {
            _trgrt14 = 1;
            _trgrt14I = 1;
        }
    }
     else 
    {
        _trgrt14 = 0;
        _trgrt14I = 0;
    }
    ;
    if (_gtv10) 
    {
         if (_trgrt13I) 
        {
             _trgrt13 = 0;
        }
         else 
        {
            _trgrt13 = 1;
            _trgrt13I = 1;
        }
    }
     else 
    {
        _trgrt13 = 0;
        _trgrt13I = 0;
    }
    ;
    GSM_In_sms = _trgrt14;
    GSM_Phone = String("+7xxxxxxxxxx");
    GSM_text = String("Proniknovenie");
    GSM_Reject_call = _trgrt13;
    GSM_Received_SMS =0; //Сброс статутса СМС Принята
    GSM_Sent_SMS =0; //Сброс статуса СМС Отправлена
    if (1)
    {
          if (GSM_team_room==3)
        {
                GSM_Number_calls =0;
                GSM_Text_sms="";
                GSM_T_Nomer ="";
              
        }
    }
    GSM_T_millis =millis();
        while(Serial_S.available())  //Получаем данные с модема
          
    {
        GSM_ch = Serial_S.read();
              
              if(GSM_ch == '\r') continue;
              if(GSM_ch == '\n') 
        {
             GSM_GotLineFromNeoway();
            GSM_line ="";
        }
              else  GSM_line += GSM_ch;
    }
    if (GSM_wait) //ожиание ответа на команду
      
    {
          
          if(GSM_line.length()>0)
        {
                if(GSM_line==F("> ")) 
            {
                GSM_count++;
                GSM_wait=0;
                GSM_line ="";
            }
        }
          if(GSM_T_millis>GSM_time) 
        {
            GSM_err (2,0);
        }
          
    }
    if (GSM_Call&&(GSM_team_room!=1))
      
    {
          
          GSM_team_room=1;
          
          GSM_Number_calls=1; //сброс числа гудков
           // GSM_resetExpectations() ; 
          
    }
    if (GSM_start_N)
    {
        if (!GSM_wait)
        {
            if (GSM_setup_comand())
            {
                GSM_start_N=0;
                GSM_start =1;
            }
        }
    }
// Инициализация
    if (GSM_start) //Не работать пока не старт
      
    {
        //______Проверка статуса сети
            if ((30000 >0)&&(GSM_team_room==0))  //проверять ли статутс сети
              
        {
                  if((GSM_T_millis-GSM_time_n) > 30000)
            {
                 GSM_team_room=3;
            }
                  
        }
        //______Конец Проверка статуса сети
        //______Отправка СМС
          if (GSM_In_sms  && !GSM_past_SMS) 
        {
              if (GSM_number_search())GSM_command_SMS =1;
              
        }
           // поступление команды на отправу СМС. Проверка корректности номера
          GSM_past_SMS= GSM_In_sms;
          if (GSM_command_SMS && (GSM_team_room==0)) 
        {
            GSM_team_room=2;
            GSM_command_SMS =0;
        }
          // начинать отправу СМС
//______Конец Отправка СМС    
        //______Команда сброс вызова или разрыв разговора 
          if (GSM_Reject_call  && !GSM_past_Reject_call) 
        {
             GSM_command_Reject_call =1;
        }
          GSM_past_Reject_call=GSM_Reject_call;
        //______Конец Команда сброс вызова или разрыв разговора 
          
        #ifdef GSM_INCLUSION_USSD  // Обработка вызова USSD запросов 
          GSM_causeProcessingUSSD();
        #endif
        #ifdef GSM_INCLUSION_DATA_TIME  //Обработка времени, даты, оператора
          GSM_causeProcessingDataTime();
        #endif
        #ifdef GSM_INCLUSION_CALLS  // исходящий вызов 
          if (GSM_causeProcessingCall()) 
        {
                if(GSM_number_search())
            {
                      GSM_wait=1; // ожитать
                      GSM_time =millis() + 20000; // время ожидания     
                      Serial_S.println("ATD"+ GSM_Phone_T+";");
                      GSM_team_room=5;
                  #ifdef GSM_DEBUG  // отладка    
                    Serial.println("Command: ATD"+ GSM_Phone_T+";");
                  
                  #endif
                    
            }
              
        }
        #endif
        //________ Обработка разных процессов
          switch (GSM_team_room) 
        {
              case 0:
                  break;
              case 1:  //Обработка входящих  вызовов 
                if (!GSM_wait)
            {
                      if (GSM_command_Reject_call) 
                {
                            if (GSM_singleCommand(F("ATH0"), 10))  
                    {
                        GSM_command_Reject_call=0;
                        GSM_Call=0;
                        GSM_count=0;
                        GSM_final=1;
                    }
                            
                }
                  
                #ifdef GSM_INCLUSION_CALLS
                #ifndef GSM_TYPE_M590
                    if (GSM_causeProcessingTake())
                {
                    GSM_Connect=1;
                    GSM_team_room=5;
                }
                  
                #endif  
                #endif  
                    
            }
                    
                if (!GSM_Call)
            {
                GSM_final =1;
                GSM_T_Nomer ="";
            }
                break;
              case 2: 
                if (!GSM_wait)
            {
                  GSM_final=GSM_sms_sent();
            }
             // отправка сообщения
                break;
              case 3: 
                if (!GSM_wait)
            {
                 GSM_final=GSM_net_status();
            }
// проверка связи с сетью
                break;
              case 4:  //приход смс
                 //if (!GSM_wait){ GSM_final=1; GSM_count=0; GSM_Error_Code =0;   }// 
                break;
            #ifdef GSM_INCLUSION_CALLS
              case 5:  //разговор 
                if (!GSM_wait) 
            {
                  
                      if (GSM_command_Reject_call) 
                {
                            if (GSM_singleCommand(F("ATH0"), 10))  
                    {
                        GSM_command_Reject_call=0;
                        GSM_Call=0;
                        GSM_count=0;
                        GSM_Connect=0;
                        GSM_final=1;
                    }
                          
                }
                      GSM_count=0;
                      
                    
            }
                break;
            #endif
            #ifdef GSM_INCLUSION_DATA_TIME
              case 6:  // дата время 
                if (!GSM_wait)
            {
                if (GSM_DataTime())
                {
                     GSM_final =1;
                }
            }
// запрос баланса
                break;
            #endif
            #ifdef GSM_INCLUSION_USSD
              case 7:  // USSD 
                if (!GSM_wait)
            {
                 GSM_final=GSM_USSD();
            }
// запрос баланса
                break;
            #endif
              
        }
        //________ Конец Обработка разных процессов
          if (GSM_final)
        {
            GSM_team_room=0;
            GSM_final=0;
        }
         //должно быть последним
    }
    _gtv8 = GSM_T_Nomer;
    digitalWrite(2, GSM_Net);
    if (((!(_gtv10)) && (_gtv12))) 
    {
         if (_trgrt12I) 
        {
             _trgrt12 = 0;
        }
         else 
        {
            _trgrt12 = 1;
            _trgrt12I = 1;
        }
    }
     else 
    {
        _trgrt12 = 0;
        _trgrt12I = 0;
    }
    ;
    GSM_Outgoing_Call = _trgrt12;
    GSM_Take_Call = ((_gtv8).equals(String("+7xxxxxxxxxx")));
    //Плата:2
//Наименование:Сигнализация
    if (!(_bounseInputD11O)) 
    {
         if (_trgrt6I) 
        {
             _trgrt6 = 0;
        }
         else 
        {
            _trgrt6 = 1;
            _trgrt6I = 1;
        }
    }
     else 
    {
        _trgrt6 = 0;
        _trgrt6I = 0;
    }
    ;
    if(((_gtv5) && (_trgrt6))) _trgs1 = 1;
    if(!(_gtv5)) _trgs1 = 0;
    digitalWrite(10, ((_gtv15) || (((((_gtv14) || (_trgs1))) && (_gtv4)))));
    digitalWrite(9, _trgs1);
    _gtv10 = _trgs1;
    //Плата:3
//Наименование:Датчик наклона
    _gtv6 = (_gtv11) == (1);
    _gtv7 = (_gtv11) == (2);
    while (var<100)
    {
        uint8_t* data = i2cRead(0x3B,14);
        accX = ((data[0] << 8) | data[1]);
        accY = ((data[2] << 8) | data[3]);
        accZ = ((data[4] << 8) | data[5]);
        tempRaw = ((data[6] << 8) | data[7]);
        gyroX = ((data[8] << 8) | data[9]);
        gyroY = ((data[10] << 8) | data[11]);
        gyroZ = ((data[12] << 8) | data[13]);
        accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
        accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
        float gyroXrate = (float)gyroX/131.0;
        float gyroYrate = -((float)gyroY/131.0);
        //float accelXrate = (atan(accX / sqrt(accY * accY + accZ * accZ)));
//float accelYrate = (atan(accY / sqrt(accX * accX + accZ * accZ)));
        gyroXangle += kalmanX.getRate()*((float)(micros()-timer)/10000); // Calculate gyro angle using the unbiased rate
        gyroYangle += kalmanY.getRate()*((float)(micros()-timer)/10000);
        kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (float)(micros()-timer)/10000); // Calculate the angle using a Kalman filter
        kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (float)(micros()-timer)/10000);
        timer = micros();
        var++;
    }
    uint8_t* data = i2cRead(0x3B,14);
    accX = ((data[0] << 8) | data[1]);
    accY = ((data[2] << 8) | data[3]);
    accZ = ((data[4] << 8) | data[5]);
    tempRaw = ((data[6] << 8) | data[7]);
    gyroX = ((data[8] << 8) | data[9]);
    gyroY = ((data[10] << 8) | data[11]);
    gyroZ = ((data[12] << 8) | data[13]);
    accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
    accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
    float gyroXrate = (float)gyroX/131.0;
    float gyroYrate = -((float)gyroY/131.0);
    gyroXangle += kalmanX.getRate()*((float)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
    gyroYangle += kalmanY.getRate()*((float)(micros()-timer)/1000000);
    kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (float)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
    kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (float)(micros()-timer)/1000000);
    timer = micros();
    gyro_axisX = kalAngleX;
    gyro_axisY = kalAngleY;
    if (_gtv5) 
    {
         if (_trgrt3I) 
        {
             _trgrt3 = 0;
        }
         else 
        {
            _trgrt3 = 1;
            _trgrt3I = 1;
        }
    }
     else 
    {
        _trgrt3 = 0;
        _trgrt3I = 0;
    }
    ;
    if ((((((((int(gyro_axisY)))-(90)) > ((_gtv1)+((map((_gtv3), (0), (1023), (1), (5)))))) || ((((int(gyro_axisY)))-(90)) < ((_gtv1)-((map((_gtv3), (0), (1023), (1), (5)))))))) || ((((((int(gyro_axisX)))-(90)) > ((_gtv2)+((map((_gtv3), (0), (1023), (1), (5)))))) || ((((int(gyro_axisX)))-(90)) < ((_gtv2)-((map((_gtv3), (0), (1023), (1), (5)))))))))) 
    {
         if (_trgrt10I) 
        {
             _trgrt10 = 0;
        }
         else 
        {
            _trgrt10 = 1;
            _trgrt10I = 1;
        }
    }
     else 
    {
        _trgrt10 = 0;
        _trgrt10I = 0;
    }
    ;
    _gtv12 = ((((_gtv5) && (_trgrt10))) && (_gtv5));
    if(((_gtv5) && (_trgrt10))) 
    {
        _tim1O = 1;
        _tim1I = 1;
    }
     else 
    {
         if(_tim1I) 
        {
            _tim1I = 0;
            _tim1P = millis();
        }
         else 
        {
             if (_tim1O) 
            {
                if (_isTimer(_tim1P, 5000)) _tim1O = 0;
            }
        }
    }
    _gtv14 = _tim1O;
    if (!(_bounseInputD3O))
    {
         if (_tim3I) 
        {
             if (_isTimer(_tim3P, 3000)) 
            {
                _tim3O = 1;
            }
        }
         else 
        {
            _tim3I =1;
            _tim3P = millis();
        }
    }
     else 
    {
        _tim3O = 0;
        _tim3I = 0;
    }
    if (_gtv7) 
    {
         if (_trgrt5I) 
        {
             _trgrt5 = 0;
        }
         else 
        {
            _trgrt5 = 1;
            _trgrt5I = 1;
        }
    }
     else 
    {
        _trgrt5 = 0;
        _trgrt5I = 0;
    }
    ;
    if (((_tim3O) || (_trgrt5))) 
    {
         if (_trgrt8I) 
        {
             _trgrt8 = 0;
        }
         else 
        {
            _trgrt8 = 1;
            _trgrt8I = 1;
        }
    }
     else 
    {
        _trgrt8 = 0;
        _trgrt8I = 0;
    }
    ;
    if(_trgrt8) 
    {
        _tim7O = 1;
        _tim7I = 1;
    }
     else 
    {
         if(_tim7I) 
        {
            _tim7I = 0;
            _tim7P = millis();
        }
         else 
        {
             if (_tim7O) 
            {
                if (_isTimer(_tim7P, 500)) _tim7O = 0;
            }
        }
    }
    digitalWrite(7, _tim7O);
    if (!(_trgrt8))
    {
         if (_tim5I) 
        {
             if (_isTimer(_tim5P, 250)) 
            {
                _tim5O = 1;
            }
        }
         else 
        {
            _tim5I =1;
            _tim5P = millis();
        }
    }
     else 
    {
        _tim5O = 0;
        _tim5I = 0;
    }
    if (_tim5O) 
    {
         if (_trgrt7I) 
        {
             _trgrt7 = 0;
        }
         else 
        {
            _trgrt7 = 1;
            _trgrt7I = 1;
        }
    }
     else 
    {
        _trgrt7 = 0;
        _trgrt7I = 0;
    }
    ;
    if(((_trgrt8) || (_trgrt7))) 
    {
        _tim6O = 1;
        _tim6I = 1;
    }
     else 
    {
         if(_tim6I) 
        {
            _tim6I = 0;
            _tim6P = millis();
        }
         else 
        {
             if (_tim6O) 
            {
                if (_isTimer(_tim6P, 100)) _tim6O = 0;
            }
        }
    }
    if (((_gtv6) || (!(_bounseInputD3O)))) 
    {
         if (_trgrt2I) 
        {
             _trgrt2 = 0;
        }
         else 
        {
            _trgrt2 = 1;
            _trgrt2I = 1;
        }
    }
     else 
    {
        _trgrt2 = 0;
        _trgrt2I = 0;
    }
    ;
    if(((_tim3O) || (_trgrt5))) _trgr1 = 0;
    if(_trgrt2) _trgr1 = 1;
    if (_trgr1) 
    {
         if (_trgrt4I) 
        {
             _trgrt4 = 0;
        }
         else 
        {
            _trgrt4 = 1;
            _trgrt4I = 1;
        }
    }
     else 
    {
        _trgrt4 = 0;
        _trgrt4I = 0;
    }
    ;
    if(_trgrt4) 
    {
        _tim4O = 1;
        _tim4I = 1;
    }
     else 
    {
         if(_tim4I) 
        {
            _tim4I = 0;
            _tim4P = millis();
        }
         else 
        {
             if (_tim4O) 
            {
                if (_isTimer(_tim4P, 100)) _tim4O = 0;
            }
        }
    }
    _gtv15 = ((_tim4O) || (_tim6O));
    if (_trgr1)
    {
         if (_tim2I) 
        {
             if (_isTimer(_tim2P, 10000)) 
            {
                _tim2O = 1;
            }
        }
         else 
        {
            _tim2I =1;
            _tim2P = millis();
        }
    }
     else 
    {
        _tim2O = 0;
        _tim2I = 0;
    }
    if (((_gtv6) || (_tim2O))) 
    {
         if (_trgrt11I) 
        {
             _trgrt11 = 0;
        }
         else 
        {
            _trgrt11 = 1;
            _trgrt11I = 1;
        }
    }
     else 
    {
        _trgrt11 = 0;
        _trgrt11I = 0;
    }
    ;
    if(_trgrt11) 
    {
        _tim8O = 1;
        _tim8I = 1;
    }
     else 
    {
         if(_tim8I) 
        {
            _tim8I = 0;
            _tim8P = millis();
        }
         else 
        {
             if (_tim8O) 
            {
                if (_isTimer(_tim8P, 500)) _tim8O = 0;
            }
        }
    }
    digitalWrite(8, _tim8O);
    _gtv5 = _tim2O;
    if (_gtv5) 
    {
         if (! _gen1I) 
        {
            _gen1I = 1;
            _gen1O = 1;
            _gen1P = millis();
        }
    }
     else 
    {
        _gen1I = 0 ;
        _gen1O= 0;
    }
    if (_gen1I) 
    {
          if (_isTimer (_gen1P , 500)) 
        {
             _gen1P = millis();
            _gen1O = ! _gen1O;
        }
    }
    _gtv4 = _gen1O;
    if (_gen1O) 
    {
        _gtv3 =  (analogRead (1));
    }
    digitalWrite(13, ((_trgr1) && (!(_gen1O))));
    digitalWrite(6, ((_tim4O) || (_tim6O) || (_tim1O) || (_gtv10)));
    if (_trgrt3) 
    {
        _gtv1 = ((int(gyro_axisY)))-(90);
    }
    if (_trgrt3) 
    {
        _gtv2 = ((int(gyro_axisX)))-(90);
    }
}
bool _isTimer(unsigned long startTime, unsigned long period)
{
    unsigned long currentTime;
    currentTime = millis();
    if (currentTime>= startTime) 
    {
        return (currentTime>=(startTime + period));
    }
     else 
    {
        return (currentTime >=(4294967295-startTime+period));
    }
}
bool GSM_setup_comand()
{
    switch (GSM_count) 
    {
        case 0: 
           GSM_count ++;
          
          break;
        case 1:
           GSM_Command(F("ATE0") ,2) ; //отключение эхо ответа
          break;
        case 2:   
           GSM_AT_Command(F("CPAS"),4);
               // проверка готовности модуля к работе 
          break;
        case 3: 
           GSM_analysisResponse(F("+CPAS: 0"));
            
          break;
        case 4: 
           GSM_AT_Command(F("CREG?"),10);
           //Проверка регистрации в сети    
          break;
        case 5: 
          if (GSM_analysisResponse(F("+CREG")))
        {
                if((GSM_reply.endsWith("1")) || (GSM_reply.endsWith("5"))) 
            {
                 GSM_Net =1;
            }
             // 1-В домашней сети, 5- в роуминге
                else 
            {
                 GSM_err(0,1);
                GSM_count--;
            }
                  
                
        }
          
          break;
        case 6: 
           GSM_AT_Command(F("CSCB=1") ,2);
             // Отключение приема широковещательных сообщений 
          break;
          
        case 7: 
           GSM_AT_Command(F("CLIP=1") ,2);
             // настройки автоматического определения номера
          break;
        case 8:
           GSM_AT_Command(F("CMGF=1") ,2) ; //настройки формата SMS сообщений, текстовый
          break;
        case 9: 
           GSM_AT_Command(F("CSCS=\"GSM\"") ,2);
           // выбора кодировки текста, кодировка ASCII
          break;
        case 10:
           GSM_AT_Command(F("CNMI=2,2,0,0,0") ,2) ; //выдовать смс в терминал и не сохранять в память
          break;
        case 11:
        #ifdef GSM_INCLUSION_DTMF  // Если добвален блок DTMF
          GSM_AT_Command(F("DDET=1") ,5); // Включение работы с DTMF
        #else
          GSM_count ++;
        #endif  
          break;
        #ifdef TELEFONNAYA_BOOK  // Если использовать телефонну книгу
        case 12:
           GSM_AT_Command(F("CPBS?"),2);
              
          break;
        case 13:
           GSM_analysisResponse(F("+CPBS: \"SM\""));
              
          break;
          
        case 14:
           GSM_AT_Command(F("CPBF=\""), GSM_Name_Contact , 2) ;
           GSM_index_array =0;
          break;
        case 15:
        #else  //Если не использовать телефонную книгу
        case 12:
        #endif  //Окнчание выбора по телефонной книге 
           GSM_count=0;
          
           GSM_Error_Code =0; //Сброc ошибки  
          return 1;
          break;
    }
    return 0;
}
void GSM_GotLineFromNeoway()
{
    bool flag_=0; //Анализ телефонной книги
    static bool isStringMessage;
    #ifdef GSM_DEBUG
    Serial.print("Message module: "); //Тест
    Serial.println(GSM_line); //Тест
    Serial.print("GSM_team_room: "); //Тест
    Serial.println(GSM_team_room); //Тест
    #endif
    if (GSM_line.length()>0)
      
    {
          
        //Обработка входящих СМС
            if (isStringMessage)
        {
                  GSM_Text_sms  =  GSM_line ; //выводим текст сообщения 
                  GSM_Received_SMS  =true; // Импульс приход сообщения
                  isStringMessage=0;
                  
                  return;
                
        }
          if (GSM_line.startsWith("+CMT"))  // пришло СМС 
            
        {
                flag_=1;
                GSM_resetExpectations();
                
                isStringMessage=1;
              
                GSM_reply="";
                
        }
        //Конец Обработка входящих СМС
        // Обработка входящих вызовов
          
          if (GSM_Call)
            
        {
                if (GSM_line.startsWith(F("+CLIP"))) 
            {
                flag_=1;
                GSM_reply="";
            }
                else if (GSM_line == F("NO CARRIER")) 
            {
                 GSM_Call=0;
            }
                
                
        }
          if (GSM_line == F("RING"))
        {
            //Входящий звонок 
                    if(!GSM_Call)GSM_resetExpectations();
                    GSM_Call=1;
                    GSM_Number_calls ++;
                    
                    
        }
        // ____ Обработка USSD sim800
        #ifdef GSM_TYPE_SIM800
        #ifdef GSM_INCLUSION_USSD
        if (GSM_line.startsWith("+CUSD"))  // пришел ответ по USSD
            
        {
                GSM_wait=0;
                GSM_count++;
                GSM_USSD_return  =   GSM_substring_commas(GSM_line,5) ;
                
        }
        #endif
        #endif
        //_______Конец обработки USSD SIM800
        #ifdef GSM_INCLUSION_CALLS  //Если добавлен блок работы со звонками
        #ifdef GSM_INCLUSION_DTMF  // Если добвален блок DTMF
          if (GSM_Connect)
        {
              if (GSM_line.startsWith(F("+DTMF"))) 
            {
                GSM_ID_DTMF(GSM_line);
            }
              
        }
        #endif
          if (GSM_team_room==5)
            
        {
                if (GSM_line == F("CONNECT"))GSM_Connect=1;
                else if (GSM_line == F("BUSY")) GSM_err (505,2);
                else if (GSM_line == F("NO ANSWER")) GSM_err (506,2);
                else if (GSM_line == F("NO CARRIER")) 
            {
                        if (GSM_Connect)
                {
                    GSM_Connect=0;
                    GSM_team_room=0;
                }
                        else GSM_err (507,2);
                      
            }
                
        }
        #endif
        // Конец Обработка входящих вызовов
          
          
          if(GSM_line == F("ERROR")) GSM_err (1, 0);
          else if(GSM_line == F("OK"))
        {
            GSM_wait=0;
            GSM_count++;
            GSM_errorCounter=0;
        }
          else 
        {
                GSM_reply+=GSM_line;
                
                
        }
          if (GSM_reply.length()>  160)
        {
            GSM_reply="";
            GSM_err(10100,2);
        }
//Защита от переполнения строки
        if (GSM_line == Module_Start)
        {
            GSM_start_N=1;
            GSM_start=0;
            GSM_err(0 , 2);
        }
//Сообщение от модуля о том, закончил загрузку 
        if (flag_)  //Получение номер телефона отправителя/звонящего
            
        {
                 GSM_T_Nomer =  GSM_substring_commas(GSM_line, 4);
              
                if(GSM_T_Nomer.indexOf("+")==-1)  GSM_T_Nomer=(String("+"))+ GSM_T_Nomer;
              
            #ifdef TELEFONNAYA_BOOK  // если используется телефооная книга
                int  GSM_ind_arr= GSM_StringArray(GSM_Telefon,  GSM_T_Nomer);
                if (GSM_ind_arr>-1)  GSM_T_Nomer = GSM_Contact [  GSM_ind_arr ];
            #endif
                flag_=0;
                
        }
        #ifdef TELEFONNAYA_BOOK  // если используется телефооная книга
          if (!GSM_start)
        {
                if (GSM_line.startsWith("+CPBF"))  // пришла запись телефонной книги
                  
            {
                if(GSM_index_array< GSM_Number_Contacts)
                {
                            GSM_Telefon[  GSM_index_array ] =  GSM_substring_commas(GSM_line, 5);
                            GSM_Contact[  GSM_index_array ]=  GSM_substring_commas(GSM_line, (GSM_line.lastIndexOf(","))) ;
                             GSM_index_array ++;
                }
                      
            }
              
        }
        #endif
    }
    // Получили строку ответа от Neoway. Любые ответы приходят в виде
// либо одной, либо нескольких строк, так что единица, которой мы должны
// оперировать - именно строка
// F("") -Хранить строку в Флеш памяти
}
void GSM_AT_Command(const __FlashStringHelper* str, String str2, byte _numRep)
{
    GSM_numberRepeats=_numRep;
    GSM_reply=""; //Сброс содержимого прошлого ответа модуля
     GSM_wait=1; // ожитать
     GSM_time =millis() + 20000; // время ожидания
    Serial_S.write("AT+");
    Serial_S.print(str);
    Serial_S.print(str2);
    Serial_S.write("\"\r\n");
    #ifdef GSM_DEBUG  // отладка
      Serial.write("Command: AT+");
      Serial.print(str);
      Serial.print(str2);
      Serial.write("\"\r\n");
      
    #endif
}
bool GSM_net_status()
{
    switch (GSM_count) 
    {
        case 0: 
           GSM_count ++;
          
          break;
        case 1: 
           GSM_AT_Command(F("CREG?") , 5);
           //Проверка регистрации в сети    
          break;
        case 2: 
          if (GSM_analysisResponse(F("+CREG")))
        {
                if((GSM_reply.endsWith("1")) || (GSM_reply.endsWith("5"))) 
            {
                 GSM_Net =1;
            }
             // 1-В домашней сети, 5- в роуминге
                else 
            {
                 GSM_Net =0;
            }
                
        }
          
          break;
            
        case 3:  // Проверка уровня сигнала 
           GSM_AT_Command(F("CSQ") , 5);
          
          break;
        case 4:   
          if (GSM_analysisResponse(F("+CSQ")))
        {
                     // 
                int t1= GSM_reply.indexOf(": ");
                if(t1>-1)    GSM_dBm  =((((GSM_reply.substring(t1+2,t1+4)).toInt())*(2))-(113));
                  
                if(GSM_dBm >0)   GSM_dBm =-120; //нет сети    
                
        }
            
          break;
        case 5: 
           GSM_count=0;
           GSM_time_n=millis(); // время последнего опроса  
           GSM_Error_Code =0; //Сброc ошибки
          return 1;
          break;
    }
    return 0;
}
bool GSM_sms_sent()
{
    switch (GSM_count) 
    {
        case 0: 
           GSM_count ++;
          
          break;
        case 1: 
           GSM_AT_Command(F("CMGF=1"),1);
             // Перевод сообщенй в текстовый режим  
          break;
        case 2: 
           GSM_AT_Command(F("CMGS=\""),  GSM_Phone_T ,1); // Отправка команды на начальный этап  отправки СМС
          break;
        case 3:  // передача текста СМС и отправка сообщения
          if (GSM_errorCounter)
        {
              GSM_err((200+ GSM_count) , 2);
            break;
        }
// выход из посылки команд если достигнуто придельное колличество ошибок
          Serial_S.println(GSM_text +String((char)26));
            
           GSM_wait=1; // ожитать
           GSM_time =millis() + 20000; // время ожидания
        #ifdef GSM_DEBUG  // отладка
          Serial.println(GSM_text +String((char)26));
            
        #endif
          break;
        case 4: 
           GSM_count=0;
          
           GSM_Sent_SMS =1;
           GSM_Error_Code =0; //Сброc ошибки
          return 1;
          
          break;
          
    }
    return 0;
}
String GSM_substring_commas(String text_analyze, int position)
{
    int t1=text_analyze.indexOf("\"", position);
    int t2=text_analyze.indexOf("\"",(t1+1));
    if(t1>-1)
    {
          if(t2>-1)  return text_analyze.substring(t1+1,t2);
          else return text_analyze.substring(t1+1);
    }
      
    return "";
}
bool GSM_number_search()
{
    if (GSM_Phone.length() <4) 
    {
        GSM_Error_Code =10005;
        return 0;
    }
    GSM_Phone_T = GSM_Phone ;
    #ifdef TELEFONNAYA_BOOK
    int ind_arr;
    ind_arr= GSM_StringArray(GSM_Contact ,  GSM_Phone);
    if (ind_arr>-1)  GSM_Phone_T = GSM_Telefon [ ind_arr ];
    else if(GSM_White_list) 
    {
        GSM_Error_Code =10006;
        return 0;
    }
      
    #endif
      
    return 1;
}
void GSM_resetExpectations()
{
//сброс текущей операции 
    GSM_wait=0; //сброс ожидания ответа
    GSM_count=0; //Сброс состояний обработки других функций
    GSM_time_n=GSM_T_millis; //отсрочка проверки статуса сети
    GSM_team_room=0;
}
void GSM_err(int error, byte _ret)
{
    GSM_errorCounter++ ;
    GSM_reply=""; //Сброс содержимого прошлого ответа модуля
    GSM_wait =0;
    if (_ret==0)
    {
          if (GSM_errorCounter >GSM_numberRepeats)
        {
                error = GSM_team_room*100+ GSM_count;
                _ret=2;
        }
          else return;
    }
    if (_ret==1)  GSM_count--;
    else if (_ret==2)
    {
          GSM_errorCounter=0;
          GSM_Error_Code =error;
          if (GSM_team_room==3)
        {
            GSM_Net =0;
            GSM_dBm =-120;
        }
          GSM_resetExpectations();
          
    }
}
void GSM_AT_Command(const __FlashStringHelper* str, byte _numRep)
{
    #ifdef GSM_DEBUG  // отладка  
      Serial.write("AT+ _ ");
      
    #endif
    Serial_S.write("AT+");
    GSM_Command(str,_numRep) ;
}
bool GSM_analysisResponse(const __FlashStringHelper* response)
{
    #ifdef GSM_DEBUG
    Serial.print("Expected Answer: "); //Тест
    Serial.println(response); //Тест
    Serial.print("Real  Answer: "); //Тест
    Serial.println(GSM_reply); //Тест
    #endif
    if (GSM_reply.startsWith(response))
    {
        GSM_count++;
        return 1;
    }
    GSM_err(0,1);
    return 0;
      
}
bool GSM_singleCommand(const __FlashStringHelper* str, byte _numRep)
{
    switch (GSM_count) 
    {
        case 0: 
          GSM_Command(str, _numRep) ;
          break;
        case 1: 
          GSM_count=0;
          
          GSM_Error_Code =0; //Сброc ошибки
          return 1;
          
          break;
    }
    return 0;
}
void GSM_Command(const __FlashStringHelper* str, byte _numRep)
{
    GSM_reply=""; //Сброс содержимого прошлого ответа модуля
    GSM_numberRepeats=_numRep;
    GSM_wait=1; // ожитать
    GSM_time =millis() + 20000; // время ожидания
    Serial_S.println(str);
    #ifdef GSM_DEBUG  // отладка
      Serial.write("Command: ");
      
      Serial.println(str);
    #endif
}
void GSM_ID_DTMF(String st)
{
    int www=(int(st.charAt(7)));
    switch (www) 
    {
        case 35:  
           GSM_DTMF =14;
          
          break;
        case 42:    
           GSM_DTMF =15;
          
          break;
        case 65:
           GSM_DTMF =10;
          break;
        case 66:  
           GSM_DTMF =11;
          break;
          
        case 67:
           GSM_DTMF =12;
          break;
        case 68:  
           GSM_DTMF =13;
          break;
          
        default:
        if (www>47 && www<58) GSM_DTMF =www-48;
    }
}
bool GSM_causeProcessingCall()
{
    static bool past_Outgoing_Call=0;
    static bool command_Outgoing_Call=0;
    if (GSM_Outgoing_Call   && !past_Outgoing_Call)  command_Outgoing_Call  =1;
    past_Outgoing_Call  = GSM_Outgoing_Call ;
    if (command_Outgoing_Call  && (GSM_team_room==0))  
    {
           command_Outgoing_Call  =0;
        return 1;
    }
     // начинать запрос времени 
    return 0;
}
bool GSM_causeProcessingTake()
{
    static bool past_Take_Call;
    static bool command_Take_Call;
    if (GSM_Take_Call    && !past_Take_Call)  
    {
         command_Take_Call=1;
    }
      past_Take_Call   =  GSM_Take_Call;
    if (command_Take_Call) 
    {
          switch (GSM_count) 
        {
              case 0: 
                GSM_count++;
                break;
              case 1: 
                GSM_Command(F("ATA"), 5);
               // Поднять трубку
              #ifdef GSM_DEBUG  // отладка    
                Serial.println("Command: ATA");
              
              #endif
                break;
              case 2: 
                GSM_count=0;
                command_Take_Call  =0;
                return 1;
                break;
              
              
        }
    }
    return 0;
}
void i2cWrite(uint8_t registerAddress, uint8_t data)
{
      Wire.beginTransmission(IMUAddress);
      Wire.write(registerAddress);
      Wire.write(data);
      Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes)
{
      uint8_t data[nbytes];
      Wire.beginTransmission(IMUAddress);
      Wire.write(registerAddress);
      Wire.endTransmission(false); // Don't release the bus
      Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
      for(uint8_t i = 0; i < nbytes; i++)
      data [i]= Wire.read();
      return data;
}

Прошу не пинать, желающих пофлюдить типа, иди читай, иди изучай, прошу воздержатся, ну не дается мне программирование. Желающих покидать тапками типа, тупой, это же просто и т.д, тоже прошу воздержатся. Если есть добрые люди которые хотят помочь, буду очень признателен.