Sergey Limansky
3
Dependencies: 411_lcdi2c1_latche TextLCD
main.cpp
- Committer:
- serg1958
- Date:
- 2021-03-17
- Revision:
- 0:e1947a05c2a2
- Child:
- 1:78d19167c0f7
File content as of revision 0:e1947a05c2a2:
/* STM32F411CEU6 i2c LCD*/
#include "mbed.h"
#include <math.h> /* cos */
#include "TextLCD.h"
I2C i2c_lcd(PB_9,PB_8); // SDA, SCL (активировать если i2c LCD )
TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type (активировать если i2c LCD)
//TextLCD lcd(PA_15, PB_3, PB_4, PB_5, PB_6, PB_7);// rs, e, d4-d7
//Serial pc(SERIAL_TX, SERIAL_RX);
//extern "C" void mbed_reset();
#define pi 3.14159265
DigitalInOut GenOut (PB_4, PIN_OUTPUT, PullUp, 1); // GEN
//Step Motor
DigitalInOut STEPX (PB_12, PIN_OUTPUT, PullDown, 1);
DigitalInOut DIRX (PB_13, PIN_OUTPUT, PullDown, 1); //OpenDrain
DigitalInOut STEPZ (PB_15, PIN_OUTPUT, PullDown, 1);
DigitalInOut DIRZ (PA_8, PIN_OUTPUT, PullDown, 1);
DigitalInOut AMX (PB_14, PIN_INPUT, PullDown, 0); // Alarm step motor X
DigitalInOut AMZ (PA_9, PIN_INPUT, PullDown, 0); // Alarm step motor Z
//Buton menu
DigitalInOut R_DZ (PB_0, PIN_INPUT, PullUp, 1);
DigitalInOut Up (PB_3, PIN_INPUT, PullUp, 1);
DigitalInOut Down (PA_15, PIN_INPUT, PullUp, 1);
//External CPU
InterruptIn CStX (PA_0);
DigitalInOut CDirX (PC_14, PIN_INPUT, PullUp, 1);
InterruptIn CStZ (PA_1);
DigitalInOut CDirZ (PC_15, PIN_INPUT, PullUp, 1);
DigitalInOut EStop (PC_13, PIN_OUTPUT, PullDown, 1);
//Joystik
DigitalInOut XJR (PA_10, PIN_INPUT, PullUp, 1);
DigitalInOut XJU (PA_11, PIN_INPUT, PullUp, 1);
DigitalInOut XJL (PA_12, PIN_INPUT, PullUp, 1);
DigitalInOut ZJR (PB_1, PIN_INPUT, PullUp, 1);
DigitalInOut ZJU (PB_2, PIN_INPUT, PullUp, 1);
DigitalInOut ZJL (PB_10, PIN_INPUT, PullUp, 1);
//Encoder & Tachometr
InterruptIn InTacho (PB_7); //
InterruptIn EncXA (PB_5);
InterruptIn EncXB (PB_6);
InterruptIn EncZA (PA_3);
InterruptIn EncZB (PA_2);
volatile int y;
int k;
volatile int k1=0;
volatile int K=0; // номер режима (0-ручной, 1 ввод коорд., 2 -резьба, 3-CPU, 4-конус, 5-ввод скоростей))
volatile long ind=0;
volatile int i=0;
volatile int j=0;
volatile int u;
volatile double time0=25000;
volatile double time1=25000; // время в мкс 1 оборота шпинделя
volatile double time2;
volatile int DX; // направление (куда нужно двигаться) DIRX
volatile int DX1;
volatile int EX; // флаг работы энкодера X
volatile int DZ; // направление (куда нужно двигаться) DIRZ
volatile int DZ1;
volatile int EZ; // флаг работы энкодера Z
volatile int DJX= 0; // флаг работы ддойстика X
volatile int DJX1= 0; // старый флаг работы ддойстика X
volatile int DJZ= 0; // флаг работы ддойстика Z
volatile int DJZ1= 0; // старый флаг работы ддойстика Z
volatile double Dlrezby=-50.00; // длина резьбы в мм со знаком
unsigned long NDlrezby; // длина резьбы в Step X
volatile double Schag=1.00; // шаг резьбы в мм на 1 оборот шпинделя
unsigned long NSchag; // шаг резьбы в Step X
volatile double Hrez=-1.00; // глубина профиля резьбы со знаком в мм
int NHrezZ; // глубина профиля резьбы в Step Z
volatile double hrez=0.05; // глубина врезания резца за один проход резьбы в мм
unsigned long Nhrez; // глубина врезания резца за один проход резьбы в Step Z
volatile double Saw=2.00; // безопасное расстояние отвода резца в мм
unsigned int NSaw; // безопасное расстояние отвода резца в мм Step Z
volatile double Back=5.00; // расстояние отвода резца в мм после нарезания резьбы
unsigned int NBack; // расстояние отвода резца в Step Z после нарезания нарезания резьбы
unsigned int NhSZ;
volatile long NvarX;
volatile long NvarZ;
volatile long NSchagX; // кол. шагов X
volatile long NSchagZ; // кол. шагов Z
float chist; // глубина чистового прохода в мм.
int Nchist; // кол. шагов для чистового прохода
volatile int FlagZ; // флаг готовности введённых данных для резьбы Z
//volatile int FlagZold; // предыдущее значение флага готовности введённых данных для резьбы Z
volatile int FlagX; // флаг готовности введённых данных для резьбы X
//volatile int FlagXold; // предыдущее значение флага готовности введённых данных для резьбы X
int ProchodX=0; // кол-во оставшихся проходов резьбы по X
volatile int Strob=0; // Начало оборота шпинделя
volatile float frequencY1=600; // частота вращения шпинделя в об/мин.
volatile float frequencY2=600;
volatile long NStX=0; // кол. StepX к выдаче
volatile long NStZ=0; // кол. StepZ к выдаче
volatile float bufX=0; // приёмный буфер X
float bufX1=0; // временный буфер X
float DISTX=400.00; // число для индикации положения оси X
volatile float bufZ=0; // приёмный буфер Z
float bufZ1=0; // временный буфер Z
float DISTZ=2000.00; // число для индикации положения оси Z
volatile int STMMZ = 400.00; // кол. ипульсов/мм энкодера Z
volatile double STZ = 2000.00; // кол. шагов ШД по Z на 1мм.(ШД 1/5: 2000=(200(имп/об)/1(мм/об)*2*5), шаг винта =1 мм/об)
volatile int KSTZ=10; // коэф. редукции Z (2 (механика) * 5 деление шага = 10)
volatile int STMMX = 400.00; // кол. ипульсов/мм энкодера X
volatile double STX = 400.00; // кол. шагов по X на 1мм. (ШД 1/5: 400=(200(имп/об)/5(мм/об)*2 механика * 5 деление шага) шаг винта =5 мм/об)
volatile int KSTX=10; // коэф. редукции X (2 (механика) * 5 (деление шага) = 10)
volatile int KX4=0; // от джойстика Х попускаем 4 и на 5 обрабатываем
volatile int N_T=1; // переключатель SpeedX1 и SpeedZ1 мм/мин или мм/оборот
volatile double SpeedX1 = 1.0; // скорость прохода X в мм/сек
volatile double SpeedX2 = 1500; // скорость быстрого перемещения X в мм/мин
volatile double SpeedZ1 = 1.0; // скорость прохода Z в мм/сек
volatile double SpeedZ2 = 400; // скорость быстрого перемещения Z в мм/мин
volatile double TimeXMax=2500; // Максимальное значение времени между шагами ШД (минимальная скорость)
//volatile double TIMERx=250; // время для таймера х
volatile double TimeXold=250; // предыдущее значение таймера X
volatile double TimeX=500; // переменная
volatile double TimeX0=500; //время в мкс между шагами ШД по Х при нарезании резьбы
volatile double TimeX1 =60000000/frequencY1/SpeedX1/STX; // время между шагами прохода X
volatile double TimeX2 =60000000/SpeedX2/STX; // время между шагами ускоренного перемещения X
volatile double SpeedX0 = 60000000/(frequencY1*TimeX1*STX);
volatile double TimeZMax=500; // Максимальное значение времени между шагами ШД (минимальная скорость)
//volatile double TIMERz=150; // время для таймера z
volatile double TimeZold=100; // предыдущее значение таймера Z
volatile double TimeZ=100;
volatile double TimeZ0 =100;
volatile double TimeZ1 =60000000/frequencY1/SpeedZ1/STZ; // время между шагами прохода Z
volatile double TimeZ2 =60000000/SpeedZ2/STZ; // время в мкс между шагами ускоренного перемещения Z
volatile double accelX=1.05; // % ускороения/торможения X
volatile double accelZ=1.01; // % ускороения/торможения Z
volatile int a=1; // признак движения с ускоронием/торможением
unsigned int NShBZ;
float angle=45; // угол конуса от 0 до +/-90 градусов (+ правый, -левый конус).
float Frec=600; // частота Gen1 в об/мин
float Gen1=7500000/Frec; // период генератора в мкс
Timer timer_Tacho;
Timeout timerZ;
Timeout timerX;
Ticker timer_Gen;
//----------------------------------------------
void Gen()
{
GenOut=!GenOut;
timer_Gen.attach_us(&Gen, (Gen1));
}
//----------------------------------------------
// Подпрограмма прерывания по Tacho
void ITacho()
{
time2 = timer_Tacho.read_us();
timer_Tacho.reset();
//InTacho.fall(&ITacho); //
//timer_Tacho.start() ;
time0 += time2;
if (time2>4000) // если частота меньше 3600 об/мин.
{
TimeX0 = 4*time2/(STX * Schag)-20; // время в мксек. между шагами ШД по Х при нарезании резьбы-34мкс(длительность обработки прерываний)
if (j>=3)
{
time1=(time0+time1)/2; //время одного оборота шпинделя в мкс.
time0 = 0; j=0; Strob=1; //
}
else { ++j; Strob=0; }
}
//se {time0=0; time1=0;}
}
//----------------------------------------------
// вывод частоты оборотов шпинделя
void OUTfrequencY ()
{
if ((timer_Tacho.read_us()-time1)>2000000) frequencY2=0;
else { frequencY2 = 60000000/time1;} // частота вращения шпинделя в о/мин
frequencY1=((frequencY2+frequencY1)/2);
lcd.locate(0, 3);
lcd.printf(" ");
lcd.locate(0, 3);
lcd.printf("%4.0f",frequencY1);
lcd.locate(4, 3);
lcd.printf(" n/m"); // выводим обороты шпинделя в об/мин
}
//----------------------------------------------
//Подпрограмма StepX
void TickX()
{
//if (a==0) TimeXold=TimeX;
timerX.attach_us(&TickX, (TimeXold)); // Задать период в с
if (FlagX)
{
if (DX)++NStX;
else --NStX;
--NvarX;
if (NvarX==0) FlagX=0;
}
if ((DJX&&K==0)||(DJX&&K==1)||(DJX&&K==4)) {
if (DX)++NStX;
else --NStX;}
if (NStX!=0)
{
if (TimeXold>(TimeX+10)){
if (TimeXold/accelX>(TimeX+10)) {TimeXold /= accelX;} // ускорение
else TimeXold=(TimeX+10);}
if (TimeXold<(TimeX+10)){
if (TimeXold*accelX<(TimeX+10)) {TimeXold *= accelX;} // торможение
else TimeXold=(TimeX+10);}
if (NStX>0) {DIRX=1; ++bufX; --NStX;}
if (NStX<0) {DIRX=0; --bufX; ++NStX;}
if (K!=1)
{
wait_us(2); //for (int q=20; q>0; ) { --q;++q;--q;} // 2 mks
STEPX=0;
wait_us(1); //for (int q=40; q>0; ) { --q;++q;--q;} //step - 3 mks
STEPX=1;
}
}
else {TimeXold=TimeXMax;}
}
//----------------------------------------------
//Подпрограмма StepZ
void TickZ()
{
//if (a==0) TimeZold=TimeZ;
timerZ.attach_us(&TickZ, (TimeZold)); // Задать период в мкс
if (FlagZ)
{
if (DZ)++NStZ;
else --NStZ;
--NvarZ;
if (NvarZ==0) FlagZ=0;
}
if ((DJZ&&K==0)||(DJZ&&K==1)||(DJZ&&K==4)) {
if (DZ)++NStZ;
else --NStZ;}
if (NStZ!=0)
{
if (TimeZold>(TimeZ-8)){
if (TimeZold/accelZ>(TimeZ-8)) {TimeZold /= accelZ;} // ускорение
else TimeZold=(TimeZ-8);}
if (TimeZold<(TimeZ-8)){
if (TimeZold*accelZ<(TimeZ-8)) {TimeZold *= accelZ;} // торможение
else TimeZold=(TimeZ-8);}
if (NStZ>0) {DIRZ=1; ++bufZ; --NStZ;}
if (NStZ<0) {DIRZ=0; --bufZ; ++NStZ;}
if (K!=1)
{
wait_us(2); //for (int q=20; q>0; ) { --q;++q;--q;} // 2 mks
STEPZ=0;
wait_us(1); //for (int q=30; q>0; ) { --q;++q;--q;} //step - 3 mks
STEPZ=1;
}
}
else {TimeZold=TimeZMax;}
}
//----------------------------------------------
// Подпрограмма вычисления по энкодеру Х
void EncX()
{
if (K==2||K==5) // резьба или ввод скоростей
{
if (DX1) ind+=1;
else ind-=1;
if (K==2)
{ // если резьба
if (ind>22) ind=0;
if (ind<0) ind=22; // кол-во вводимых параметров 0-5
i=ind/4;
}
if (K==5)
{ // если ввод скоростей
if (ind>14) ind=0;
if (ind<0) ind=14; // кол-во вводимых параметров 0-3
i=ind/4;
}
}
if (K==0||K==1||K==4)
{
if (!NStX) DX=DX1;
if (!DJX)
{
if (DX)++NStX;
else --NStX;
TimeX=250;
}
}
}
//----------------------------------------------
// Подпрограмма прерывания по энкодеру ХА
void IEncXA()
{
if (EncXA==EncXB) DX1=1;
else DX1=0;
EncX();
}
//----------------------------------------------
// Подпрограмма прерывания по энкодеру ХВ
void IEncXB()
{
if (EncXA!=EncXB) DX1=1;
else DX1=0;
EncX();
}
//----------------------------------------------
void EncZ()
{
if (K ==2) // резьба
{
if (!DZ1 &&(i==0)) {Dlrezby+=0.0025;}
if (!DZ1 &&(i==1)) {Schag+=0.0025;}
if (!DZ1 &&(i==2)) {Hrez+=0.0025;}
if (!DZ1 &&(i==3)) {hrez+=0.0025;}
if (!DZ1 &&(i==4)) {Saw+=0.0025;}
if (!DZ1 &&(i==5)) {Back+=0.0025;}
if (DZ1 &&(i==0)) {Dlrezby-=0.0025;}
if (DZ1 &&(i==1)) {Schag-=0.0025;}
if (DZ1 &&(i==2)) {Hrez-=0.0025;}
if (DZ1 &&(i==3)) {hrez-=0.0025;}
if (DZ1 &&(i==4)) {Saw-=0.0025;}
if (DZ1 &&(i==5)) {Back-=0.0025;}
}
if (K==4) //конус
{
if (!DZ1)
{
angle+=0.125f;
if (angle>=90.0f) angle=90.0f;
}
if (DZ1)
{
angle-=0.125f;
if (angle<=-90.0f) angle=-90.0f;
}
}
if (K==5)
{
if (!DZ1 &&(i==0)) { SpeedX1+=0.0025;
if (SpeedX1>5.00) SpeedX1=5.00; }
if (!DZ1 &&(i==1)) { SpeedX2+=0.25;
if (SpeedX2>1500) SpeedX2=1500; }
if (!DZ1 &&(i==2)) { SpeedZ1+=0.0025;
if (SpeedZ1>3.00) SpeedZ1=3.00; }
if (!DZ1 &&(i==3)) { SpeedZ2+=0.25;
if (SpeedZ2>400) SpeedZ2=400; }
if (DZ1 &&(i==0)) { SpeedX1-=0.0025;
if (SpeedX1<0.01) SpeedX1=0.01; }
if (DZ1 &&(i==1)) { SpeedX2-=0.25;
if (SpeedX2<1.00) SpeedX2=1.00; }
if (DZ1 &&(i==2)) { SpeedZ1-=0.0025;
if (SpeedZ1<0.01) SpeedZ1=0.01; }
if (DZ1 &&(i==3)) { SpeedZ2-=0.25;
if (SpeedZ2<1.00) SpeedZ2=1.00; }
}
if (!NStZ) DZ=DZ1;
if (K==0||K==1)
{
if (!DJZ)
{
if (DZ) {NStZ+=5;}
else {NStZ-=5;}
TimeZ=100;
}
}
}
//----------------------------------------------
// Подпрограмма прерывания по энкодеру ZА
void IEncZA()
{
if (K!=3) // ручной режим или ввод координат и т.д. кроме CPU
{
if (EncZA==EncZB) DZ1=0;
else DZ1=1;
EncZ();
}
}
//----------------------------------------------
// Подпрограмма прерывания по энкодеру ZB
void IEncZB()
{
if (K!=3) // ручной режим или ввод координат ит.д. кроме CPU
{
if (EncZA!=EncZB) DZ1=0;
else DZ1=1;
EncZ();
}
}
//----------------------------------------------
//Подпрограмма вывода в LCD
void OUTLCD()
{
//if (!K || K==1)
//{
DISTX = float ( bufX );
lcd.locate(0,0);
lcd.printf("x=%+7.2f",DISTX/STX);
lcd.locate(5,2);
if (!R_DZ)
{
lcd.printf("dia");
DISTZ = 2* float ( bufZ); // диаметр: значения координат * 2
}
else
{
lcd.printf("rad");
DISTZ = float ( bufZ);
}
lcd.locate(0,1);
lcd.printf("z=%+7.2f",DISTZ/STZ);
//}
bufZ1=bufZ;
bufX1=bufX;
}
//---------------------------
// Подпрограмма прерывания от CPU X в режиме внешнего управления
void CPUX()
{
if (K==3)
{
if (CDirX) {++bufX; DIRX=1;} // вправо
else {--bufX; DIRX=0;} // влево
wait_us(2);
STEPZ=0;
for (int q=30; q>0; )
{ --q;++q;--q;}
STEPZ=1; //step - 4 mks
}
}
//----------------------------------------------
// Подпрограмма прерывания от CPU Z в режиме внешнего управления
void CPUZ()
{
if (K==3)
{
if (CDirZ) {++bufZ; DIRZ=1;} // вправо
else {--bufX; DIRZ=0;} // влево
wait_us(2);
STEPZ=0;
for (int q=30; q>0; )
{ --q;++q;--q;}
STEPZ=1; //step - 4 mks
}
}
//----------------------------------------------
void wanProch ()
{
int k=1;
if (k==1) // врезаемся по Z на hrez мм.
{
TimeXold=TimeX0; // предварительно для таймера Х
TimeZ=TimeZ2; // период между шагами
NvarZ=Nhrez; // кол-во шагов
lcd.locate(17, 3); lcd.printf("-hr");
lcd.locate(10, 1); lcd.printf("vz=%7.2f",SpeedZ2);
FlagZ=1; // данные готовы
while(NvarZ>0){
OUTLCD();OUTfrequencY(); // ждём пока таймер выдаст "NSvar" стробов
if (NvarZ==0) break;}
FlagZ=0; ++k;
}
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY ();
//---------------------------
if (k==2) // нарезание резьбы один проход
{
NvarX=NSchagX; // кол-во шагов
TimeX=(TimeX0);
//lcd.locate(10, 2); lcd.printf("Tx=%7.6f",TimeXold*1000.0);
lcd.locate(17, 3); lcd.printf("-LX");
lcd.locate(10, 0); lcd.printf("vx=%7.2f",Schag);
while(Strob) {}
while(!Strob) {}
FlagX=1; // данные готовы
while (NvarX>0){ // ждём пока таймер выдаст "Nvar" стробов
TimeX=TimeX0;
if (NvarX==0) break;}
OUTLCD();OUTfrequencY();
//lcd.locate(10, 2); lcd.printf("Tx=%7.6f",TimeXold);
FlagX=0; ++k;
}
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY ();
//---------------------------
if (k==3) // отводим резец по Z на hrez+swZ мм.
{
TimeZ=TimeZ2; //период между шагами
NvarZ=NhSZ; // кол-во шагов для отвода резца на безопасное расстояние
lcd.locate(17, 3); lcd.printf(" Sv");
lcd.locate(10, 1); lcd.printf("vz=%7.2f",SpeedZ2);
DZ= !DZ; // смена направления движения по оси
FlagZ=1; // данные готовы
while (NvarZ>0){
OUTLCD(); OUTfrequencY(); // ждём пока таймер выдаст "NSvar" стробов
if (NvarZ==0) break;}
FlagZ=0; ++k;
}
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY ();
//---------------------------
if (k==4) // отводим резец по Х к началу резьбы
{
TimeX=TimeX2; // период между шагами
NvarX=NSchagX; // кол-во шагов
lcd.locate(17, 3); lcd.printf(" LX");
lcd.locate(10, 0); lcd.printf("vx=%7.2f",SpeedX2);
DX= !DX; // смена направления движения по оси
FlagX=1; // данные готовы
while (NvarX>0){
OUTLCD(); OUTfrequencY(); // ждём пока таймер выдаст "NSvar" стробов
if (NvarX==0) break;}
FlagX=0; ++k;
DX= !DX; // смена направления движения по оси
}
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY ();
//---------------------------
if (k==5) // подводим резец по Z на hrez+swZ мм.
{
TimeZ=TimeZ2; // период между шагами
NvarZ=NhSZ; // кол-во шагов
lcd.locate(17, 3); lcd.printf("-Sv");
lcd.locate(10, 1); lcd.printf("vz=%7.2f",SpeedZ2);
DZ= !DZ; // смена направления движения по оси
FlagZ=1; // данные готовы
while (NvarZ>0){
OUTLCD(); OUTfrequencY(); // ждём пока таймер выдаст "NSvar" стробов
if (NvarZ==0) break;}
FlagZ=0; ++k;
}
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY ();
--ProchodX;
}
//----------------------------------------------
// в случае сигнала alarm от ШД
void Alarm ()
{
__disable_irq();
lcd.cls(); // Clear LCD
wait_ms(20);
if (AMX) {
lcd.locate(5,1);
lcd.printf("ALARM X!");}
if (AMZ) {
lcd.locate(5,2);
lcd.printf("ALARM Z!");}
EStop =0; // для внешнего сомпьютера
while (AMX||AMZ){
if (!AMX&&!AMZ) break;} // ждем пока будет устранена авария
NVIC_SystemReset(); // рестарт системы
}
//----------------------------------------------
void Enc_enable ()
{
EncXA.rise(&IEncXA); EncXA.fall(&IEncXA); //
EncXB.rise(&IEncXB); EncXB.fall(&IEncXB); //
EncZA.rise(&IEncZA); EncZA.fall(&IEncZA); //
EncZB.rise(&IEncZB); EncZB.fall(&IEncZB); //
}
//----------------------------------------------
void Enc_disable ()
{
EncXA.rise(NULL); EncXA.fall(NULL); // запрещаем прерывания от энкодеров
EncXB.rise(NULL); EncXB.fall(NULL); //
EncZA.rise(NULL); EncZA.fall(NULL); //
EncZB.rise(NULL); EncZB.fall(NULL);
}
//----------------------------------------------
void Jx () // джойстик X
{
if (!XJR&& XJU) {DX=1; TimeX=TimeX1; DJX=1; } //если X вправо
if (!XJR&&!XJU) {DX=1; TimeX=TimeX2; DJX=2; } //если X быстро вправо
if (!XJL&& XJU) {DX=0; TimeX=TimeX1; DJX=1; } //если X влево
if (!XJL&&!XJU) {DX=0; TimeX=TimeX2; DJX=2; } //если X быстро влево
}
//----------------------------------------------
void Jz () // джойстик Z
{
if (!ZJR&& ZJU) {DZ=1; TimeZ=TimeZ1; DJZ=1; } //если Z вправо
if (!ZJR&&!ZJU) {DZ=1; TimeZ=TimeZ2; DJZ=2; } //если Z быстро вправо
if (!ZJL&& ZJU) {DZ=0; TimeZ=TimeZ1; DJZ=1; } //если Z влево
if (!ZJL&&!ZJU) {DZ=0; TimeZ=TimeZ2; DJZ=2; } //если Z быстро влево
}
//----------------------------------------------
void SpX1Z1 ()
{ // нормальная установка скоростей
if (N_T)
{
TimeX1 = 1000000/(STX*SpeedX1); // время в мкс между шагами ШДX если шпиндель не вращается
TimeZ1 = 1000000/(STZ*SpeedZ1); // время в мкс между шагами ШДZ если шпиндель не вращается
TimeX2 = 60000000/(SpeedX2*STX); // время в мкс между шагами ШДX при скорости Speed (мм/мин)
TimeZ2 = 60000000/(SpeedZ2*STZ); // время в мкс между шагами ШДZ при скорости Speed (мм/мин)
}
else
{
TimeX1 = 20*time1/(STX*SpeedX1); // время в мкс между шагами ШДX на 1 оборот шпинделя *20, а то очень быстро.
TimeZ1 = 20*time1/(STZ*SpeedZ1); // время в мкс между шагами ШДZ на 1 оборот шпинделя *20, а то очень быстро.
TimeX2 = 60000000/(SpeedX2*STX); // время в мкс между шагами ШДX при скорости Speed (мм/мин)
TimeZ2 = 60000000/(SpeedZ2*STZ); // время в мкс между шагами ШДZ при скорости Speed (мм/мин)
}
}
//----------------------------------------------
//----------------------------------------------
int main()
{
InTacho.mode(PullUp); //
EncXA.mode(PullUp);
EncXB.mode(PullUp);
EncZA.mode(PullUp);
EncZB.mode(PullUp);
InTacho.fall(&ITacho); //
timer_Tacho.start() ; //
Enc_enable ();
SpX1Z1 ();
timerX.attach_us(&TickX, (TimeXMax)); //
timerZ.attach_us(&TickZ, (TimeZMax)); //
timer_Gen.attach_us(&Gen, (Gen1));
ZJU.write ( 1);
STEPX=1;
STEPZ=1;
EStop=0;
//__enable_irq();
lcd.cls(); // Clear LCD
lcd.locate(3,1); lcd.printf("Latche control");
lcd.locate(4,3); lcd.printf("STM32F411ce");
wait(2);
lcd.cls();
K=0; k1=0;
//----------------------------------------------
//Основной цикл
while(1)
{
if (!Up)
{
wait_ms(100);
if (!Up)
{
--K;
if (K<0) K=5;
lcd.cls();
wait_ms(100);
}
}
if (!Down)
{
wait_ms(100);
if (!Down)
{
++K;
if (K>5) K=0;
lcd.cls(); wait_ms(100);
}
}
if (AMZ||AMX) Alarm ();
//--------------------------------------------
if (K==0||K==1) // режим ручного управления и ввода координат
{
if (!XJR||!XJL) // джойстик X
{ wait_ms (50);
if (!XJR&&XJU) { if (DJX!=1) Jx();}
if (!XJR&&!XJU) { if (DJX!=2) Jx();}
if (!XJL&&XJU) { if (DJX!=1) Jx();}
if (!XJL&&!XJU) { if (DJX!=2) Jx();}
}
else DJX=0;
//----------------
if (!ZJR||!ZJL) // джойстик X
{ wait_ms (50);
if (!ZJR&&ZJU) { if (DJZ!=1) Jz();}
if (!ZJR&&!ZJU) { if (DJZ!=2) Jz();}
if (!ZJL&&ZJU) { if (DJZ!=1) Jz();}
if (!ZJL&&!ZJU) { if (DJZ!=2) Jz();}
}
else DJZ=0;
//----------------
if (K==0)
{
if ((ZJR&&ZJL&&!ZJU)||(XJR&&XJL&&!XJU)) // ни влево, ни вправо, но нажата кнопка на любом
{ // джойстике - можем выставить лимбы энкодеров
DJX=0; DJZ=0;
Enc_disable();
while(!ZJU||!XJU) {if (ZJU&&XJU) { break;}}
Enc_enable();
}
lcd.locate(10, 0); lcd.printf("vx=%7.2f",SpeedX1);
lcd.locate(10, 1); lcd.printf("vz=%7.2f",SpeedZ1);
lcd.locate(10, 2); lcd.printf("VX=%7.0f",(SpeedX2));
lcd.locate(10, 3); lcd.printf("VZ=%7.0f",(SpeedZ2));
/*lcd.locate(10, 0); lcd.printf(" ");
lcd.locate(10, 0); lcd.printf("Tx=%7.2f",TimeX);
lcd.locate(10, 1); lcd.printf(" ");
lcd.locate(10, 1); lcd.printf("Tz=%7.2f",TimeZ);
//lcd.locate(10, 2); lcd.printf("VX=%7.2f",TimeX2); */
lcd.locate(0, 2); lcd.printf("Man ");
k1=0;
}
if (K==1) // режим ввода значений X Z
{
lcd.locate(11, 3);
lcd.printf("Enter X,Z");
if (!ZJU&&ZJR&&ZJL) bufZ=0;
if (!XJU&&XJR&&XJL) bufX=0; // обнуление координат X или Z
}
OUTLCD();
k1=0;
OUTfrequencY ();
} //end K0 K1
//----------------------------------------------
// режим: нарезание резьбы
if (K ==2)
{
if (!k1)
{
lcd.locate(0, 0); lcd.printf("Lx%7.2f",Dlrezby);
lcd.locate(10, 0); lcd.printf("st%6.2f",Schag);
lcd.locate(0, 1); lcd.printf("Hr%7.2f",Hrez);
lcd.locate(10, 1); lcd.printf("hr%6.2f",hrez);
lcd.locate(0, 2); lcd.printf("Sf%7.2f",Saw);
lcd.locate(10, 2); lcd.printf("Bk%6.2f",Back);
if (i==0){ lcd.locate(18, 3); lcd.printf("Lx");}
if (i==1){ lcd.locate(18, 3); lcd.printf("st");}
if (i==2){ lcd.locate(18, 3); lcd.printf("Hr");}
if (i==3){ lcd.locate(18, 3); lcd.printf("hr");}
if (i==4){ lcd.locate(18, 3); lcd.printf("Sf");}
if (i==5){ lcd.locate(18, 3); lcd.printf("Bk");}
SpX1Z1 ();
ProchodX=100*abs(Hrez)/(100*hrez);
lcd.locate(11, 3); lcd.printf(" %d ",ProchodX); // кол-во проходов по Х
lcd.locate(14, 3); lcd.printf("thr");
int Hrez1=100*abs(Hrez);
int hrez1 = 100*hrez;
int chist1 = Hrez1 % hrez1; // остаток в сотках на чистовой проход
chist = float(float(chist1)/100); // остаток в мм на чистовой проход
Nchist=int(chist*float(STZ)); // кол. шагов глубины чистового прохода
OUTfrequencY ();
if ( Dlrezby>0 ) DX=1; else DX= 0; // направление резьбы 1-левая, 0-правая резьба
if ( Hrez>0 ) DZ=1; else DZ= 0; // направление врезания 1-внутренняя, 0-наружная резьба
TimeX=TimeX0; // период между шагами по Х
NSchagX=abs(Dlrezby)*STX; // кол-во шагов по Х на всю длину резьбы
NSaw=Saw*STZ;
Nhrez= abs(hrez)*STZ;
NhSZ=NSaw+Nhrez;
NBack=Back*STZ+(abs(Hrez)*STZ); //отвод на Back мм
lcd.locate(11, 3); lcd.printf(" %d ",ProchodX);
} //end (!k1)---------------------------
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY ();
if ((!XJU||!ZJU)&& (k1==0))
{ // begin (k1==0))
while (!XJU||!ZJU) {wait_ms(20); if(XJU&&ZJU) break;} // ждём отпускания кнопки
++k1;
//-------------------------------------------------
Enc_disable (); // запрещаем прерывания от энкодеров
lcd.cls(); OUTLCD(); OUTfrequencY ();
for(ProchodX; ProchodX>0; ) // цикл по кол-ву проходов
{
lcd.locate(11, 3); lcd.printf(" %d ",ProchodX);
wanProch ();
lcd.locate(11, 3); lcd.printf(" %d ",ProchodX);
//while (1){wait_ms(20); if(!XJU||!ZJU) break;}
}
if (Nchist!=0)
{
lcd.locate(11, 3); lcd.printf("ch");
Nhrez = Nchist;
ProchodX=1;
wanProch ();
}
if (!ProchodX) // отвод резца после окончания резьбы
{
TimeZ=TimeZ2; //период между шагами
NvarZ=NBack; // кол-во шагов
lcd.locate(17, 3); lcd.printf(" BK");
lcd.locate(10, 1); lcd.printf("vz=%7.2f",SpeedZ2);
DZ= !DZ; // смена направления движения по оси
FlagZ=1; // данные готовы
while (NvarZ>0){
OUTLCD(); OUTfrequencY ();
if (NvarZ==0) break;} // ждём пока таймер выдаст "NSvar" стробов
FlagZ=0; ++k; K=0;
Enc_enable (); // разрешаем прерывания от энкодеров
lcd.cls();k1=0;
}
} // end k1=0
SpX1Z1 (); // установка скоростей по умолчанию
} // end (K ==2)
//-------------------------------------------------
// режим: управления от внешнего компьютера
if (K==3)
{
if (k1==0)
{
lcd.locate(10, 3); lcd.printf(" CPU ");
OUTLCD(); OUTfrequencY ();
CStX.rise(&CPUX); CStZ.rise(&CPUZ); // разрешаем прерывания от CPU
timerX.attach_us(&TickX, (NULL)); // запрещаем прерывания от timerX
timerZ.attach_us(&TickZ, (NULL)); // запрещаем прерывания от timerZ
Enc_disable (); // запрещаем прерывания от энкодеров
++k1; EStop =1;
}
if (AMZ||AMX) Alarm ();
if (k1==1)
{
if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();
OUTfrequencY();
if (!Up||!Down)
{
wait_ms(100);
if (!Up)
{
--K;
while(!Up) {wait_ms(100);}
}
if (!Down)
{
++K;
while(!Down) {wait_ms(100);}
}
wait_ms(100);
EStop =0;
++k1;
}
}
if (k1==2)
{
CStX.rise(NULL); CStZ.rise(NULL); // запрещаем прерывания от CPU
timerX.attach_us(&TickX, (TimeX1)); // разрешаем прерывания от timerX
timerZ.attach_us(&TickZ, (TimeZ1)); // разрешаем прерывания от timerZ
Enc_enable (); // разрешаем прерывания от энкодеров
EStop =0; k1=0; wait_ms(100);
}
SpX1Z1 (); // утановка скоростей по умолчанию
} //end K3
//-------------------------------------------------
// режим: конус
if (K==4)
{
double xn;
double zn;
double TimeX3;
double TimeZ3;
SpX1Z1 ();
OUTLCD();
OUTfrequencY ();
lcd.locate(10, 2); lcd.printf("angle %+3.0f",angle);
lcd.locate(10, 3); lcd.printf("cone deg");
double anglerad = double(angle)*pi/180;
xn=double (sin(anglerad));
zn=double (cos(anglerad));
TimeX3 = 1000000/(STX*SpeedX1*(xn));
TimeZ3 = 1000000/(STZ*SpeedZ1*(zn));
float speedX= float (SpeedX1*float(xn));
float speedZ= float (SpeedZ1*float(zn));
if (angle==0) speedX=0;
//lcd.locate(10, 0); lcd.printf("%-+8.3f",TimeX3);
//lcd.locate(10, 1); lcd.printf("%-+8.3f",TimeZ3);
lcd.locate(10, 0); lcd.printf("vx %-+3.4f",speedX);
lcd.locate(10, 1); lcd.printf("vz %-+3.4f",speedZ);
if (!XJR||!XJL) // джойстик X
{ wait_ms (50);
if (!XJR&&XJU) { if (DJX!=1) Jx();}
if (!XJR&&!XJU) { if (DJX!=2) Jx();}
if (!XJL&&XJU) { if (DJX!=1) Jx();}
if (!XJL&&!XJU) { if (DJX!=2) Jx();}
while (!XJR||!XJL) {OUTLCD(); OUTfrequencY ();}
}
else DJX=0;
OUTLCD(); OUTfrequencY ();
if (!ZJR&&ZJU||!ZJL&&ZJU) //джойстик Z
{
if (!ZJR&&ZJU) //если Z вперёд (-Z)
{
DZ=1; //
if (xn>0) DX=0; //X в плюс
if (xn<0) DX=1; //X в минус
}
if (!ZJL&&ZJU)
{
DZ=0; //если Z назад
if (xn>0) DX=1; //X в минус
if (xn<0) DX=0; //X в плюс
}
if (angle==90) {DJZ=0;}
else
{
TimeZ = TimeZ3;
DJZ=1;
}
if (angle==0) {DJX=0;} //запрет движения по X
else
{
TimeX=abs(TimeX3);
DJX=1;
}
Enc_disable();
while (!ZJR||!ZJL) {OUTLCD(); OUTfrequencY ();}
}
else
{
DJZ=0; DJX=0;
Enc_enable();
SpX1Z1 (); // утановка скоростей по умолчанию
}
OUTLCD(); OUTfrequencY ();
} //end K4
//-------------------------------------------------
// режим установки скоростей по X и Z
if (K==5)
{
if (!XJU||!ZJU) N_T=!N_T;
SpX1Z1 (); // утановка скоростей по умолчанию
if (N_T)
{
lcd.locate(0, 0); lcd.printf(" mm/s ");
}
else
{
lcd.locate(0, 0); lcd.printf(" mm/n ");
}
lcd.locate(10, 0); lcd.printf(" mm/min ");
lcd.locate(0, 1); lcd.printf("vx=%6.2f",SpeedX1);
lcd.locate(10, 1); lcd.printf("Vx=%7.2f",SpeedX2);
lcd.locate(0, 2); lcd.printf("vz=%6.2f",SpeedZ1);
lcd.locate(10, 2); lcd.printf("Vz=%7.2f",SpeedZ2);
if (i==0){ lcd.locate(12, 3); lcd.printf(" vx ");}
if (i==1){ lcd.locate(12, 3); lcd.printf(" Vx ");}
if (i==2){ lcd.locate(12, 3); lcd.printf(" vz ");}
if (i==3){ lcd.locate(12, 3); lcd.printf(" Vz ");}
OUTfrequencY();
} //end K5
//-------------------------------------------------
//SpX1Z1 ();
} //end while(1)
} //end main