Форум arduino.ua

Напишіть краще що хочете зробити, як повинен працювати двігун.

Всім доброго вечора, зіткнувся з проблемою що в программі просто відмовляются працювати крокові двигуни. Використоував бібліотеку GyverStepper.h. Двигун задіяний в функціях  hold, seek, calibration.
Сильно палками не бийте, а так буду дуже вдячний за допомогу

#include <SharpIR.h>
#include <GyverStepper.h>
#include "GyverHacks.h"

SharpIR sensor( SharpIR::GP2Y0A41SK0F, A1 );
GStepper<STEPPER2WIRE> stepper(1000, 5, 6);
#define TIMEOUT 3000
#define max_angd  180
#define min_angd 10
#define deadzone 10
#define min_target 8
#define MISTAKES 4
GTimer sonarTimer(100);
GTimer timeoutTimer(TIMEOUT);
const byte steps_num = (max_angd - min_angd) / 2;
int distance[steps_num + 1];
long val = min_angd ;
byte catch_num;
int maxdist = 50;
byte mistakes;
long catch_pos;
byte mode = true;
boolean next;
boolean direct;
boolean catch_flag, catched_flag, hold_flag;
void setup() {
  Serial.begin(9600);
  stepper.setRunMode(FOLLOW_POS);
  stepper.setMaxSpeed(400);
  stepper.setAcceleration(500);
  calibration();
 
 
}

void loop() {
if (mode) seek();       // режим поиска цели
  else hold();
}

void hold() {
 
  if (!hold_flag)           // движение к середине цели
  if (!stepper.tick()) {
    stepper.setTarget(catch_pos);
    }
    // крутим серво
  else {                    // расчётная точка достигнута
   
    if (sensor.getDistance()) {                   // отдельный таймер сонара
      byte pos = (val - min_angd) / 2;         // перевод градусов в элемент массива
      int curr_dist = sensor.getDistance();;            // получаем сигнал с датчика
      if (curr_dist == 0) curr_dist = maxdist;   // 0 это не только 0, но и максимум
      int diff = distance[pos] - curr_dist;       // ищем разницу
      if ((diff < deadzone) || (curr_dist > 1 && curr_dist < 10)) {   // если вышли из зоны ИЛИ поднесли руку на 1-10 см
        if (timeoutTimer.isReady())               // отработка таймаута
          mode = true;                            // если цель потеряна и вышло время - переходим на поиск
        hold_flag = false;
      } else {                                    // если цель всё ещё в зоне видимости
        timeoutTimer.reset();                     // сбросить таймер
      }
    }
  }
}

void seek() {
 
  if (direct) {             // движемся в прямом направлении
    if (val < max_angd)
    if (!stepper.tick()) {
    stepper.setTarget(max_angd);
    }// плавный поворот
    else {
      direct = false;       // смена направления
      delay(50);            // задержка в крайнем положении
    }
  }
  else {                    // движемся в обратном направлении
    if (val > min_angd)
    if (!stepper.tick()) {
      stepper.setTarget(min_angd);
    }// плавный поворот
    else {
      direct = true;        // смена направления
      delay(50);            // задержка в крайнем положении
    }
  }
  search();                 // ищем цель дальномером
}

void search() {

  if (val % 2 == 0 && next) {                 // каждые 2 градуса
    next = false;
    byte pos = (val - min_angd) / 2;         // перевод градусов в элемент массива
    int curr_dist = sensor.getDistance();;
    if (curr_dist == 0) curr_dist = maxdist;
    int diff = distance[pos] - curr_dist;
    if (diff > deadzone) {                      // разность показаний больше мертвой зоны
      if (!catch_flag) {
        catch_flag = true;        // флаг что поймали какое то значение
        catch_pos = val;        // запомнили угол начала предполагаемой цели
      }
      catch_num++;                // увеличили счётчик значений
      if (catch_num > min_target)  // если поймали подряд много значений
        catched_flag = true;      // считаем, что это цель
    } else {                                    // если "пусто"
      if (catch_flag) {               // если ловим цель
        if (mistakes > MISTAKES) {    // если число ошибок превысило допустимое
          // сбросить всё
          catch_flag = false;
          catched_flag = false;
          catch_num = 0;
          mistakes = 0;
        } else {
          mistakes++;                 // увеличить число ошибок
        }
      }

      if (catched_flag) {             // поймали цель!
        mode = false;                 // переход в режим удержание
        // catch_pos - середина цели. Считаем по разному, если двигались вперёд или назад
        if (direct) catch_pos += catch_num;
        else catch_pos -= catch_num;
       
        // сбросить всё
        hold_flag = false;
        catch_flag = false;
        catched_flag = false;
        catch_num = 0;
        mistakes = 0;       
      }
    }
  }
}

void calibration(){
 
  for (val = min_angd; val <= max_angd; val++) { 
   if (!stepper.tick()) {
    static bool dir;
    dir = !dir;
    stepper.setTarget(dir ? -1000 : 1000);
   }
    if (val % 2 == 0) { 
      byte pos = (val - min_angd) / 2;
      int curr_dist = sensor.getDistance();
      if (curr_dist == 0) distance[pos] = maxdist;
      else distance[pos] = sensor.getDistance();
    }   
  }

}