2001serial joy4jostik: различия между версиями

Текущая версия от 14:33, 15 ноября 2019

 //////////////////////////////////////////////
 //     2018.08.28 woronin,  umkiedu@gmail.com
 //     Robot UMKI controller  K6_2
 //     To connect using 4joyjostik mobile app by link http://arduino-robot.site/basic/serial
 //     - for ANDROID 4.0.1 or later version;
 //////////////////////////////////////////////
 #include <SoftwareSerial.h>
 #define diod_led A1 // Аналоговый порт для диодика
 SoftwareSerial BTSerial(6, 7); // RX, TX
 int byte_forward[] = {0, 0,   129, 0, 4, 0, 0};
 int byte_bakward[] = {0, 0,   127, 0, 4, 0, 0};
 int byte_left[]    = {0, 129, 0,   0, 4, 0, 0};
 int byte_right[]   = {0, 127, 0,   0, 4, 0, 0};
 int byte_stop[]    = {0, 0,   0,   0, 4, 0, 0};
 int byte_start[]   = {0, 0,   0,   0, 4, 0, 2};
 int byte_a[]       = {0, 0,   0,   0, 4, 1, 0}; // A
 int byte_b[]       = {0, 0,   0,   0, 4, 2, 0}; // B
 int byte_c[]       = {0, 0,   0,   0, 4, 4, 0}; // C
 int byte_x[]       = {0, 0,   0,   0, 4, 8, 0}; // X
 int byte_y[]       = {0, 0,   0,   0, 4, 16, 0};// Y
 int byte_z[]       = {0, 0,   0,   0, 4, 32, 0};// Z
 int ml_speed = 3; // скорость моторов
 int mr_speed = 5; // скорость моторов только для К6
 int motor_r1 = 2; // направление правый
 int motor_l1 = 4; // направление левый
 int speaker = 6; // ножка спикера
 int program_move[255], program_time[255], program_speed[255]; //инициализация трех массивов, направления, времени, 
 скорости
 int side, pwm = 255, press_time, time1, time2, press_but = 0, press_last = 0, flag_last_but = 0, flag_start_program = 0;
 int flag_source = 0; // флаг источника данных команд 0- ничего , 1 - блютус, 2 - снап
 int timeX = 150; // время в милисек для кнопки X
 int timeY = 300; // время в милисек для кнопки Y
 int n = 0; //  номер команды движения из 128
 int count_snap_cikl = 0 ; // счетчик циклов в коде от снапа,задается в подпрограмме снапа

void setup() {
  // инициализируем те порты,
  BTSerial.begin(9600);
  Serial.begin(19200);
  pinMode(diod_led, OUTPUT); // назначаем пин для диода с резистором на плате
  Serial.println("88888"); //  отладка поиска старта программы
}

void loop() // выполняется циклически записываем в порт по шнурку все полученны данные с блютуса
{
  int inByte[128], i, count; //i - это элемент массива команды из 7 байт
  static int count_snap = 0; //  длина в байтах команды от снапа
  static int flag_snap = 0; // только для снапа, когда 0 - ожидает конца пакета, 1 - получен последний байт пакета
  // todo  сделать защиту от переполнения массива inByte[]
  // todo  сдедать таймаут для ожидания 0x8f
  // читаем из блютус порта 7 байт
  count = BTSerial.available();
  if (count < 1) return;
  switch (flag_source) {
    case 0: //  определяем по первому байту откуда пришли байты в порт - либо с моб, либо от снап
      inByte[0] = BTSerial.read(); //  прочитали первый байт
      if (inByte[0] == 0) flag_source = 1; // идут команды с мобилки пульта блютус
      else if (inByte[0] == 0x8e) {
        flag_source = 2; //  идет программа снапа
        count_snap = 1; // первый байт мы уже прочитали
        flag_snap = 0; // признак принятия пакета
      }
      Serial.println(); // перевод строки в мониторе когда  пришел первый байт в слове
      Serial.print(inByte[0], HEX); // вывод в COM порт побайтоно в шестнадцатиричной системе
      Serial.print(" "); // ставим пробел между байтаами, чтобы удобно было смотреть монитор порта
      return;
      break;
    case 1: //  здесь идут 7 байт с мобильника. Один уже прочтен ранее
      //            Serial.print("count = ");
      //            Serial.println(count);
      if (count > 11) return; // Защита от перезаписывания в ардуино при режиме гироскопа
      if (count < 6) return; //  ни чего не обрабатываем до последнего байта слова
      for (i = 0; i < count; i++) {
        inByte[i + 1] = BTSerial.read();
        delay(10);
        Serial.print(inByte[i + 1], HEX); // вывод в COM порт побайтоно в шестнадцатиричной системе
        Serial.print(" "); // ставим пробел между байтаами, чтобы удобно было смотреть монитор порта
      }
      break;
    case 2: //  здесь мы читаем байты от програмы SNAP
      for (i = 0; i < count; i++) {
        inByte[count_snap] = BTSerial.read();
        if (inByte[count_snap] == 0x8f) {
          count_snap++;
          flag_snap = 1;
          break;
        }
        delay(10);
        count_snap++; // увеличиывем счетчик для массива исполняемой программы из Снапа
      }
      if (flag_snap == 0) return;
      break;
  }
  // начало разбор принятых байт
  if (flag_source == 1) { //  функция  обработки по пультику
    if ((inByte[1] == byte_forward[1]) && (inByte[2] == byte_forward[2]) && (inByte[5] == byte_forward[5]) && (inByte[6] == 
 byte_forward[6])) {
      press_but = 48; // нажата кнопка вперед
    }
    else if ((inByte[1] == byte_left[1]) && (inByte[2] == byte_left[2])  && (inByte[5] == byte_left[5]) && (inByte[6] == 
 byte_left[6])) {
      press_but = 52; // нажата кнопка влево
    }
    else if ((inByte[1] == byte_right[1]) && (inByte[2] == byte_right[2])  && (inByte[5] == byte_right[5]) && (inByte[6] == 
 byte_right[6])) {
      press_but = 56; // нажата кнопка вправо
    }
    else if ((inByte[1] == byte_bakward[1]) && (inByte[2] == byte_bakward[2])  && (inByte[5] == byte_bakward[5]) && 
 (inByte[6] == byte_bakward[6])) {
      press_but = 60; // нажата кнопка назад
    }
    else if ((inByte[1] == byte_stop[1]) && (inByte[2] == byte_stop[2]) && (inByte[5] == byte_stop[5]) && (inByte[6] == 
 byte_stop[6])) {
      press_but = 65; // нажата кнопка стоп
    }
    else if ((inByte[1] == byte_start[1]) && (inByte[2] == byte_start[2]) && (inByte[5] == byte_start[5]) && (inByte[6] == 
 byte_start[6])) {
      press_but = 66; // нажата кнопка старт
    }
    // Разбор нажатия дополнительных кнопок
    else if ((inByte[1] == byte_a[1]) && (inByte[2] == byte_a[2]) && (inByte[4] == byte_a[4]) && (inByte[5] == byte_a[5])) 
 {
      press_but = 71; // нажата кнопка A
    }
    else  if ((inByte[1] == byte_b[1]) && (inByte[2] == byte_b[2]) && (inByte[4] == byte_b[4]) && (inByte[5] == byte_b[5])) 
 {
      press_but = 72; // нажата кнопка B
    }
    else  if ((inByte[1] == byte_c[1]) && (inByte[2] == byte_c[2]) && (inByte[4] == byte_c[4]) && (inByte[5] == byte_c[5])) 
 {
      press_but = 74; // нажата кнопка C
    }
    else if ((inByte[1] == byte_x[1]) && (inByte[2] == byte_x[2]) && (inByte[4] == byte_x[4]) && (inByte[5] == byte_x[5])) 
 {
      press_but = 78; // нажата кнопка X
    }
    else if ((inByte[1] == byte_y[1]) && (inByte[2] == byte_y[2]) && (inByte[4] == byte_y[4]) && (inByte[5] == byte_y[5])) 
 {
      press_but = 80; // нажата кнопка Y
    }
    else if ((inByte[1] == byte_z[1]) && (inByte[2] == byte_z[2]) && (inByte[4] == byte_z[4]) && (inByte[5] == byte_z[5])) 
 {
      press_but = 90; // нажата кнопка Z
    }
    // конец разбор принятых байт
    if (n >= 25) n = 0; // массив из 25 шагов команды, защита от переполнения
    // обработка нажатия кнопки
    if ( press_but > 67) { // если нажата кнопка A B C X Y Z то
      flag_source = 0; //  опускаем флаг источника данных
      press_last = press_but; // запоминаем последнюю нажатую кнопку
      return; // уходим на следующий цикл
    }
    // Разбор кнопок движения +++++++++++++++++++++++++++++++++++
    // Движение вперед
    if ( press_but == 48) {
      n++; // для программы увеличиваем на единицу индекс массива
      time1 = millis();  // Запоминаем время нажатия кнопки
      flag_last_but = 1; // поднимаем флаг нажатой кнопки   для подсчета времени
      if (flag_start_program == 1)  {
        memset (program_move, 0, sizeof(int) * 25); //для первой команды  после программы обнуляем массив
        n = 1; // ставим индексы всех массивов в 0
        flag_start_program = 0; // опускаем флаг старта программы
      }
      if (press_last == 78) {
        go_forward_x(pwm);
        program_move[n] = 0x0; //запоминаем в программе шаг вперед на времяX
      }
      else if (press_last == 80) {
        go_forward_y(pwm);
        program_move[n] = 0x2; //запоминаем в программе шаг впере на время Y
      }
      else
      {
        go_forward_z(pwm);
        program_move[n] = 0x1; //запоминаем в программе шаг вперед на время Z  по стопу
      }
    }
    // Движение влево ++++++++++++++++++++++++++++
    if ( press_but == 52) {
      n++; // для программы увеличиваем на единицу индекс массива
      time1 = millis();  // Запоминаем время нажатия кнопки
      flag_last_but = 1; // поднимаем флаг нажатой кнопки   для подсчета времени
      if (flag_start_program == 1)  {
        memset (program_move, 0, sizeof(int) * 25); //для первой команды  после программы обнуляем массив
        n = 1; // ставим индексы всех массивов в 0
        flag_start_program = 0; // опускаем флаг старта программы
      }
      if (press_last == 78) {
        go_left_x(pwm);
        program_move[n] = 0x4; //запоминаем в программе шаг влево на времяX
      }
      else if (press_last == 80) {
        go_left_y(pwm);
        program_move[n] = 0x6; //запоминаем в программе шаг влево на время Y
      }
      else
      {
        go_left_z(pwm);
        program_move[n] = 0x5; //запоминаем в программе шаг влево на время Z  по стопу
      }
    }
    // Движение вправо ++++++++++++++++++++++++++++++
    if ( press_but == 56) {
      n++; // для программы увеличиваем на единицу индекс массива
      time1 = millis();  // Запоминаем время нажатия кнопки
      flag_last_but = 1; // поднимаем флаг нажатой кнопки   для подсчета времени
      if (flag_start_program == 1)  {
        memset (program_move, 0, sizeof(int) * 25); //для первой команды  после программы обнуляем массив
        n = 1; // ставим индексы всех массивов в 0
        flag_start_program = 0; // опускаем флаг старта программы
      }
      if (press_last == 78) {
        go_right_x(pwm);
        program_move[n] = 0x8; //запоминаем в программе шаг вправо на времяX
      }
      else if (press_last == 80) {
        go_right_y(pwm);
        program_move[n] = 0xA; //запоминаем в программе шаг вправо на время Y
      }
      else
      {
        go_right_z(pwm);
        program_move[n] = 0x9; //запоминаем в программе шаг вправо на время Z  по стопу
      }
    }
    // Движение назад +++++++++++++++++++++++++++++++++++++
    if ( press_but == 60) {
      n++; // для программы увеличиваем на единицу индекс массива
      time1 = millis();  // Запоминаем время нажатия кнопки
      flag_last_but = 1; // поднимаем флаг нажатой кнопки   для подсчета времени
      if (flag_start_program == 1)  {
        memset (program_move, 0, sizeof(int) * 25); //для первой команды  после программы обнуляем массив
        n = 1; // ставим индексы всех массивов в 0
        flag_start_program = 0; // опускаем флаг старта программы
      }
      if (press_last == 78) {
        go_bakward_x(pwm);
        program_move[n] = 0xC; //запоминаем в программе шаг назад на времяX
      }
      else if (press_last == 80) {
        go_bakward_y(pwm);
        program_move[n] = 0xE; //запоминаем в программе шаг назад на время Y
      }
      else
      {
        go_bakward_z(pwm);
        program_move[n] = 0xD; //запоминаем в программе шаг назад на время Z  по стопу
      }
    }
    // Движение стоп ++++++++++++++++++++++++++++++
    if ( press_but == 65) {
      time2 = millis();  // Запоминаем время нажатия кнопки
      if (n > 0) {
        if (flag_last_but == 1)
          program_time[n] = time2 - time1; //записываем по индексу в массив время движения
        flag_last_but = 0; // опускаем флаг нажатой кнопки
      }
      go_stop(pwm);
    }
    // Движение по кнопке программа +++++++++++++++++++
    if ( press_but == 66) {
      if (press_last == 71)go_program_a (pwm, n); // запускаем езду по командам программе в прямой последовательности
      if (press_last == 72)go_program_b (pwm, n); // запускаем езду по командам программе в обратной последовательности
      flag_start_program = 1; // поднимаем флаг старта программы
    }
    flag_source = 0; //  опускаем флаг источника данных
  } //  endif (flag_source == 1)
  if (flag_source == 2) { // обрабатываем массив принятых данных от СНАПА ++++++++++++++++
    for (i = 0; i < count_snap - 2; i++) {
      if (inByte[i] == 0x03 )  count_snap_cikl++; // подсчитываем количество повторов цикла в коде
      if (count_snap_cikl == 0) count_snap_cikl = 1; //Задаем один проход если небыло циклов в программе снапа
    }
    for (i = 1; i <= count_snap - 2; i++) {
      program_move[i - 1] = inByte[i]; // переносим принятый массив байт в массив для исполнения
    }
    for (i = 1; i < count_snap_cikl; i++) { //  исполняем программу в цикле с учетом повторов
      go_program_a (pwm, count_snap - 2); // запускаем езду по командам программе в прямой последовательности
    }
    flag_source = 0; //  опускаем флаг источника данных
  }//  endif (flag_source == 2)
}//все, конец рабочего цикла

////////// подпрограммы ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

 void go_forward_x(int pwm) // вперед поехали после нажатой X
{
  digitalWrite(motor_r1, LOW); // вперед правый
  digitalWrite(motor_l1, LOW); // вперед левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeX);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_forward_y(int pwm)  // вперед поехали после нажатой Y
{
  digitalWrite(motor_r1, LOW); // вперед правый
  digitalWrite(motor_l1, LOW); // вперед левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeY);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_forward_z(int pwm)  // вперед поехали после нажатой Z
{
  digitalWrite(motor_r1, LOW); // вперед правый
  digitalWrite(motor_l1, LOW); // вперед левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
}

void go_back(int pwm)  // назад поехали
{
  digitalWrite(motor_r1, HIGH); // назад правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
}

void go_bakward_x(int pwm) // назад поехали после нажатой X
{
  digitalWrite(motor_r1, HIGH); // назад правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeX);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_bakward_y(int pwm)  // назад поехали после нажатой Y
{
  digitalWrite(motor_r1, HIGH); // назад правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeY);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_bakward_z(int pwm)  // назад поехали после нажатой Z
{
  digitalWrite(motor_r1, HIGH); // назад правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
}

void go_left_x(int pwm)  // влево поехали после нажатой X
{
  digitalWrite(motor_r1, LOW); // вперед правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeX);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_left_y(int pwm)  // влево поехали после нажатой Y
{
  digitalWrite(motor_r1, LOW); // вперед правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeY);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_left_z(int pwm)  // влево поехали после нажатой Z
{
  digitalWrite(motor_r1, LOW); // вперед правый
  digitalWrite(motor_l1, HIGH); // назад левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
}

void go_right_x(int pwm) // вправо поехали после нажатой X
{
  digitalWrite(motor_r1, HIGH ); // назад правый
  digitalWrite(motor_l1, LOW); // вперед левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeX);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_right_y(int pwm) // вправо поехали после нажатой Y
{
  digitalWrite(motor_r1, HIGH ); // назад правый
  digitalWrite(motor_l1, LOW); // вперед левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
  delay(timeY);
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

void go_right_z(int pwm) // вправо поехали после нажатой Z
{
  digitalWrite(motor_r1, HIGH ); // назад правый
  digitalWrite(motor_l1, LOW); // вперед левый
  analogWrite(ml_speed, pwm); // скорость
  analogWrite(mr_speed, pwm);
}

void go_speaker(int pwm) // Писк спикера
{
  analogWrite(speaker, pwm - 1); // вкл спикер не знаю почему но так включается
  delay (500);
  analogWrite(speaker, pwm); // выкл спикер
}

void go_blink(int pwm) // моргание диодика
{
  digitalWrite(diod_led, HIGH); // вкл диодик
  delay (1500);
  digitalWrite(diod_led, LOW); // выкл диодик
}

void go_stop(int pwm) // стоп
{
  analogWrite(ml_speed, 0); // скорость стоп
  analogWrite(mr_speed, 0);
}

 /////////////////// Подпрограммы езды по ранее введенным командам +++++++++++++++++++++++++++++++++++++++++++++++++++++++
void go_program_a(int pwm, int n_con) // Подпрограмма езды по заданым шагам программы после А
{
  int n;
  for (n = 0; n < n_con; n++) {
    Serial.print(n);
    Serial.print(" np= ");
    Serial.print(program_move[n], HEX);
    Serial.print(" tm= ");
    Serial.println(program_time[n]);
    if     (program_move[n] == 0x0)  { // вперед при нажатой X; T min
      go_forward_x(pwm);
    }
    else if (program_move[n] == 0x2) { // вперед при нажатой Y; T ave
      go_forward_y(pwm);
    }
    else if (program_move[n] == 0x1) { // вперед при нажатой Z; T max
      go_forward_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0x4) {// влево при нажатой X; T min
      go_left_x(pwm);
    }
    else if (program_move[n] == 0x6) {// влево при нажатой Y; T ave
      go_left_y(pwm);
    }
    else if (program_move[n] == 0x5) {// влево при нажатой Z; T max
      go_left_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0x7) {// звук спикера
      go_speaker(pwm);
    }
    else if (program_move[n] == 0x8) {// вправо при нажатой X; T min
      go_right_x(pwm);
    }
    else if (program_move[n] == 0xA) {// вправо при нажатой Y; T ave
      go_right_y(pwm);
    }
    else if (program_move[n] == 0x9) {// вправо при нажатой Z; T max
      go_right_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0xC) {// назад при нажатой X; T min
      go_bakward_x(pwm);
    }
    else if (program_move[n] == 0xE) {// назад при нажатой Y; T ave
      go_bakward_y(pwm);
    }
    else if (program_move[n] == 0xD) {// назад при нажатой Z; T max
      go_bakward_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0xF) {// Моргнуть диодиком
      go_blink(pwm);
    }
    delay(500);
  }
}

void go_program_b(int pwm, int n_con) // Подпрограмма езды по заданым шагам программы после B
{
  int n;
  for (n = n_con; n > 0; n--) {
    Serial.print(n);
    Serial.print(" np= ");
    Serial.print(program_move[n]);
    Serial.print(" tm= ");
    Serial.println(program_time[n]);
    if     (program_move[n] == 0x0)  { // вперед при нажатой X; T min
      go_forward_x(pwm);
    }
    else if (program_move[n] == 0x2) { // вперед при нажатой Y; T ave
      go_forward_y(pwm);
    }
    else if (program_move[n] == 0x1) { // вперед при нажатой Z; T max
      go_forward_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0x4) {// влево при нажатой X; T min
      go_left_x(pwm);
    }
    else if (program_move[n] == 0x6) {// влево при нажатой Y; T ave
      go_left_y(pwm);
    }
    else if (program_move[n] == 0x5) {// влево при нажатой Z; T max
      go_left_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0x8) {// вправо при нажатой X; T min
      go_right_x(pwm);
    }
    else if (program_move[n] == 0xA) {// вправо при нажатой Y; T ave
      go_right_y(pwm);
    }
    else if (program_move[n] == 0x9) {// вправо при нажатой Z; T max
      go_right_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    else if (program_move[n] == 0xC) {// назад при нажатой X; T min
      go_bakward_x(pwm);
    }
    else if (program_move[n] == 0xE) {// назад при нажатой Y; T ave
      go_bakward_y(pwm);
    }
    else if (program_move[n] == 0xD) {// назад при нажатой Z; T max
      go_bakward_z(pwm);
      delay(program_time[n]);
      go_stop(pwm);
    }
    delay(500);
  }
}
2001serial joy4jostik: различия между версиями

Текущая версия от 14:33, 15 ноября 2019

Навигация

Поиск
