#include <GyverStepper.h>
//****НАСТРАИВАЕМ РАБОТУ
int dist = 40; //рабочий ход мм
int cycles = 30; //количество циклов в минуту
int center = 45; //расстояние от концевика до центра мм
long stp = 8; //деление шага
int shkiv = 32; //мм за один оборот мотора
long roundstps = stp*200; //чилсо шагов на оборот в полном шаге
long worksteps= (dist*roundstps)/shkiv; //шагов в рабочем ходе в одну сторону
long centersteps=(center*roundstps)/shkiv; //шагов от концевика до центра
long spd; //скорость движение шаг/с
long maxspd =(dist*2*cycles*roundstps)/60/shkiv; //мкс скорость движение шаг/с
long accel =maxspd*2; //ускорение движение шаг/с*с
GStepper< STEPPER2WIRE> stepper1(roundstps, 12, 10, 4);
//*****ФЛАГИ
volatile boolean F_KONC_A;
volatile boolean F_KONC_B;
volatile boolean F_START;
volatile uint32_t debounce;
void Initial_positionA() { //ФУНКЦИЯ ОБРАБОТКИ ПРЕРЫВАНИЯ концевик A
if (millis() - debounce >= 200 && digitalRead(2)) {
debounce = millis();
F_KONC_A = 1;
F_START = 1;
}
}
void Initial_positionB() { //ФУНКЦИЯ ОБРАБОТКИ ПРЕРЫВАНИЯ концевик B
if (millis() - debounce >= 200 && digitalRead(3)) {
debounce = millis();
F_KONC_B = 1;
}
}
//********************************************************************************
void setup() {
F_KONC_A==0;
F_KONC_B==0;
F_START==0;
pinMode(2, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
digitalWrite(2, LOW);
digitalWrite(3, LOW);
attachInterrupt(0, Initial_positionA, RISING); //ОБЪЯВЛЕНИЕ ПРЕРЫВАНИЯ А
attachInterrupt(1, Initial_positionB, RISING); //ОБЪЯВЛЕНИЕ ПРЕРЫВАНИЯ B
stepper1.setRunMode(FOLLOW_POS);
stepper1.setMaxSpeed(maxspd);
stepper1.setAcceleration(accel);
Serial.begin(9600);
Serial.println("Hello World!"); // отправить
Serial.println(worksteps); // дистанция в шагах
Serial.println(maxspd); // скорость
Serial.println(accel); // ускорение
Serial.println(roundstps); //шагов в одном обороте
Serial.println(centersteps); //шагов до центра
}
void loop() {
if (!stepper1.tick() && F_START==0){//Если флаг СТАРТ поднят
stepper1.setMaxSpeed(maxspd/2);
stepper1.setTarget(centersteps*2, RELATIVE);//Едем ДАЛЕКОоо вперёд. Костыль такой.
}
static bool dir = true;
if (!stepper1.tick()) { //Начинаем работать вперёд-назад
stepper1.setMaxSpeed(maxspd);
stepper1.setTarget(dir ? worksteps : -worksteps, RELATIVE);
dir = !dir;
}
if (F_KONC_A ==1){ //если флаг А поднят
stepper1.brake(); //РЕЗКО тормозим, чтобы не сломать концевик
stepper1.setMaxSpeed(maxspd/2);
stepper1.setTarget(-centersteps-(worksteps/3), RELATIVE); //Едем в центр
F_KONC_A = 0; //Флаг А снова готов к труду и обороне
}
if (F_KONC_B ==1){ //если флаг В поднят
stepper1.brake(); //опять же тормозим резко
stepper1.setMaxSpeed(maxspd/2);
stepper1.setTarget(centersteps-(worksteps/2), RELATIVE); //Тут едем в центр но немного ближе, т.к. следующий шаг туда же
F_KONC_B = 0;//Опускаем флаг В
}
}