roboforum.ru

Технический форум по робототехнике.

Управление шаговыми двигателями с помощью прерываний

Программирование микроконтроллеров AVR, PIC, ARM.
Разработка и изготовление печатных плат для модулей.

Управление шаговыми двигателями с помощью прерываний

Сообщение BuharovS » 27 июл 2017, 06:21

Здравствуйте. У меня вопрос: приходилось ли кому управлять шаговиками с помощью прерываний таймеров(нужен именно этот вариант управления)? Возникла проблема в обеспечении плавности ускорения ШД. Есть кривой код. Может кто подскажет, как внедрить формулу: скорость=корень(2*расстояние*ускорение) или другую, или вовсе другой метод есть? Мой код ниже.

//#include <EEPROM.h>
#include <LCD_1602_RUS.h>
LCD_1602_RUS lcd(0x27, 20, 4);

//#define Stop 3 //остановка процесса
#define Step 5 //энкодер
#define Dir 4 //энкодер
#define Start 6 //пуск
//#define koncevik_up 7 //концевик верхний
//#define koncevik_down 8 //концевик нижний
#define Stepper_meln_STP 11 // шаги мельницы
#define Stepper_meln_DIR 12 // направление мельницы
#define Stepper_podyem_STP 10 //шаги подъемника
#define Stepper_podyem_DIR 9 // направление подъемника
//константы
#define value_step_max1 10000 //максимальное число шагов подъемника
#define termin_vibr 20000 // число шагов цикла вибрации
#define termin 100 // амплитуда вибрации
//
unsigned long cntr_2, //счетчик прерываний // таймер 2
val_2, // счетчик импульсов // таймер 2
delta_2=80, // переменная пропуска шагов, изменения скорости
cntr_prev_2, // предыдущее значение прерывания
val_N_2, // количество необходимых шагов
val_T_2,
val_prev_2,
value_step2=0;
int delta2_2=20,delta_T_2,speed_max=4;
unsigned long cntr_1, //счетчик прерываний // таймер 1
val_1, // счетчик импульсов // таймер 1
delta_1=60, // переменная пропуска шагов, изменения скорости
cntr_prev_1, // предыдущее значение прерывания
val_N_1, // количество необходимых шагов
val_T_1,
val_prev_1,
value_step1=0;
int delta2_1=20,delta_T_1;
boolean up=0, down=0, vibr=0, flip2=0;

unsigned long t, t_0, t_current,t_stop, t0;
long // address_sek=0,address_min=1,
min_,
Min_prev, sekund=1000,
t_s=0,
t_s_prev=0,
increment=1,
dt;
byte min_save, sek_save;

byte flag_konec, fl_stop, flag=0, fl_start, fl=0, start_, stop_;
boolean fl_koncevik_up,fl_koncevik_down,fl_koncC,fl_koncO,fl_run=1, friction,
res_time,button_start,button_up,button_down,
encoder_A, encoder_B, encoder_A_prev;

void setup()
{ //Serial.begin(250000); //подключение сериал порта
TCCR2B=(1<<CS00); //
TCCR1B=(1<<CS00); // установка частоты таймеров
lcd.begin(); // инициализация дисплея
lcd.backlight(); // включение подсветки
// pinMode(2, INPUT_PULLUP);

pinMode(Stepper_podyem_DIR, 1); //9
pinMode(Stepper_podyem_STP, 1); //10
pinMode(Stepper_meln_STP, 1); //11
pinMode(Stepper_meln_DIR, 1); //12

pinMode(Start, INPUT_PULLUP); //6
pinMode(Stop, INPUT_PULLUP); //3
pinMode(Step, INPUT_PULLUP); //5 энкодер
pinMode(Dir, INPUT_PULLUP); //4 энкодер

pinMode(koncevik_up, INPUT_PULLUP); //7
pinMode(koncevik_down, INPUT_PULLUP); //8


//if( EEPROM.read(address_sek) >0 ){ t_s=EEPROM.read(address_sek);}
//if( EEPROM.read(address_min) >0 ){ min_=EEPROM.read(address_min);}
lcd.setCursor(0,0); lcd.print(L"Уставка: ");//
lcd.setCursor(0,1); lcd.print(L"Мин: ");
if (min_<=9){lcd.setCursor(5,1); lcd.print(L" ");lcd.setCursor(6,1);lcd.print(min_);} else {lcd.setCursor(5,1); lcd.print(min_);}
lcd.setCursor(8,1); lcd.print(L":");
if (t_s<=9){lcd.setCursor(10,1); lcd.print(L"0");lcd.setCursor(11,1);lcd.print(t_s);} else {lcd.setCursor(10,1); lcd.print(t_s);}
lcd.setCursor(12,1); lcd.print(L" ");


}

void loop()
{
t0=millis();

//stop_=digitalRead(Stop);
//1 установка времени
// 1.1 обработка энкодера
if(flag==0 )
{

start_= digitalRead(Start);
if(fl==0){lcd.setCursor(0,0); lcd.print(L"Уставка: "); fl=2;}
if(fl==1){TIMSK1 &= ~(1 << TOIE1);lcd.setCursor(0,0); lcd.print(L"ГОТОВО "); fl=2;}
if(fl==2){if(flag_konec==1){flag_konec=0;min_=min_save; t_s=sek_save;}
if (t_s<=9){lcd.setCursor(10,1); lcd.print(L"0");lcd.setCursor(11,1);lcd.print(t_s);} else {lcd.setCursor(10,1); lcd.print(t_s);lcd.setCursor(12,1); lcd.print(L" ");}
if (min_<=9){lcd.setCursor(5,1); lcd.print(L" ");lcd.setCursor(6,1);lcd.print(min_);} else {lcd.setCursor(5,1); lcd.print(min_);}
lcd.setCursor(8,1); lcd.print(L":"); lcd.setCursor(0,1); lcd.print(L"Мин: "); fl=8;}

encoder_A = digitalRead(Step);
encoder_B = digitalRead(Dir);
//*************изменение состояний энкодера*********************************************************************
if(!encoder_A && encoder_A_prev)
//*************движение по часовой*************************
{ if(encoder_B)
{ dt=t0-t_current; //выборка инкремента
if (dt>100) increment=1; else if(dt< 20) increment=20; else if(dt< 30) increment=15; else if(dt< 40) increment=13; else if(dt< 50) increment=11; else if(dt< 30) increment=10; else if(dt< 70) increment=7; else if(dt< 80) increment=5; else if(dt< 90) increment=3; else if(dt<= 100) increment=2;
if(min_<30)
{t_s_prev=t_s; t_s+=increment; Min_prev=min_;}
if(t_s>59) {t_s=0; min_++; if(min_ == 30&&t_s>0) { min_ = 30; t_s=0;}}//Обнулить значение счетчика

t_current=t0;
}
//****************движение против часовой **********************************
else
{
dt=t0-t_current;
t_s_prev=t_s;
Min_prev=min_;
if (dt>100) increment=1; else if(dt< 20) increment=20; else if(dt< 30) increment=15; else if(dt< 40) increment=13; else if(dt< 50) increment=11; else if(dt< 30) increment=10; else if(dt< 70) increment=7; else if(dt< 80) increment=5; else if(dt< 90) increment=3; else if(dt<= 100) increment=2;
if(t_s > 0)
{
t_s-=increment;
}
else { min_--; t_s=59; }
if(min_<0){min_=0; t_s=0;}
t_current=t0;
}

//*****************обновление цифр времени************************************************
//if(!res_time){min_=0; t_s=0;}
if(t_s_prev>t_s || t_s_prev<t_s)
{ if (t_s<=0){lcd.setCursor(10,1);lcd.print(L"0");lcd.setCursor(11,1);lcd.print(L"0");}
else if (t_s<=9){lcd.setCursor(10,1); lcd.print(L"0");lcd.setCursor(11,1);lcd.print(t_s);} else {lcd.setCursor(10,1); lcd.print(t_s);}
lcd.setCursor(12,1);lcd.print(L" ");
}
if(Min_prev>min_||Min_prev<min_)
{ if (min_<=9){lcd.setCursor(5,1); lcd.print(L" ");lcd.setCursor(6,1);lcd.print(min_);} else {lcd.setCursor(5,1); lcd.print(min_);}}
//****************************************************************************************
} // конец цикла установки времени, обработки энкодера

/*if(!stop_) // сброс времени
{ t_stop=t0;
if (!digitalRead(Stop)&&t_stop%1000==0) //если спустя 1с нажата кнопка Стоп, сбросить и обновить таймер
{min_=0; t_s=0; min_save=0; sek_save=0; fl=0;
lcd.setCursor(5,1); lcd.print(L" "); lcd.setCursor(6,1); lcd.print("0");lcd.setCursor(10,1); lcd.print(L"0");lcd.setCursor(11,1); lcd.print("0");lcd.setCursor(12,1); lcd.print(L" ");}
} //конец обработчика сброса времени*/

if (!start_&&fl_start==0&&(t_s>0 || min_>0 )) //если установили время и нажали Старт
{ fl=4;fl_start=1;}
if(fl==4)
{ min_save=min_; sek_save=min_*60;
val_N_2 = (sek_save+t_s)*5500; //
if(val_N_2<=100) {delta_T_2=10; val_prev_2=2;}//
// else if(val_N_2>100&&val_N_2<=6000) {delta_T_2=val_N_2/1.7; val_prev_2=map(val_N_2, 101,6000,20,80);}//
else if(val_N_2>6000){delta_T_2=3500; val_prev_2=100; }//
val_T_2=val_N_2-delta_T_2;// пересчет момента замедления
sek_save=t_s;lcd.setCursor(11,0); lcd.print(val_N_2);
//lcd.setCursor(0,0); lcd.print(L"Oсталосb:");lcd.setCursor(0,1); lcd.print(L"Мин: ");
//lcd.setCursor(0,1); lcd.print(L"Мин: ");
fl=3;
down=1;
}

if(fl==3)
{
val_N_1= 7000;
if(val_N_1<=100) {delta_T_1=10; val_prev_1=2;}
else if(val_N_1>100&&val_N_1<=6000) {delta_T_1=val_N_1/1.7; val_prev_1=map(val_N_1, 101,6000,20,80);}
else if(val_N_1>6000){delta_T_1=4000; val_prev_1=90; }
val_T_1=val_N_1-delta_T_1;
cntr_prev_1=0; // подготовка к опусканию, установка шагoв спуска
if(down){digitalWrite(9,1);} // pin 9 установка направления двигателя на опускание
if(up){digitalWrite(9,0);} // pin 9 установка направления двигателя на подъем
flag=8; //флаг цикла сброшен
fl=8; //флаг цикла сброшен
TIMSK1 |= (1 << TOIE1);
}
encoder_A_prev = encoder_A;
}
// 1.2 пересчет секунд в импульсы
// 1.3 обновление дисплея
//2 старт цикла
// 2.1 проверка условий( установлено ли время)
// 2.2 опускание
// 2.3 истирание
if(flag==1)
{
if(millis()%1000==0 )
{ t_s--;if (t_s<=9){lcd.setCursor(10,1); lcd.print(L"0");lcd.setCursor(11,1);lcd.print(t_s);} else {lcd.setCursor(10,1); lcd.print(t_s);}
if(t_s<0){ t_s=59; min_--;if (min_<=9){lcd.setCursor(5,1); lcd.print(L" ");lcd.setCursor(6,1);lcd.print(min_);} else {lcd.setCursor(5,1); lcd.print(min_);}}
if(min_<=0&&t_s<=0) { fl_start=0; t0=0; min_=0; t_s=0; flag=0; fl=1; }
} flag_konec=0;
if(!stop_){flag=0; fl_stop=0;fl_start=0;t_stop=t0;TIMSK2 &= ~(1 << TOIE2);
}

}
// digitalWrite(Stepper_podyem_DIR,flip1);
digitalWrite(Stepper_meln_DIR,flip2);
// 2.4 подъем
// 2.5 сброс лишнего(вибрация)
}

ISR(TIMER1_OVF_vect) // 1 двигатель подъемника пин 10 step, 9 dir
{
cli(); //запрет прерываний
cntr_1++; //инкремент счетчика прерываний
if(cntr_1-cntr_prev_1==delta_1)
{if(val_1<val_T_1&&val_1%delta2_1==0)
{delta_1--;if(delta_1<=4)delta_1=4; if(delta_1<=15){delta2_1=40;}}
else if(val_1>=val_T_1&&val_1%delta2_1==0)
{delta_1++;if(delta_1<=35){delta2_1=val_prev_1;} if(delta_1>=90)delta_1=90;}
cntr_prev_1=cntr_1;digitalWrite(10,1);val_1++;
} else digitalWrite(10,0);

if(val_1>=val_N_1) // если закончило движение
{
if(down){friction=1;down=0; flag=1;TIMSK1 &= ~(1 << TOIE1);} // если был спуск, включить таймер 2 и двигатель мельницы, установлен флаг отсчета
if(up) {vibr=1;friction=0;up=0;down=0; TIMSK1 &= ~(1 << TOIE1);} // если был подъем, включить режим вибрации
val_2=0;cntr_prev_2=0;
val_1=0;cntr_prev_1=0;
val_N_1=0; cntr_1=0; fl_start=0;
cntr_prev_2=0;
TIMSK2 |= (1 << TOIE2);} //включение таймера 2
sei(); // разрешение прерываний
}

ISR(TIMER2_OVF_vect) // 2 двигатель истирания пин 11 step, 12 dir
{
cli();
cntr_2++; //инкремент счетчика прерываний

if(friction)
{
if(cntr_2-cntr_prev_2==delta_2) //скорость (интервал между шагами)
{if(val_2<val_T_2&&val_2%delta2_2==0) // разгон ?
{delta_2--; if(delta_2<=speed_max)delta_2=speed_max; if(delta_2<=15){delta2_2++;}} // типа установка интервала изменения скорости
else if(val_2>=val_T_2&&val_2%delta2_2==0) // торможение ?
{delta_2++; if(delta_2<=55){delta2_2--;} if(delta_2<=1)delta_2=1;} // типа установка интервала изменения скорости

cntr_prev_2=cntr_2;digitalWrite(11,1);val_2++;
} else digitalWrite(11,0);
if(val_2>=val_N_2)
{ val_2=0;cntr_prev_2=0;vibr=1; val_N_2=0; friction=0; cntr_2=0; TIMSK2 &= ~(1 << TOIE2);
up=1; flag=0;
fl=3; //переход в обработку опускания/подъема
}
}


if(vibr)
{
if(cntr_2%2==0)
{val_2++; digitalWrite(11,1);}
else digitalWrite(11,0);

if(val_2>=termin) { flip2=!flip2;val_2=0;}
if(cntr_2>=termin_vibr) {TIMSK2 &= ~(1 << TOIE2);cntr_2=0;min_=0; t_s=0; vibr=0; flag=0; fl=1;flag_konec=1;}
}
sei();
}
BuharovS
 
Сообщения: 2
Зарегистрирован: 26 июл 2017, 13:44

Re: Управление шаговыми двигателями с помощью прерываний

Сообщение setar » 27 июл 2017, 13:27

https://github.com/MarlinFirmware/Marli ... lanner.cpp
ищи calculate_trapezoid_for_block

ну и в целом посмотри на метод управления.
тут и планировщик с буфером входящих команд, и ускорения и ограничения по максимальной и минимальной скорости и учет jerk (рывок для моментального изменения скорости)
Аватара пользователя
setar
Site Admin
 
Сообщения: 10984
Зарегистрирован: 04 окт 2004, 12:58
Откуда: St.Petersburg
Skype: taranenko.sergey
ФИО: Сергей Тараненко

Re: Управление шаговыми двигателями с помощью прерываний

Сообщение BuharovS » 27 июл 2017, 13:47

Благодарю, буду изучать
BuharovS
 
Сообщения: 2
Зарегистрирован: 26 июл 2017, 13:44

Re: Управление шаговыми двигателями с помощью прерываний

Сообщение Dmitry__ » 27 июл 2017, 14:25

Можно еще посмотреть на менее монстроидальный "grbl"
Аватара пользователя
Dmitry__
 
Сообщения: 8033
Зарегистрирован: 13 янв 2011, 15:25
Откуда: Санкт-Петербург


Вернуться в Микроконтроллеры

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 18