Интересный код. Исправление ошибок)

ЧЧ
,
1

Проверьте весь код


#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <UniversalTelegramBot.h>

// Wifi network station credentials
#define WIFI_SSID "wifi login"
#define WIFI_PASSWORD "parol"
// Telegram BOT Token (Get from Botfather)
#define BOT_TOKEN "token"

#include <Wire.h>
#include <PCA9634.h>
#include <Adafruit_PWMServoDriver.h>                          // библиотека для моторной платы
#include "Adafruit_APDS9960.h"
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x70);  // адрес платы
Adafruit_APDS9960 apds9960;
float sum;
PCA9634 testModule(0x4D);
#define sensor_addr 0x3F // Переключатели адреса в положении "OFF"

// Константы для I2C шилда
#define I2C_HUB_ADDR        0x70
#define EN_MASK             0x08
#define DEF_CHANNEL         0x00
#define MAX_CHANNEL         0x08

/*
  I2C порт 0x07 - выводы GP16 (SDA), GP17 (SCL)
  I2C порт 0x06 - выводы GP4 (SDA), GP13 (SCL)
  I2C порт 0x05 - выводы GP14 (SDA), GP15 (SCL)
  I2C порт 0x04 - выводы GP5 (SDA), GP23 (SCL)
  I2C порт 0x03 - выводы GP18 (SDA), GP19 (SCL)
*/

const unsigned long BOT_MTBS = 1000; // mean time between scan messages

WiFiClientSecure secured_client;
UniversalTelegramBot bot(BOT_TOKEN, secured_client);
unsigned long bot_lasttime; // last time messages' scan has been done

#include "TLC59108.h"
#define HW_RESET_PIN 0  // Только программный сброс
#define I2C_ADDR TLC59108::I2C_ADDR::BASE

#include "IRremote.h"    // библиотека для пультов ДУ
IRrecv irrecv(27);       // пин 27 для ИК-приемника
decode_results results;  // переменная для принимаемого сигнала

TLC59108 leds(I2C_ADDR + 0);

int backLine = 1475;
float speedDino = 15; //скорость динамики

int massSens[17];
byte digitData[17];
int Kf[] = { 120, 100, 90, 75, 65, 50, 25, 10, 0, -10, -25, -50, -65, -75, -90, -100, -120 };
int findKf[17];
int maxKf = 0, minKf = 0, l;
float Lkf, Rkf;
byte rotate = 0;
int color = 0;
bool inTurn = false;
int targetColor = 0;
bool flagmove = false;
uint32_t t = 0;
String chat_id;

static volatile int p00 = 0;
static volatile int p01 = 0;
static volatile int p02 = 0;
static volatile int p03 = 0;
static volatile int p04 = 0;
static volatile int p05 = 0;
static volatile int p06 = 0;
static volatile int p07 = 0;
static volatile int p08 = 0;
static volatile int p09 = 0;
static volatile int p10 = 0;
static volatile int p11 = 0;
static volatile int p12 = 0;
static volatile int p13 = 0;
static volatile int p14 = 0;
static volatile int p15 = 0;
static volatile int p16 = 0;
static volatile int p17 = 0;
static volatile int p18 = 0;

#define I2C_HUB_ADDR 0x70  // настройки I2C для платы MGB-I2C63EN
#define EN_MASK 0x08
#define DEF_CHANNEL 0x00
#define MAX_CHANNEL 0x08

/*
  I2C порт 0x07 - выводы GP16 (SDA), GP17 (SCL)
  I2C порт 0x06 - выводы GP4 (SDA), GP13 (SCL)
  I2C порт 0x05 - выводы GP14 (SDA), GP15 (SCL)
  I2C порт 0x04 - выводы GP5 (SDA), GP23 (SCL)
  I2C порт 0x03 - выводы GP18 (SDA), GP19 (SCL)
*/

void handleNewMessages(int numNewMessages)
{
  Serial.print("handleNewMessages ");
  Serial.println(numNewMessages);

  for (int i = 0; i < numNewMessages; i++)
  {
    chat_id = bot.messages[i].chat_id;
    String text = bot.messages[i].text;

    String from_name = bot.messages[i].from_name;
    if (from_name == "")
      from_name = "Guest";
    if (text == "/Yellow") //команда до синей линии
    {
      targetColor = 1;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на жёлтую станцию", "");
    }
    if (text == "/Purple") //команда до синей линии
    {
      targetColor = 2;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на серенивую станцию", "");
    }
    if (text == "/Red") //команда до синей линии
    {
      targetColor = 3;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на Красную станцию", "");
    }
    if (text == "/Green") //команда до синей линии
    {
      targetColor = 4;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на Зелёную станцию", "");
    }
    if (text == "/Blue") //команда до синей линии
    {
      targetColor = 5;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на синюю станцию", "");
    }

    if (text == "/start")
    {
      String welcome = "Привет," + from_name + " я робот доставщик, " + ".\n";
      welcome += "Тут ты можешь выбрать на какую станцию мне отвезти груз.\n\n";
     
      welcome += "/Blue : синий груз\n";
      welcome += "/Green : зелёный груз\n";
      welcome += "/Red : красный груз\n";
      welcome += "/Purple : серенивый груз\n";
      welcome += "/Yellow : жёлтый  груз\n";
      bot.sendMessage(chat_id, welcome, "Markdown");
    }
  }
}

void setup() {
  // Инициализация последовательного порта
  Serial.begin(115200);

  Serial.print("Connecting to Wifi SSID ");
  Serial.print(WIFI_SSID);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  secured_client.setCACert(TELEGRAM_CERTIFICATE_ROOT); // Add root certificate for api.telegram.org
  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(500);
  }
  Serial.print("\nWiFi connected. IP address: ");
  Serial.println(WiFi.localIP());

  irrecv.enableIRIn();  // запускаем прием
  // Инициализация датчика
  Wire.begin();
  Wire.setClock(100000L);
  delay(100);
  init_sensor();

  // Инициализация датчика
  if (!apds9960.begin()) {
    Serial.println("Failed to initialize device!");
  }
  // Инициализация режимов работы датчика
  apds9960.enableColor(true);
  apds9960.enableProximity(true);

  setBusChannel(0x03);
  testModule.begin();
  for (int channel = 0; channel < testModule.channelCount(); channel++)
  {
    testModule.setLedDriverMode(channel, PCA9634_LEDOFF); // выключить все светодиоды в режиме 0/1
  }

  pwm.begin();
  // Частота (Гц)
  pwm.setPWMFreq(100);
  // Все порты выключены
  pwm.setPWM(8, 0, 4096);
  pwm.setPWM(9, 0, 4096);
  pwm.setPWM(10, 0, 4096);
  pwm.setPWM(11, 0, 4096);

  delay(1000);
  l = Targetline();
  setBusChannel(0x03);
  for (int channel = 0; channel < testModule.channelCount(); channel++)
  {
    testModule.setLedDriverMode(channel, PCA9634_LEDPWM); // установка режима ШИМ (0-255)
  }
  testModule.write1(7, 0x90);
  delay(333);
  testModule.write1(7, 0x00);
}

void loop() {
  // Измерение
  poll_sensor();
  digitSensor();
  addKf();
  //serial();
  checkColor();

  if (flagmove) { // если можно двигаться едем
    checkStop();  // смотрим доехали ли до перекрёстка
    if (rotate != 0) { //если доехали до перекрёстка и нужно поворачивать то смотрим куда поворачивать
      if (rotate == 1) { //поворачиваем на станцию загрузки
        if (inTurn) { //если до этого повернули на станцию загрузки и то останавливаемся 
          flagmove = false;
          //отправляем сообщение в телеграм о прибытии
          return;
        }
        motorA_setpower(speedDino, true); //поворачиваем налево
        delay(3000);
        rotate = 0;
        stopDina();
        delay(3000);
      }
      if (rotate == 2) {
        //поворачиваем направо
      }
      if (rotate == 3) { //поворот на цвете
        if (color == targetColor) { //цвет с датчика совпадает с цветом запрошеным в талеграмм
          bot.sendMessage(chat_id, "Поворачиваю на станцию", "");
          motorA_setpower(speedDino, true);
          delay(1000);
          rotate = 0;
          color = 0;
          inTurn = true;          
        } else {
          bot.sendMessage(chat_id, "Еду прямо", ""); //если поворот после цвета не тот проезжаем перекрёсток прямо
          bot.sendMessage(chat_id, String(color), "");
          //?
          delay(500);
          rotate = 0;
        }
      }
    }

    //if (millis() - t < 10000) {
    if (-minKf == maxKf) {
      motorA_setpower(speedDino, true);
      motorB_setpower(speedDino, true);
    } else {
      maxKf = maxKf < 0 ? 0 : maxKf;
      minKf = minKf > 0 ? 0 : minKf;

      Lkf = speedDino / 100 * (float)minKf;
      Rkf = speedDino / 100 * (float)maxKf;
      //Serial.println(String(speedDino - Rkf) + " / " + String(speedDino + Lkf));
      motorB_setpower(speedDino - Rkf, true);
      motorA_setpower(speedDino + Lkf, true);
    }
  } else {
    motorB_setpower(0, true);
    motorA_setpower(0, true);

    if (millis() - bot_lasttime > BOT_MTBS)
    {
      updateTG();
    }
  }
}

void checkColor() {
  uint16_t red_data   = 0;
  uint16_t green_data = 0;
  uint16_t blue_data  = 0;
  uint16_t clear_data = 0;
  uint16_t prox_data  = 0;
  // Определение цвета
  while (!apds9960.colorDataReady()) {
    delay(5);
  }
  apds9960.getColorData(&red_data, &green_data, &blue_data, &clear_data);
  // Определение близости препятствия
  prox_data = apds9960.readProximity();
  // Вывод измеренных значений в терминал

  sum = red_data + green_data + blue_data;
  Serial.println("RED   = " + String((float)red_data / sum * 100)); //вычисляем процент красного в какой то карточке
  Serial.println("GREEN   = " + String((float)green_data / sum * 100));
  Serial.println("BLUE   = " + String((float)blue_data / sum * 100));
  red_data = (float)red_data / sum * 100;
  green_data = (float)green_data / sum * 100;
  blue_data = (float)blue_data / sum * 100;

  setBusChannel(0x03);
  for (int channel = 0; channel < testModule.channelCount(); channel++)
  {
    testModule.setLedDriverMode(channel, PCA9634_LEDPWM); // установка режима ШИМ (0-255)
  }
  
 /*
  * 1 - yellow
  * 2 - purple
  * 3 - red
  * 4 - green
  * 5 - blue
  */
  if (false) { //своё условие для определения белого цвета
    testModule.write1(3, 0x90);
    testModule.write1(7, 0x90);
    testModule.write1(4, 0x90);
  } else {
    if (red_data > blue_data && green_data > blue_data) {
      Serial.println ("YELLOW");
      testModule.write1(3, 0x90);
      testModule.write1(7, 0x90);
      testModule.write1(4, 0x00);
      color = 1;
    } else {
      if (blue_data < 54 && blue_data > 45 && red_data < 30 && red_data >= 20) {
        Serial.println ("PURPLE");
        testModule.write1(3, 0x90);
        testModule.write1(7, 0x00);
        testModule.write1(4, 0x90);
        color = 2;
      } else {
        if (red_data > green_data && red_data > blue_data) {
          Serial.println ("RED");
          testModule.write1(3, 0x90);
          testModule.write1(7, 0x00);
          testModule.write1(4, 0x00);
          color = 3;
        } else {
          if (green_data > red_data && green_data > blue_data) {
            Serial.println ("GREEN");
            testModule.write1(3, 0x00);
            testModule.write1(7, 0x90);
            testModule.write1(4, 0x00);
            color = 4;
          } else if (blue_data > red_data && blue_data > green_data)
          {
            Serial.println ("BLUE");
            testModule.write1(3, 0x00);
            testModule.write1(7, 0x00);
            testModule.write1(4, 0x90);
            color = 5;
          }
        }
      }
    }
  }
}

void checkStop() {
  byte sum = 0;

  for (int i = 0; i < 17; i++) { //если заехали на Т образный перекрёсток
    if (digitData[i] == 1) sum++;
  }
  if (sum >= 12) { //если 12 датчиков наехали на чёрное
    rotate = 1;
    stopDina();
    return;
  }
  sum = 0;
  for (int i = 8; i < 17; i++) { //если сенсоры справа нашли дорогу (поворот направо)
    if (digitData[i] == 1) sum++;
  }
  if (sum >= 3) { //сколько датчиков наехали на линию?
    rotate = 2;
    stopDina();
  }
  sum = 0;
  for (int i = 0; i < 8; i++) { //поворот налево
    if (digitData[i] == 1) sum++;
  }
  if (sum >= 7) {
    rotate = 3;
    stopDina();
  }
}

void stopDina() {
  motorA_setpower(-10, true);
  motorB_setpower(-10, true);
  delay(100);
  motorA_setpower(0, true);
  motorB_setpower(0, true);
}

void updateTG() {
  int numNewMessages = bot.getUpdates(bot.last_message_received + 1);

  while (numNewMessages)
  {
    Serial.println("got response");
    handleNewMessages(numNewMessages);
    numNewMessages = bot.getUpdates(bot.last_message_received + 1);
  }
}

void serial() {
  Serial.print(p00);
  Serial.print(" ");
  Serial.print(p01);
  Serial.print(" ");
  Serial.print(p02);
  Serial.print(" ");
  Serial.print(p03);
  Serial.print(" ");
  Serial.print(p04);
  Serial.print(" ");
  Serial.print(p05);
  Serial.print(" ");
  Serial.print(p06);
  Serial.print(" ");
  Serial.print(p07);
  Serial.print(" ");
  Serial.print(p08);
  Serial.print(" ");
  Serial.print(p09);
  Serial.print(" ");
  Serial.print(p10);
  Serial.print(" ");
  Serial.print(p11);
  Serial.print(" ");
  Serial.print(p12);
  Serial.print(" ");
  Serial.print(p13);
  Serial.print(" ");
  Serial.print(p14);
  Serial.print(" ");
  Serial.print(p15);
  Serial.print(" ");
  Serial.print(p16);
  Serial.print(" ");
  Serial.print(p17);
  Serial.print(" ");
  Serial.print(p18);
  Serial.println();
  delay(10);
}

void digitSensor() {
  digitData[0] = p01 > l ? 1 : 0;
  digitData[1] = p02 > l ? 1 : 0;
  digitData[2] = p03 > l ? 1 : 0;
  digitData[3] = p04 > l ? 1 : 0;
  digitData[4] = p05 > l ? 1 : 0;
  digitData[5] = p06 > l ? 1 : 0;
  digitData[6] = p07 > l ? 1 : 0;
  digitData[7] = p08 > l ? 1 : 0;
  digitData[8] = p09 > l ? 1 : 0;
  digitData[9] = p10 > l ? 1 : 0;
  digitData[10] = p11 > l ? 1 : 0;
  digitData[11] = p12 > l ? 1 : 0;
  digitData[12] = p13 > l ? 1 : 0;
  digitData[13] = p14 > l ? 1 : 0;
  digitData[14] = p15 > l ? 1 : 0;
  digitData[15] = p16 > l ? 1 : 0;
  digitData[16] = p17 > l ? 1 : 0;
  for (int i = 0; i < 17; i++) {
    Serial.print(digitData[i]);
  }
  Serial.println();
}

void addKf() {
  maxKf = 0;
  minKf = 0;
  for (int i = 0; i < 17; i++) {
    findKf[i] = digitData[i] * Kf[i];
    maxKf = max(maxKf, findKf[i]);
    minKf = min(minKf, findKf[i]);
  }
}

// Инициализация датчика
void init_sensor() {
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x10);        // Регистр настройки всей микросхемы
  Wire.write(0b00000000);  // Нормальный режим работы
  Wire.write(0b01001111);  // АЦП в непрерывном режиме, 200 ksps, встроенный ИОН для ЦАП
  Wire.endTransmission();
  delay(1000);
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x20);        // Регистр настройки порта 0 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x21);        // Регистр настройки порта 1 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x22);        // Регистр настройки порта 2 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x23);        // Регистр настройки порта 3 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x24);        // Регистр настройки порта 4 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x25);        // Регистр настройки порта 5 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x26);        // Регистр настройки порта 6 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x27);        // Регистр настройки порта 7 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x28);        // Регистр настройки порта 8 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x29);        // Регистр настройки порта 9 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2A);        // Регистр настройки порта 10 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2B);        // Регистр настройки порта 11 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2C);        // Регистр настройки порта 12 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2D);        // Регистр настройки порта 13 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2E);        // Регистр настройки порта 14 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2F);        // Регистр настройки порта 15 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x30);        // Регистр настройки порта 16 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x31);        // Регистр настройки порта 17 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x32);        // Регистр настройки порта 18 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  delay(1000);
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x20);        // Регистр настройки порта 0 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x21);        // Регистр настройки порта 1 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x22);        // Регистр настройки порта 2 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x23);        // Регистр настройки порта 3 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x24);        // Регистр настройки порта 4 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x25);        // Регистр настройки порта 5 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x26);        // Регистр настройки порта 6 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x27);        // Регистр настройки порта 7 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x28);        // Регистр настройки порта 8 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x29);        // Регистр настройки порта 9 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2A);        // Регистр настройки порта 10 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2B);        // Регистр настройки порта 11 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2C);        // Регистр настройки порта 12 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2D);        // Регистр настройки порта 13 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2E);        // Регистр настройки порта 14 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2F);        // Регистр настройки порта 15 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x30);        // Регистр настройки порта 16 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x31);        // Регистр настройки порта 17 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x32);        // Регистр настройки порта 18 (подключен к оптическому сенсору)
  Wire.write(0b01111001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  delay(1000);
  // Отладка регистров
  /*
    int a = 0;
    int b = 0;
    Wire.beginTransmission(sensor_addr);
    Wire.write(0x45); // Регистр данных АЦП
    Wire.endTransmission();
    Wire.requestFrom(sensor_addr, 2);
    Serial.println(Wire.available());
    a = Wire.read();
    b = Wire.read();
    Serial.println(String(a, 2));
    Serial.println(String(b, 2));
  */
}

// Получение данных с датчика
void poll_sensor() {
  int adc_sensor_data[38] = { 0 };
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x40);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 10);
  if (Wire.available() == 10) {
    adc_sensor_data[0] = Wire.read();  // ADC00
    adc_sensor_data[1] = Wire.read();
    adc_sensor_data[2] = Wire.read();  // ADC01
    adc_sensor_data[3] = Wire.read();
    adc_sensor_data[4] = Wire.read();  // ADC02
    adc_sensor_data[5] = Wire.read();
    adc_sensor_data[6] = Wire.read();  // ADC03
    adc_sensor_data[7] = Wire.read();
    adc_sensor_data[8] = Wire.read();  // ADC04
    adc_sensor_data[9] = Wire.read();
  }
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x45);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 10);
  if (Wire.available() == 10) {
    adc_sensor_data[10] = Wire.read();  // ADC05
    adc_sensor_data[11] = Wire.read();
    adc_sensor_data[12] = Wire.read();  // ADC06
    adc_sensor_data[13] = Wire.read();
    adc_sensor_data[14] = Wire.read();  // ADC07
    adc_sensor_data[15] = Wire.read();
    adc_sensor_data[16] = Wire.read();  // ADC08
    adc_sensor_data[17] = Wire.read();
    adc_sensor_data[18] = Wire.read();  // ADC09
    adc_sensor_data[19] = Wire.read();
  }
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x4A);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 10);
  if (Wire.available() == 10) {
    adc_sensor_data[20] = Wire.read();  // ADC10
    adc_sensor_data[21] = Wire.read();
    adc_sensor_data[22] = Wire.read();  // ADC11
    adc_sensor_data[23] = Wire.read();
    adc_sensor_data[24] = Wire.read();  // ADC12
    adc_sensor_data[25] = Wire.read();
    adc_sensor_data[26] = Wire.read();  // ADC13
    adc_sensor_data[27] = Wire.read();
    adc_sensor_data[28] = Wire.read();  // ADC14
    adc_sensor_data[29] = Wire.read();
  }
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x4F);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 8);
  if (Wire.available() == 8) {
    adc_sensor_data[30] = Wire.read();  // ADC15
    adc_sensor_data[31] = Wire.read();
    adc_sensor_data[32] = Wire.read();  // ADC16
    adc_sensor_data[33] = Wire.read();
    adc_sensor_data[34] = Wire.read();  // ADC17
    adc_sensor_data[35] = Wire.read();
    adc_sensor_data[36] = Wire.read();  // ADC18
    adc_sensor_data[37] = Wire.read();
  }
  p00 = adc_sensor_data[36] * 256 + adc_sensor_data[37];
  p01 = adc_sensor_data[34] * 256 + adc_sensor_data[35];
  p02 = adc_sensor_data[32] * 256 + adc_sensor_data[33];
  p03 = adc_sensor_data[30] * 256 + adc_sensor_data[31];
  p04 = adc_sensor_data[28] * 256 + adc_sensor_data[29];
  p05 = adc_sensor_data[26] * 256 + adc_sensor_data[27];
  p06 = adc_sensor_data[24] * 256 + adc_sensor_data[25];
  p07 = adc_sensor_data[22] * 256 + adc_sensor_data[23];
  p08 = adc_sensor_data[20] * 256 + adc_sensor_data[21];
  p09 = adc_sensor_data[18] * 256 + adc_sensor_data[19];
  p10 = adc_sensor_data[16] * 256 + adc_sensor_data[17];
  p11 = adc_sensor_data[14] * 256 + adc_sensor_data[15];
  p12 = adc_sensor_data[12] * 256 + adc_sensor_data[13];
  p13 = adc_sensor_data[10] * 256 + adc_sensor_data[11];
  p14 = adc_sensor_data[8] * 256 + adc_sensor_data[9];
  p15 = adc_sensor_data[6] * 256 + adc_sensor_data[7];
  p16 = adc_sensor_data[4] * 256 + adc_sensor_data[5];
  p17 = adc_sensor_data[2] * 256 + adc_sensor_data[3];
  p18 = adc_sensor_data[0] * 256 + adc_sensor_data[1];

  massSens[0] = p01;
  massSens[1] = p02;
  massSens[2] = p03;
  massSens[3] = p04;
  massSens[4] = p05;
  massSens[5] = p06;
  massSens[6] = p07;
  massSens[7] = p08;
  massSens[8] = p09;
  massSens[9] = p10;
  massSens[10] = p11;
  massSens[11] = p12;
  massSens[12] = p13;
  massSens[13] = p14;
  massSens[14] = p15;
  massSens[15] = p16;
  massSens[16] = p17;
}

int Targetline() {
  poll_sensor()
  int max = 0;
  for(int i =  0; i < 17; i++ ){
    if(massSens[i] > max){
      max = massSens[i];
    }
  }
  max -= 100;
  return max;
}

// Мощность мотора "A" от -100% до +100% (от знака зависит направление вращения)
void motorA_setpower(float pwr, bool invert) {
  // Проверка, инвертирован ли мотор
  if (!invert) {
    pwr = -pwr;
  }
  // Проверка диапазонов
  if (pwr < -100) {
    pwr = -100;
  }
  if (pwr > 100) {
    pwr = 100;
  }
  int pwmvalue = fabs(pwr) * 40.95;
  if (pwr < 0) {
    pwm.setPWM(10, 0, 4096);
    pwm.setPWM(11, 0, pwmvalue);
  } else {
    pwm.setPWM(11, 0, 4096);
    pwm.setPWM(10, 0, pwmvalue);
  }
}

// Мощность мотора "B" от -100% до +100% (от знака зависит направление вращения)
void motorB_setpower(float pwr, bool invert) {
  // Проверка, инвертирован ли мотор
  if (invert) {
    pwr = -pwr;
  }
  // Проверка диапазонов
  if (pwr < -100) {
    pwr = -100;
  }
  if (pwr > 100) {
    pwr = 100;
  }
  int pwmvalue = fabs(pwr) * 40.95;
  if (pwr < 0) {
    pwm.setPWM(8, 0, 4096);
    pwm.setPWM(9, 0, pwmvalue);
  } else {
    pwm.setPWM(9, 0, 4096);
    pwm.setPWM(8, 0, pwmvalue);
  }
}

bool setBusChannel(uint8_t i2c_channel)  // смена I2C порта
{
  if (i2c_channel >= MAX_CHANNEL) {
    return false;
  } else {
    Wire.beginTransmission(I2C_HUB_ADDR);
    Wire.write(i2c_channel | EN_MASK);
    Wire.endTransmission();
    return true;
  }
}
2

Есть, сэр!

Разрешите приступить?!

1 лайк
3

“как проверить, лезть под платье? так схлопочешь по мозгам…” (c) В.В.

4

строка 153 - сиреневый проверочное слово сирень

2 лайка
5

В оригинале было “по мордам”

“По мозгам” здесь неуместно, т.к. подходит не для всех, а только для тех, у кого они есть :frowning:

6

с этим как раз напряжёнка, нашел и исправил еще одну ошибку в коде, кошмар )))

7

Проверил!
Неинтересно.

8

и где ошибки?

9

А про это вопросов не было.

10

А разве ТС что-то писал про ошибки? Он приказал проверить код. Человек проверил и доложил (хотя, и докладывать приказа не было). Какие к нему могут быть вопросы?

А я вот всё жду разрешения начать проверку :frowning:

1 лайк
11

Весь - не получается. Максимум 15000 символов.

12

Ограничение бесплатной подписки?

13

Угу.
По этим первым 15000 сообщает, что текст на 100% уникален.

14

а вы что хотели?

15 

#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <UniversalTelegramBot.h>

// Wifi network station credentials
#define WIFI_SSID "******"
#define WIFI_PASSWORD " *****"
// Telegram BOT Token (Get from Botfather)
#define BOT_TOKEN "*******************************************************"

#include <Wire.h>
#include <PCA9634.h>
#include <Adafruit_PWMServoDriver.h>                          // библиотека для моторной платы
#include "Adafruit_APDS9960.h"
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x70);  // адрес платы
Adafruit_APDS9960 apds9960;
float sum;
PCA9634 testModule(0x4D);
#define sensor_addr 0x3F // Переключатели адреса в положении "OFF"

// Константы для I2C шилда
#define I2C_HUB_ADDR        0x70
#define EN_MASK             0x08
#define DEF_CHANNEL         0x00
#define MAX_CHANNEL         0x08

/*
  I2C порт 0x07 - выводы GP16 (SDA), GP17 (SCL)
  I2C порт 0x06 - выводы GP4 (SDA), GP13 (SCL)
  I2C порт 0x05 - выводы GP14 (SDA), GP15 (SCL)
  I2C порт 0x04 - выводы GP5 (SDA), GP23 (SCL)
  I2C порт 0x03 - выводы GP18 (SDA), GP19 (SCL)
*/

const unsigned long BOT_MTBS = 1000; // mean time between scan messages

WiFiClientSecure secured_client;
UniversalTelegramBot bot(BOT_TOKEN, secured_client);
unsigned long bot_lasttime; // last time messages' scan has been done

#include "TLC59108.h"
#define HW_RESET_PIN 0  // Только программный сброс
#define I2C_ADDR TLC59108::I2C_ADDR::BASE

#include "IRremote.h"    // библиотека для пультов ДУ
IRrecv irrecv(27);       // пин 27 для ИК-приемника
decode_results results;  // переменная для принимаемого сигнала

TLC59108 leds(I2C_ADDR + 0);

int backLine = 1475;
float speedDino = 15;

int massSens[17];
byte digitData[17];
int Kf[] = { 120, 100, 90, 75, 65, 50, 25, 10, 0, -10, -25, -50, -65, -75, -90, -100, -120 };
int findKf[17];
int maxKf = 0, minKf = 0, l;
float Lkf, Rkf;
byte rotate = 0;
int color = 0;
bool inTurn = false;
int targetColor = 0;
bool flagmove = false;
uint32_t t = 0;
String chat_id;

static volatile int p00 = 0;
static volatile int p01 = 0;
static volatile int p02 = 0;
static volatile int p03 = 0;
static volatile int p04 = 0;
static volatile int p05 = 0;
static volatile int p06 = 0;
static volatile int p07 = 0;
static volatile int p08 = 0;
static volatile int p09 = 0;
static volatile int p10 = 0;
static volatile int p11 = 0;
static volatile int p12 = 0;
static volatile int p13 = 0;
static volatile int p14 = 0;
static volatile int p15 = 0;
static volatile int p16 = 0;
static volatile int p17 = 0;
static volatile int p18 = 0;

#define I2C_HUB_ADDR 0x70  // настройки I2C для платы MGB-I2C63EN
#define EN_MASK 0x08
#define DEF_CHANNEL 0x00
#define MAX_CHANNEL 0x08

/*
  I2C порт 0x07 - выводы GP16 (SDA), GP17 (SCL)
  I2C порт 0x06 - выводы GP4 (SDA), GP13 (SCL)
  I2C порт 0x05 - выводы GP14 (SDA), GP15 (SCL)
  I2C порт 0x04 - выводы GP5 (SDA), GP23 (SCL)
  I2C порт 0x03 - выводы GP18 (SDA), GP19 (SCL)
*/

void handleNewMessages(int numNewMessages)
{
  Serial.print("handleNewMessages ");
  Serial.println(numNewMessages);

  for (int i = 0; i < numNewMessages; i++)
  {
    chat_id = bot.messages[i].chat_id;
    String text = bot.messages[i].text;

    String from_name = bot.messages[i].from_name;
    if (from_name == "")
      from_name = "Guest";

    if (text == "/Yellow")
    {
      targetColor = 1;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на желтую станцию", "");
    }
    if (text == "/Purple")
    {
      targetColor = 1;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на фиолетовую станцию", "");
    }
    if (text == "/Red")
    {
      targetColor = 1;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на красную станцию", "");
    }
    if (text == "/Green")
    {
      targetColor = 1;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на зеленую станцию", "");
    }
    if (text == "/Blue")
    {
      targetColor = 1;
      flagmove = true;
      bot.sendMessage(chat_id, "Еду на синюю станцию", "");
    }

    if (text == "/start")
    {
      String welcome = "Привет," + from_name + " я робот доставщик, " + ".\n";
      welcome += "Тут ты можешь выбрать на какую станцию мне отвезти груз.\n\n";
      welcome += "/Yellow : желтый груз\n";
      welcome += "/Purple : фиолетовый груз\n";
      welcome += "/Red : красный груз\n";
      welcome += "/Green : зеленый груз\n";
      welcome += "/Blue : синий груз\n";
      bot.sendMessage(chat_id, welcome, "Markdown");
    }
  }
}

void setup() {
  // Инициализация последовательного порта
  Serial.begin(115200);

  Serial.print("Connecting to Wifi SSID ");
  Serial.print(WIFI_SSID);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  secured_client.setCACert(TELEGRAM_CERTIFICATE_ROOT); // Add root certificate for api.telegram.org
  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(500);
  }
  Serial.print("\nWiFi connected. IP address: ");
  Serial.println(WiFi.localIP());

  irrecv.enableIRIn();  // запускаем прием
  // Инициализация датчика
  Wire.begin();
  Wire.setClock(100000L);
  delay(100);
  init_sensor();

  // Инициализация датчика
  if (!apds9960.begin()) {
    Serial.println("Failed to initialize device!");
  }
  // Инициализация режимов работы датчика
  apds9960.enableColor(true);
  apds9960.enableProximity(true);

  setBusChannel(0x03);
  testModule.begin();
  for (int channel = 0; channel < testModule.channelCount(); channel++)
  {
    testModule.setLedDriverMode(channel, PCA9634_LEDOFF); // выключить все светодиоды в режиме 0/1
  }

  pwm.begin();
  // Частота (Гц)
  pwm.setPWMFreq(100);
  // Все порты выключены
  pwm.setPWM(8, 0, 4096);
  pwm.setPWM(9, 0, 4096);
  pwm.setPWM(10, 0, 4096);
  pwm.setPWM(11, 0, 4096);

  delay(1000);
  l = Targetline();
  setBusChannel(0x03);
  for (int channel = 0; channel < testModule.channelCount(); channel++)
  {
    testModule.setLedDriverMode(channel, PCA9634_LEDPWM); // установка режима ШИМ (0-255)
  }
  testModule.write1(7, 0x90);
  delay(333);
  testModule.write1(7, 0x00);
}

void loop() {
  // Измерение
  poll_sensor();
  digitSensor();
  addKf();
  //serial();
  checkColor();

  if (flagmove) {
    checkStop();
    if (rotate != 0) {
      if (rotate == 1) {
        if (inTurn) {
          flagmove = false;
          bot.sendMessage(chat_id, "Приехал на станцию выгрузки", "");
          return;
        }
        motorA_setpower(speedDino, true);
        delay(1000);
        rotate = 0;
        stopDina();
        delay(3000);
      }
      if (rotate == 2) {
        motorB_setpower(speedDino, true);
        delay(1000);
        rotate = 0;
        stopDina();
        delay(3000);
      }
      if (rotate == 3) { //поворот на цвете
        if (color == targetColor) {
          bot.sendMessage(chat_id, "Поворачиваю на станцию", "");
          motorA_setpower(speedDino, true);
          delay(1000);
          rotate = 0;
          color = 0;
          inTurn = true;          
        } else {
          bot.sendMessage(chat_id, "Еду прямо", "");
          bot.sendMessage(chat_id, String(color), "");
          motorA_setpower(speedDino, true);
          motorB_setpower(speedDino, true);
          delay(500);
          rotate = 0;
        }
      }
    }

    //if (millis() - t < 10000) {
    if (-minKf == maxKf) {
      motorA_setpower(speedDino, true);
      motorB_setpower(speedDino, true);
    } else {
      maxKf = maxKf < 0 ? 0 : maxKf;
      minKf = minKf > 0 ? 0 : minKf;

      Lkf = speedDino / 100 * (float)minKf;
      Rkf = speedDino / 100 * (float)maxKf;
      //Serial.println(String(speedDino - Rkf) + " / " + String(speedDino + Lkf));
      motorB_setpower(speedDino - Rkf, true);
      motorA_setpower(speedDino + Lkf, true);
    }
  } else {
    motorB_setpower(0, true);
    motorA_setpower(0, true);

    if (millis() - bot_lasttime > BOT_MTBS)
    {
      updateTG();
    }
  }
}

void checkColor() {
  uint16_t red_data   = 0;
  uint16_t green_data = 0;
  uint16_t blue_data  = 0;
  uint16_t clear_data = 0;
  uint16_t prox_data  = 0;
  // Определение цвета
  while (!apds9960.colorDataReady()) {
    delay(5);
  }
  apds9960.getColorData(&red_data, &green_data, &blue_data, &clear_data);
  // Определение близости препятствия
  prox_data = apds9960.readProximity();
  // Вывод измеренных значений в терминал

  sum = red_data + green_data + blue_data;
  Serial.println("RED   = " + String((float)red_data / sum * 100)); //вычисляем процент красного в какой то карточке
  Serial.println("GREEN   = " + String((float)green_data / sum * 100));
  Serial.println("BLUE   = " + String((float)blue_data / sum * 100));
  red_data = (float)red_data / sum * 100;
  green_data = (float)green_data / sum * 100;
  blue_data = (float)blue_data / sum * 100;

  setBusChannel(0x03);
  for (int channel = 0; channel < testModule.channelCount(); channel++)
  {
    testModule.setLedDriverMode(channel, PCA9634_LEDPWM); // установка режима ШИМ (0-255)
  }
  
 /*
  * 1 - yellow
  * 2 - purple
  * 3 - red
  * 4 - green
  * 5 - blue
  */
  if (abs(red_data - 33) <= 12 && abs(green_data - 33) <= 10 && abs(blue_data - 33) <= 10) {
    testModule.write1(3, 0x90);
    testModule.write1(7, 0x90);
    testModule.write1(4, 0x90);
  } else {
    if (red_data > blue_data && green_data > blue_data) {
      Serial.println ("YELLOW");
      testModule.write1(3, 0x90);
      testModule.write1(7, 0x90);
      testModule.write1(4, 0x00);
      color = 1;
    } else {
      if (blue_data < 54 && blue_data > 45 && red_data < 30 && red_data >= 20) {
        Serial.println ("PURPLE");
        testModule.write1(3, 0x90);
        testModule.write1(7, 0x00);
        testModule.write1(4, 0x90);
        color = 1;
      } else {
        if (red_data > green_data && red_data > blue_data) {
          Serial.println ("RED");
          testModule.write1(3, 0x90);
          testModule.write1(7, 0x00);
          testModule.write1(4, 0x00);
          color = 1;
        } else {
          if (green_data > red_data && green_data > blue_data) {
            Serial.println ("GREEN");
            testModule.write1(3, 0x00);
            testModule.write1(7, 0x90);
            testModule.write1(4, 0x00);
            color = 1;
          } else if (blue_data > red_data && blue_data > green_data)
          {
            Serial.println ("BLUE");
            testModule.write1(3, 0x00);
            testModule.write1(7, 0x00);
            testModule.write1(4, 0x90);
            color = 1;
          }
        }
      }
    }
  }
}

void checkStop() {
  byte sum = 0;

  for (int i = 0; i < 17; i++) { //если заехали на Т образный перекрёсток
    if (digitData[i] == 1) sum++;
  }
  if (sum >= 12) {
    rotate = 1;
    stopDina();
    return;
  }
  sum = 0;
  for (int i = 8; i < 17; i++) { //если сенсоры справа нашли дорогу (поворот направо)
    if (digitData[i] == 1) sum++;
  }
  if (sum >= 7) {
    rotate = 2;
    stopDina();
  }
  sum = 0;
  for (int i = 0; i < 8; i++) { //поворот налево
    if (digitData[i] == 1) sum++;
  }
  if (sum >= 7) {
    rotate = 3;
    stopDina();
  }
}

void stopDina() {
  motorA_setpower(-10, true);
  motorB_setpower(-10, true);
  delay(100);
  motorA_setpower(0, true);
  motorB_setpower(0, true);
}

void updateTG() {
  int numNewMessages = bot.getUpdates(bot.last_message_received + 1);

  while (numNewMessages)
  {
    Serial.println("got response");
    handleNewMessages(numNewMessages);
    numNewMessages = bot.getUpdates(bot.last_message_received + 1);
  }
}

void serial() {
  Serial.print(p00);
  Serial.print(" ");
  Serial.print(p01);
  Serial.print(" ");
  Serial.print(p02);
  Serial.print(" ");
  Serial.print(p03);
  Serial.print(" ");
  Serial.print(p04);
  Serial.print(" ");
  Serial.print(p05);
  Serial.print(" ");
  Serial.print(p06);
  Serial.print(" ");
  Serial.print(p07);
  Serial.print(" ");
  Serial.print(p08);
  Serial.print(" ");
  Serial.print(p09);
  Serial.print(" ");
  Serial.print(p10);
  Serial.print(" ");
  Serial.print(p11);
  Serial.print(" ");
  Serial.print(p12);
  Serial.print(" ");
  Serial.print(p13);
  Serial.print(" ");
  Serial.print(p14);
  Serial.print(" ");
  Serial.print(p15);
  Serial.print(" ");
  Serial.print(p16);
  Serial.print(" ");
  Serial.print(p17);
  Serial.print(" ");
  Serial.print(p18);
  Serial.println();
  delay(10);
}

void digitSensor() {
  digitData[0] = p01 > l ? 1 : 0;
  digitData[1] = p02 > l ? 1 : 0;
  digitData[2] = p03 > l ? 1 : 0;
  digitData[3] = p04 > l ? 1 : 0;
  digitData[4] = p05 > l ? 1 : 0;
  digitData[5] = p06 > l ? 1 : 0;
  digitData[6] = p07 > l ? 1 : 0;
  digitData[7] = p08 > l ? 1 : 0;
  digitData[8] = p09 > l ? 1 : 0;
  digitData[9] = p10 > l ? 1 : 0;
  digitData[10] = p11 > l ? 1 : 0;
  digitData[11] = p12 > l ? 1 : 0;
  digitData[12] = p13 > l ? 1 : 0;
  digitData[13] = p14 > l ? 1 : 0;
  digitData[14] = p15 > l ? 1 : 0;
  digitData[15] = p16 > l ? 1 : 0;
  digitData[16] = p17 > l ? 1 : 0;
  for (int i = 0; i < 17; i++) {
    Serial.print(digitData[i]);
  }
  Serial.println();
}

void addKf() {
  maxKf = 0;
  minKf = 0;
  for (int i = 0; i < 17; i++) {
    findKf[i] = digitData[i] * Kf[i];
    maxKf = max(maxKf, findKf[i]);
    minKf = min(minKf, findKf[i]);
  }
}

// Инициализация датчика
void init_sensor() {
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x10);        // Регистр настройки всей микросхемы
  Wire.write(0b00000000);  // Нормальный режим работы
  Wire.write(0b01001111);  // АЦП в непрерывном режиме, 200 ksps, встроенный ИОН для ЦАП
  Wire.endTransmission();
  delay(1000);
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x20);        // Регистр настройки порта 0 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x21);        // Регистр настройки порта 1 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x22);        // Регистр настройки порта 2 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x23);        // Регистр настройки порта 3 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x24);        // Регистр настройки порта 4 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x25);        // Регистр настройки порта 5 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x26);        // Регистр настройки порта 6 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x27);        // Регистр настройки порта 7 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x28);        // Регистр настройки порта 8 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x29);        // Регистр настройки порта 9 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2A);        // Регистр настройки порта 10 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2B);        // Регистр настройки порта 11 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2C);        // Регистр настройки порта 12 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2D);        // Регистр настройки порта 13 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2E);        // Регистр настройки порта 14 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2F);        // Регистр настройки порта 15 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x30);        // Регистр настройки порта 16 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x31);        // Регистр настройки порта 17 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x32);        // Регистр настройки порта 18 (подключен к оптическому сенсору)
  Wire.write(0b00000000);  // Сброс конфигурации порта
  Wire.write(0b00000000);
  Wire.endTransmission();
  delay(1000);
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x20);        // Регистр настройки порта 0 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x21);        // Регистр настройки порта 1 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x22);        // Регистр настройки порта 2 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x23);        // Регистр настройки порта 3 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x24);        // Регистр настройки порта 4 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x25);        // Регистр настройки порта 5 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x26);        // Регистр настройки порта 6 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x27);        // Регистр настройки порта 7 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x28);        // Регистр настройки порта 8 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x29);        // Регистр настройки порта 9 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2A);        // Регистр настройки порта 10 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2B);        // Регистр настройки порта 11 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2C);        // Регистр настройки порта 12 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2D);        // Регистр настройки порта 13 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2E);        // Регистр настройки порта 14 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x2F);        // Регистр настройки порта 15 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x30);        // Регистр настройки порта 16 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x31);        // Регистр настройки порта 17 (подключен к оптическому сенсору)
  Wire.write(0b01110001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x32);        // Регистр настройки порта 18 (подключен к оптическому сенсору)
  Wire.write(0b01111001);  // Диапазон входного напряжения 0 ... 10 В, встроенный ИОН, порт в режиме входа АЦП
  Wire.write(0b11100000);  // Порт не ассоциирован с другим портом, количество выборок АЦП - 128
  Wire.endTransmission();
  delay(1000);
  // Отладка регистров
  /*
    int a = 0;
    int b = 0;
    Wire.beginTransmission(sensor_addr);
    Wire.write(0x45); // Регистр данных АЦП
    Wire.endTransmission();
    Wire.requestFrom(sensor_addr, 2);
    Serial.println(Wire.available());
    a = Wire.read();
    b = Wire.read();
    Serial.println(String(a, 2));
    Serial.println(String(b, 2));
  */
}

// Получение данных с датчика
void poll_sensor() {
  int adc_sensor_data[38] = { 0 };
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x40);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 10);
  if (Wire.available() == 10) {
    adc_sensor_data[0] = Wire.read();  // ADC00
    adc_sensor_data[1] = Wire.read();
    adc_sensor_data[2] = Wire.read();  // ADC01
    adc_sensor_data[3] = Wire.read();
    adc_sensor_data[4] = Wire.read();  // ADC02
    adc_sensor_data[5] = Wire.read();
    adc_sensor_data[6] = Wire.read();  // ADC03
    adc_sensor_data[7] = Wire.read();
    adc_sensor_data[8] = Wire.read();  // ADC04
    adc_sensor_data[9] = Wire.read();
  }
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x45);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 10);
  if (Wire.available() == 10) {
    adc_sensor_data[10] = Wire.read();  // ADC05
    adc_sensor_data[11] = Wire.read();
    adc_sensor_data[12] = Wire.read();  // ADC06
    adc_sensor_data[13] = Wire.read();
    adc_sensor_data[14] = Wire.read();  // ADC07
    adc_sensor_data[15] = Wire.read();
    adc_sensor_data[16] = Wire.read();  // ADC08
    adc_sensor_data[17] = Wire.read();
    adc_sensor_data[18] = Wire.read();  // ADC09
    adc_sensor_data[19] = Wire.read();
  }
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x4A);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 10);
  if (Wire.available() == 10) {
    adc_sensor_data[20] = Wire.read();  // ADC10
    adc_sensor_data[21] = Wire.read();
    adc_sensor_data[22] = Wire.read();  // ADC11
    adc_sensor_data[23] = Wire.read();
    adc_sensor_data[24] = Wire.read();  // ADC12
    adc_sensor_data[25] = Wire.read();
    adc_sensor_data[26] = Wire.read();  // ADC13
    adc_sensor_data[27] = Wire.read();
    adc_sensor_data[28] = Wire.read();  // ADC14
    adc_sensor_data[29] = Wire.read();
  }
  Wire.beginTransmission(sensor_addr);
  Wire.write(0x4F);  // Регистр данных АЦП
  Wire.endTransmission();
  Wire.requestFrom(sensor_addr, 8);
  if (Wire.available() == 8) {
    adc_sensor_data[30] = Wire.read();  // ADC15
    adc_sensor_data[31] = Wire.read();
    adc_sensor_data[32] = Wire.read();  // ADC16
    adc_sensor_data[33] = Wire.read();
    adc_sensor_data[34] = Wire.read();  // ADC17
    adc_sensor_data[35] = Wire.read();
    adc_sensor_data[36] = Wire.read();  // ADC18
    adc_sensor_data[37] = Wire.read();
  }
  p00 = adc_sensor_data[36] * 256 + adc_sensor_data[37];
  p01 = adc_sensor_data[34] * 256 + adc_sensor_data[35];
  p02 = adc_sensor_data[32] * 256 + adc_sensor_data[33];
  p03 = adc_sensor_data[30] * 256 + adc_sensor_data[31];
  p04 = adc_sensor_data[28] * 256 + adc_sensor_data[29];
  p05 = adc_sensor_data[26] * 256 + adc_sensor_data[27];
  p06 = adc_sensor_data[24] * 256 + adc_sensor_data[25];
  p07 = adc_sensor_data[22] * 256 + adc_sensor_data[23];
  p08 = adc_sensor_data[20] * 256 + adc_sensor_data[21];
  p09 = adc_sensor_data[18] * 256 + adc_sensor_data[19];
  p10 = adc_sensor_data[16] * 256 + adc_sensor_data[17];
  p11 = adc_sensor_data[14] * 256 + adc_sensor_data[15];
  p12 = adc_sensor_data[12] * 256 + adc_sensor_data[13];
  p13 = adc_sensor_data[10] * 256 + adc_sensor_data[11];
  p14 = adc_sensor_data[8] * 256 + adc_sensor_data[9];
  p15 = adc_sensor_data[6] * 256 + adc_sensor_data[7];
  p16 = adc_sensor_data[4] * 256 + adc_sensor_data[5];
  p17 = adc_sensor_data[2] * 256 + adc_sensor_data[3];
  p18 = adc_sensor_data[0] * 256 + adc_sensor_data[1];

  massSens[0] = p01;
  massSens[1] = p02;
  massSens[2] = p03;
  massSens[3] = p04;
  massSens[4] = p05;
  massSens[5] = p06;
  massSens[6] = p07;
  massSens[7] = p08;
  massSens[8] = p09;
  massSens[9] = p10;
  massSens[10] = p11;
  massSens[11] = p12;
  massSens[12] = p13;
  massSens[13] = p14;
  massSens[14] = p15;
  massSens[15] = p16;
  massSens[16] = p17;
}

int Targetline() {
  int l = 0;
  for (int i = 0; i < 17; i++) {
    l = max(l, massSens[i]);
  }
  if (l == 0) l = 1475;
  l -= 35;
  return l;
}

// Мощность мотора "A" от -100% до +100% (от знака зависит направление вращения)
void motorA_setpower(float pwr, bool invert) {
  // Проверка, инвертирован ли мотор
  if (!invert) {
    pwr = -pwr;
  }
  // Проверка диапазонов
  if (pwr < -100) {
    pwr = -100;
  }
  if (pwr > 100) {
    pwr = 100;
  }
  int pwmvalue = fabs(pwr) * 40.95;
  if (pwr < 0) {
    pwm.setPWM(10, 0, 4096);
    pwm.setPWM(11, 0, pwmvalue);
  } else {
    pwm.setPWM(11, 0, 4096);
    pwm.setPWM(10, 0, pwmvalue);
  }
}

// Мощность мотора "B" от -100% до +100% (от знака зависит направление вращения)
void motorB_setpower(float pwr, bool invert) {
  // Проверка, инвертирован ли мотор
  if (invert) {
    pwr = -pwr;
  }
  // Проверка диапазонов
  if (pwr < -100) {
    pwr = -100;
  }
  if (pwr > 100) {
    pwr = 100;
  }
  int pwmvalue = fabs(pwr) * 40.95;
  if (pwr < 0) {
    pwm.setPWM(8, 0, 4096);
    pwm.setPWM(9, 0, pwmvalue);
  } else {
    pwm.setPWM(9, 0, 4096);
    pwm.setPWM(8, 0, pwmvalue);
  }
}

bool setBusChannel(uint8_t i2c_channel)  // смена I2C порта
{
  if (i2c_channel >= MAX_CHANNEL) {
    return false;
  } else {
    Wire.beginTransmission(I2C_HUB_ADDR);
    Wire.write(i2c_channel | EN_MASK);
    Wire.endTransmission();
    return true;
  }
}
16

Для чего вы постите тут эти портянки ???

17

Проверяйте, не понятно что ли? :grin:

18

Во-во. Как можно с такой непонятливостью в программисты лезть.

19

не, не пойдёт, сиреневый сменил на фиолетовый, а ведь это пурпурный