- Код: Выделить всё • Развернуть
/*Здравствуйте!
Хотел немного рассказать о генерации импульсов для ШД.
Есть задача произвести движение на заданное расстояние.
Нам понадобится структура WLDrive
*/
struct WLDrive
{
float t_st; //время торможения, сек
float t_fi; //время остановки, сек
float v_st; //начальная скорость, ед/сек
float v_ma; //скорость перемещения, ед/сек
float v_now; //текущая скорость
float v_decel_ch; //скорость с которой начали тормозить
float dim; //единица одного шага (дискрета перемещения)
long step_mov; //счётчик сделанных шагов
long step_now; //текущее положение
//флаги
bool readyF; //готов к перемещению
bool busyF; //занят
bool startF; //пуск
bool accelF; //разгон
bool decelF; //торможение
bool breakF;//остановка по требованию
bool outStep;
bool outDir;
int T1ch; //период
int T1; //счётчик периода
long tickMov; //счётчик единиц времени
WLDrive()
{
t_st=t_fi=0.1; //устанавливаем начальные значения, время разгона и торможения = 0.1с
v_st=10;
v_fi=100;
readyF=busyF=startF=false;
accelF=decelF=breakF=false;
freq=1000;
dim=1;
step_mov=step_now;
}
void reset()
{
readyF=busyF=startF=false;
accelF=decelF=breakF=false;
}
void break() //остановка
{
breakF=true;
}
bool setMovInc(float distance); //задаём перемещение на заданное расстояние
{
if(busyF
||startF) return false;
readyMov=true;
step_mov=distance/dimm
if(step_mov!=0)
return true;
else
return false;
}
bool setMovAbs(float position); //задаём перемещение в заданную позицию
{
return setMovInc(step_now*dimm+position);
}
bool start() //пуск
{
if(busyF
||!readyF) return false; //если привод не готов или находится в работе
startF=true;
}
float Velocity()
{
float ch_delta=tickMov/freq; //сколько времени прошло
float ret; //скорость которая была расчитана
if(accelF)
{
if(t_st<=0) t_st=0.001;
ret=v_st+(v_fi-v_st)*(ch_delta/t_st); //T
if(ch_delta>t_st) return v_fi;
if(ch_delta<=0) return v_st;
return ret;
}
if(decelF)
{
if(t_fi<=0) t_fi=0.001;
ret=v_st+(v_decel_ch-v_st)*((t_fi-ch_delta)/t_fi); //T
if(ch_delta>t_fi) return v_st;
if(ch_delta<=0) return v_decel_ch;
return ret;
}
return -1;
}
void redirect() //пересчёт
{
if(!busyF)//если двигатель не занят
{
if(startF //если он был запущен и он готов к перемещению
&&readyF)
{
busyF=true; //устанавливаем флаг занятости
startF=false //сбрасываем флаг пуска
if(step_mov>0) //устанавливаем бит направления
outDir=true;
else
outDir=false;
tickMov=0;
decelF=false;
accelF=true;
v_now=Velocity();
T1ch=(long)((freq)*dim)/v_now; //длительность периода
T1=0;
return;
}
}
else
{
tickMov++;
if(++T1>(T1ch>>1)) //если меньше пол периода то выводим 0 если болше 1
outStep=true;
else
outStep=false;
if(T1<T1ch) return;//если не сделали весь период то выходим
if(step_mov*dim>=(v_now*t_fi+(v_en-v_now)*t_fi/2) //если оставшаяся дистанция меньше либо равна дистанции для остановки то начинаем остановку
||breakF)
{
accelF=false;
decelF=true;
v_decel_ch=v_now;
tickMov=0;
}
v_now=Velocity();
T1ch=(long)((freq)*dim)/v_now; //длительность периода
T1=0;
if(step_mov>0)
{
step_mov--;
step_now++;
}
else
{
step_mov++;
step_now--;
}
if(step_mov==0 //если сделали все шаги
||(breakF&&decelF&&v_now==v_st)) //если остановка по требованию и мы замедлились
{
busyF=false;
}
}
}
}
//Если для C
void main(void)
{
WLDrive Drive;//Создаём переменную
Drive.t_st=0.1;
Drive.t_fi=0.2;
Drive.v_st=1;
Drive.v_fi=5;
Drive.freq=1000;//максимальная частота вывода в порт
Drive.dim=1;
Drive.setMovInc(100);//переместится на 100
Drive.start();
do
{
Drive.redirect();
//берём биты направления и шага из Drive
/*
например как для аннруино
digitalWrite(OutStep1,Drive.outStep);
digitalWrite(OutDir1,Drive.outDir);
*/
}
while(Drive.busyF)//если Drive в работе то продолжаем цикл
}
Это сильно упрощённый код, от рабочего кода моей программы.
Если что могу пояснить...
