main.cpp

00001 
00002 /* STM32F411CEU6 i2c LCD*/
00003 #include "mbed.h"
00004 #include <math.h>       /* cos */
00005 #include "TextLCD.h"
00006 I2C i2c_lcd(PB_9,PB_8);                                       // SDA, SCL (активировать если i2c LCD )
00007 TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4);            // I2C bus, PCF8574 Slaveaddress, LCD Type (активировать если i2c LCD)
00008  
00009 //TextLCD lcd(PA_15, PB_3, PB_4, PB_5, PB_6, PB_7);// rs, e, d4-d7
00010 //Serial pc(SERIAL_TX, SERIAL_RX);
00011 //extern "C" void mbed_reset();
00012 #define pi 3.14159265
00013 
00014 DigitalInOut GenOut         (PB_4, PIN_OUTPUT, PullUp, 1);                 // GEN 
00015 
00016 
00017 //Step Motor
00018 DigitalInOut STEPX          (PB_12, PIN_OUTPUT, PullDown, 1);
00019 DigitalInOut DIRX           (PB_13, PIN_OUTPUT, PullDown, 1);                //OpenDrain
00020 DigitalInOut STEPZ          (PB_15, PIN_OUTPUT, PullDown, 1);
00021 DigitalInOut DIRZ           (PA_8, PIN_OUTPUT, PullDown, 1);
00022 DigitalInOut AMX            (PB_14, PIN_INPUT, PullDown, 0);         // Alarm step motor X
00023 DigitalInOut AMZ            (PA_9, PIN_INPUT, PullDown, 0);         // Alarm step motor Z
00024 
00025 //Buton menu
00026 DigitalInOut R_DZ           (PB_0, PIN_INPUT, PullUp, 1);
00027 DigitalInOut Up             (PB_3, PIN_INPUT, PullUp, 1);
00028 DigitalInOut Down           (PA_15, PIN_INPUT, PullUp, 1);
00029 
00030 //External CPU
00031 InterruptIn  CStX           (PA_0);
00032 DigitalInOut CDirX          (PC_14, PIN_INPUT, PullUp, 1);
00033 InterruptIn  CStZ           (PA_1);
00034 DigitalInOut CDirZ          (PC_15, PIN_INPUT, PullUp, 1);
00035 DigitalInOut EStop          (PC_13, PIN_OUTPUT, PullDown, 1);
00036 
00037 //Joystik
00038 DigitalInOut  XJR           (PA_10, PIN_INPUT, PullUp, 1);
00039 DigitalInOut  XJU           (PA_11, PIN_INPUT, PullUp, 1);
00040 DigitalInOut  XJL           (PA_12, PIN_INPUT, PullUp, 1);
00041 DigitalInOut  ZJR           (PB_1, PIN_INPUT, PullUp, 1);
00042 DigitalInOut  ZJU           (PB_2, PIN_INPUT, PullUp, 1);
00043 DigitalInOut  ZJL           (PB_10, PIN_INPUT, PullUp, 1);
00044 
00045 
00046 //Encoder & Tachometr
00047 InterruptIn InTacho         (PB_7);             //
00048 InterruptIn EncXA           (PB_5);
00049 InterruptIn EncXB           (PB_6);
00050 InterruptIn EncZA           (PA_3);
00051 InterruptIn EncZB           (PA_2);
00052 
00053 volatile int y;
00054 int k;
00055 volatile int k1=0;
00056 volatile int K=0;                           // номер режима (0-ручной, 1 ввод коорд., 2 -резьба, 3-CPU, 4-конус, 5-ввод скоростей))
00057 volatile long ind=0;
00058 volatile int i=0;
00059 volatile int j=0;
00060 volatile int u; 
00061 volatile double time0=25000;
00062 volatile double time1=25000;                // время в мкс 1 оборота шпинделя
00063 
00064 volatile double time2;
00065 
00066 volatile int DX;                            // направление (куда нужно двигаться) DIRX
00067 volatile int DX1;
00068 volatile int EX;                            // флаг работы энкодера X
00069 volatile int DZ;                            // направление (куда нужно двигаться) DIRZ
00070 volatile int DZ1;
00071 volatile int EZ;                            // флаг работы энкодера Z
00072 volatile int DJX= 0;                        // флаг работы ддойстика X
00073 volatile int DJX1= 0;                       // старый флаг работы ддойстика X
00074 volatile int DJZ= 0;                        // флаг работы ддойстика Z
00075 volatile int DJZ1= 0;                       // старый флаг работы ддойстика Z
00076 
00077 volatile double Dlrezby=-50.00;             // длина резьбы в мм со знаком
00078 unsigned long NDlrezby;                     // длина резьбы в Step X  
00079 volatile double Schag=1.00;                 // шаг резьбы в мм на 1 оборот шпинделя
00080 unsigned long NSchag;                       // шаг резьбы в Step X
00081 volatile double Hrez=-1.00;                 // глубина профиля резьбы со знаком в мм
00082 int NHrezZ;                                 // глубина профиля резьбы в Step Z
00083 volatile double hrez=0.05;                   // глубина врезания резца за один проход резьбы в мм
00084 unsigned long Nhrez;                        // глубина врезания резца за один проход резьбы в Step Z
00085 volatile double Saw=2.00;                   // безопасное расстояние отвода резца в мм 
00086 unsigned int NSaw;                          // безопасное расстояние отвода резца в мм Step Z
00087 volatile double Back=5.00;                  // расстояние отвода резца в мм  после нарезания резьбы
00088 unsigned int NBack;                         // расстояние отвода резца в Step Z после нарезания нарезания резьбы
00089 unsigned int NhSZ;
00090 volatile long NvarX;
00091 volatile long NvarZ;
00092 volatile long NSchagX;                      // кол. шагов X 
00093 volatile long NSchagZ;                      // кол. шагов Z
00094 float chist;                                // глубина чистового прохода в мм.
00095 int Nchist;                                 // кол. шагов для чистового прохода
00096 
00097 
00098 volatile int FlagZ;                         // флаг готовности введённых данных для резьбы Z
00099 //volatile int FlagZold;                      // предыдущее значение флага готовности введённых данных для резьбы Z
00100 volatile int FlagX;                         // флаг готовности введённых данных для резьбы X
00101 //volatile int FlagXold;                      // предыдущее значение флага готовности введённых данных для резьбы X
00102 int ProchodX=0;                             // кол-во оставшихся проходов резьбы по X
00103 volatile int Strob=0;                       // Начало оборота шпинделя
00104 volatile float frequencY1=600;              // частота вращения шпинделя в об/мин.
00105 volatile float frequencY2=600;
00106 
00107 volatile long NStX=0;                                       // кол. StepX к выдаче
00108 volatile long NStZ=0;                                       // кол. StepZ к выдаче
00109 volatile float bufX=0;                                      // приёмный буфер X
00110 float bufX1=0;                                              // временный буфер X
00111 float DISTX=400.00;                                         // число для индикации положения оси X
00112 volatile float  bufZ=0;                                     // приёмный буфер Z
00113 float bufZ1=0;                                              // временный буфер Z
00114 float DISTZ=2000.00;                                        // число для индикации положения оси Z
00115 
00116 volatile int STMMZ = 400.00;                                // кол. ипульсов/мм энкодера Z
00117 volatile double STZ = 2000.00;                              // кол. шагов ШД по Z на 1мм.(ШД 1/5: 2000=(200(имп/об)/1(мм/об)*2*5), шаг винта =1 мм/об)
00118 volatile int KSTZ=10;                                       // коэф. редукции Z (2 (механика) * 5 деление шага = 10)
00119 volatile int STMMX = 400.00;                                // кол. ипульсов/мм энкодера X
00120 volatile double STX = 400.00;                               // кол. шагов по X на 1мм. (ШД 1/5: 400=(200(имп/об)/5(мм/об)*2 механика * 5 деление шага) шаг винта =5 мм/об)
00121 volatile int KSTX=10;                                       // коэф. редукции X (2 (механика) * 5 (деление шага) = 10)
00122 volatile int KX4=0;                                         // от джойстика Х попускаем 4 и на 5 обрабатываем
00123 
00124 volatile int N_T=1;                                         // переключатель SpeedX1 и SpeedZ1 мм/мин или мм/оборот
00125 volatile double SpeedX1 = 1.0;                              // скорость прохода X в мм/сек
00126 volatile double SpeedX2 = 1500;                             // скорость быстрого перемещения X в мм/мин
00127 volatile double SpeedZ1 = 1.0;                              // скорость прохода Z в мм/сек
00128 volatile double SpeedZ2 = 400;                              // скорость быстрого перемещения Z в мм/мин
00129 
00130 volatile double TimeXMax=2500;                              // Максимальное значение времени между шагами ШД (минимальная скорость)
00131 //volatile double TIMERx=250;                               // время для таймера х
00132 volatile double TimeXold=250;                           // предыдущее значение таймера X
00133 volatile double TimeX=500;                               // переменная
00134 volatile double TimeX0=500;                              //время в мкс между шагами ШД по Х при нарезании резьбы
00135 volatile double TimeX1 =60000000/frequencY1/SpeedX1/STX;          // время между шагами прохода X
00136 volatile double TimeX2 =60000000/SpeedX2/STX;                     // время между шагами ускоренного перемещения X 
00137 volatile double SpeedX0 = 60000000/(frequencY1*TimeX1*STX);
00138 
00139 
00140 volatile double TimeZMax=500;                               // Максимальное значение времени между шагами ШД (минимальная скорость)
00141 //volatile double TIMERz=150;                                // время для таймера z
00142 volatile double TimeZold=100;                             // предыдущее значение таймера Z
00143 volatile double TimeZ=100;
00144 volatile double TimeZ0 =100;
00145 volatile double TimeZ1 =60000000/frequencY1/SpeedZ1/STZ;           // время между шагами прохода Z
00146 volatile double TimeZ2 =60000000/SpeedZ2/STZ;                      // время в мкс между шагами ускоренного перемещения Z
00147 volatile double accelX=1.05;                                 // % ускороения/торможения X
00148 volatile double accelZ=1.01;                                 // % ускороения/торможения Z
00149 volatile int a=1;                                            // признак движения с ускоронием/торможением
00150 unsigned int NShBZ;
00151 float angle=45;                                               // угол конуса от 0 до +/-90 градусов (+ правый, -левый конус).
00152 
00153 float Frec=600;                            // частота Gen1 в об/мин     
00154 float Gen1=7500000/Frec;                        // период генератора в мкс 
00155 
00156 Timer timer_Tacho;
00157 Timeout timerZ;
00158 Timeout timerX;
00159 Ticker timer_Gen;
00160 
00161 //----------------------------------------------  
00162 void Gen()
00163 {
00164  GenOut=!GenOut; 
00165  timer_Gen.attach_us(&Gen, (Gen1));
00166 }
00167 
00168 //----------------------------------------------  
00169 // Подпрограмма прерывания по Tacho
00170 void ITacho()
00171   {
00172     time2 = timer_Tacho.read_us();
00173     timer_Tacho.reset();
00174     //InTacho.fall(&ITacho);                          //
00175     //timer_Tacho.start() ; 
00176     time0 += time2;
00177     if (time2>4000)                                 // если частота меньше  3600 об/мин.
00178     {
00179         TimeX0 = 4*time2/(STX * Schag)-20;   // время в мксек. между шагами ШД по Х при нарезании резьбы-34мкс(длительность обработки прерываний) 
00180         if (j>=3) 
00181         {
00182         time1=(time0+time1)/2;                      //время одного оборота шпинделя в мкс.  
00183         time0 = 0;  j=0; Strob=1;                   //
00184         }
00185         else { ++j; Strob=0; }
00186     }
00187     //se {time0=0; time1=0;}
00188   }
00189 //---------------------------------------------- 
00190 // вывод частоты оборотов шпинделя
00191  void OUTfrequencY ()
00192 { 
00193   if ((timer_Tacho.read_us()-time1)>2000000)  frequencY2=0;
00194   else { frequencY2 = 60000000/time1;}                       // частота вращения шпинделя в о/мин
00195   frequencY1=((frequencY2+frequencY1)/2);
00196   lcd.locate(0, 3);
00197   lcd.printf("    ");
00198   lcd.locate(0, 3);
00199   lcd.printf("%4.0f",frequencY1);
00200   lcd.locate(4, 3); 
00201   lcd.printf(" n/m");                                   // выводим обороты шпинделя в об/мин 
00202 }
00203 
00204 //----------------------------------------------
00205   //Подпрограмма StepX
00206   void  TickX()
00207 {
00208   //if (a==0) TimeXold=TimeX;
00209   timerX.attach_us(&TickX, (TimeXold));     // Задать период в с 
00210     if (FlagX)                  
00211          {
00212           if (DX)++NStX;
00213             else --NStX;
00214           --NvarX;
00215           if (NvarX==0) FlagX=0;  
00216          }
00217     if ((DJX&&K==0)||(DJX&&K==1)||(DJX&&K==4)) { 
00218     if (DX)++NStX;
00219         else --NStX;}
00220     if (NStX!=0)
00221       {     
00222         if (TimeXold>(TimeX+10)){
00223              if (TimeXold/accelX>(TimeX+10)) {TimeXold /= accelX;}       // ускорение
00224                 else TimeXold=(TimeX+10);}
00225         if (TimeXold<(TimeX+10)){
00226             if (TimeXold*accelX<(TimeX+10)) {TimeXold *= accelX;}       // торможение
00227                 else TimeXold=(TimeX+10);}
00228                 
00229         if (NStX>0)  {DIRX=1; ++bufX; --NStX;}
00230         if (NStX<0) {DIRX=0; --bufX; ++NStX;}
00231         if (K!=1)   
00232         {
00233             wait_us(2);                         //for (int q=20; q>0;  ) { --q;++q;--q;}                // 2 mks                    
00234             STEPX=0; 
00235             wait_us(1);                         //for (int q=40; q>0;  )  { --q;++q;--q;}               //step - 3 mks                             
00236             STEPX=1;          
00237         }
00238       }
00239       else {TimeXold=TimeXMax;}
00240 } 
00241 //----------------------------------------------
00242  //Подпрограмма StepZ
00243   void  TickZ()
00244 {   
00245      //if (a==0) TimeZold=TimeZ;
00246      timerZ.attach_us(&TickZ, (TimeZold));     // Задать период в мкс 
00247         if (FlagZ)                  
00248          {
00249             if (DZ)++NStZ;
00250             else --NStZ;
00251             --NvarZ;
00252             if (NvarZ==0) FlagZ=0;  
00253          }
00254         if ((DJZ&&K==0)||(DJZ&&K==1)||(DJZ&&K==4))  {
00255             if (DZ)++NStZ;
00256                 else --NStZ;} 
00257 
00258         if (NStZ!=0)
00259          {
00260             if (TimeZold>(TimeZ-8)){
00261                 if (TimeZold/accelZ>(TimeZ-8)) {TimeZold /= accelZ;}       // ускорение
00262                     else TimeZold=(TimeZ-8);}
00263             if (TimeZold<(TimeZ-8)){
00264                 if (TimeZold*accelZ<(TimeZ-8)) {TimeZold *= accelZ;}       // торможение
00265                     else TimeZold=(TimeZ-8);}
00266             
00267             if (NStZ>0)  {DIRZ=1; ++bufZ; --NStZ;}
00268             if (NStZ<0) {DIRZ=0; --bufZ; ++NStZ;}    
00269             if (K!=1) 
00270             {
00271                 wait_us(2);                         //for (int q=20; q>0;  ) { --q;++q;--q;}                  // 2 mks                    
00272                 STEPZ=0; 
00273                 wait_us(1);                         //for (int q=30; q>0;  ) { --q;++q;--q;}                  //step - 3 mks                              
00274                 STEPZ=1; 
00275              }   
00276          }
00277          else {TimeZold=TimeZMax;} 
00278 } 
00279  
00280 //----------------------------------------------
00281 // Подпрограмма вычисления по энкодеру Х
00282  void EncX()
00283 {
00284   if (K==2||K==5)                                        // резьба или ввод скоростей 
00285     {
00286       if (DX1) ind+=1;
00287         else ind-=1;
00288       if (K==2)  
00289         {                                         // если резьба
00290         if (ind>22) ind=0;  
00291         if (ind<0) ind=22;                                 // кол-во вводимых параметров 0-5
00292         i=ind/4; 
00293         } 
00294       if (K==5)  
00295         {                                         // если ввод скоростей
00296         if (ind>14) ind=0;  
00297         if (ind<0) ind=14;                                 // кол-во вводимых параметров 0-3
00298         i=ind/4; 
00299         }
00300     }
00301   if (K==0||K==1||K==4) 
00302      {
00303         if (!NStX)  DX=DX1;
00304         if (!DJX)
00305         {
00306         if (DX)++NStX;
00307         else --NStX;
00308         TimeX=350;
00309         }
00310      } 
00311 }
00312 //----------------------------------------------
00313 // Подпрограмма прерывания по энкодеру ХА
00314  void IEncXA()
00315 {
00316   if (EncXA==EncXB) DX1=1;
00317     else DX1=0;
00318   EncX();
00319 }
00320 //----------------------------------------------     
00321 // Подпрограмма прерывания по энкодеру ХВ
00322   void IEncXB()
00323  {
00324   if (EncXA!=EncXB) DX1=1;
00325     else DX1=0;
00326   EncX();   
00327 } 
00328 //----------------------------------------------
00329 void EncZ()
00330 {
00331     if (K ==2)                                          // резьба
00332    { 
00333      if (!DZ1 &&(i==0)) {Dlrezby+=0.0025;}
00334      if (!DZ1 &&(i==1)) {Schag+=0.0025;}
00335      if (!DZ1 &&(i==2)) {Hrez+=0.0025;}
00336      if (!DZ1 &&(i==3)) {hrez+=0.0025;}
00337      if (!DZ1 &&(i==4)) {Saw+=0.0025;}
00338      if (!DZ1 &&(i==5)) {Back+=0.0025;}
00339     
00340      if (DZ1 &&(i==0)) {Dlrezby-=0.0025;}
00341      if (DZ1 &&(i==1)) {Schag-=0.0025;}
00342      if (DZ1 &&(i==2)) {Hrez-=0.0025;}
00343      if (DZ1 &&(i==3)) {hrez-=0.0025;}
00344      if (DZ1 &&(i==4)) {Saw-=0.0025;}
00345      if (DZ1 &&(i==5)) {Back-=0.0025;}
00346    }
00347    if (K==4)                                              //конус
00348    {
00349       if (!DZ1) 
00350         { 
00351             angle+=0.125f;  
00352             if (angle>=90.0f) angle=90.0f;
00353         }
00354        if (DZ1)  
00355         { 
00356             angle-=0.125f; 
00357             if (angle<=-90.0f) angle=-90.0f; 
00358         }    
00359     }
00360     
00361    if (K==5)   
00362    {
00363       if (!DZ1 &&(i==0)) { SpeedX1+=0.0025;
00364           if (SpeedX1>5.00) SpeedX1=5.00; }  
00365       if (!DZ1 &&(i==1)) { SpeedX2+=0.25;
00366           if (SpeedX2>1500) SpeedX2=1500; }  
00367       if (!DZ1 &&(i==2)) { SpeedZ1+=0.0025;
00368           if (SpeedZ1>3.00) SpeedZ1=3.00; }  
00369       if (!DZ1 &&(i==3)) { SpeedZ2+=0.25;
00370           if (SpeedZ2>400) SpeedZ2=400; }  
00371    
00372       if (DZ1 &&(i==0)) { SpeedX1-=0.0025;
00373           if (SpeedX1<0.01) SpeedX1=0.01; }
00374       if (DZ1 &&(i==1)) { SpeedX2-=0.25;
00375           if (SpeedX2<1.00) SpeedX2=1.00; }
00376       if (DZ1 &&(i==2)) { SpeedZ1-=0.0025;
00377           if (SpeedZ1<0.01) SpeedZ1=0.01; }
00378       if (DZ1 &&(i==3)) { SpeedZ2-=0.25;
00379           if (SpeedZ2<1.00) SpeedZ2=1.00; } 
00380     }
00381     if (!NStZ) DZ=DZ1;
00382     if (K==0||K==1)
00383     {   
00384         if (!DJZ)
00385         {       
00386         if (DZ) {NStZ+=5;}
00387         else {NStZ-=5;}
00388         TimeZ=100;
00389         }
00390     }  
00391 }
00392 //----------------------------------------------
00393 // Подпрограмма прерывания по энкодеру ZА
00394  void IEncZA()
00395 {
00396  if (K!=3)                              // ручной режим или ввод координат и т.д. кроме CPU  
00397   { 
00398    if (EncZA==EncZB) DZ1=0;
00399     else DZ1=1;
00400    EncZ();
00401   }             
00402 }
00403 //----------------------------------------------
00404 // Подпрограмма прерывания по энкодеру ZB
00405  void IEncZB()
00406 {
00407   if (K!=3)                              // ручной режим или ввод координат ит.д. кроме CPU  
00408    {
00409     if (EncZA!=EncZB) DZ1=0;                                           
00410      else DZ1=1;
00411     EncZ();
00412    }
00413 }
00414 //----------------------------------------------
00415 //Подпрограмма вывода в LCD
00416 void OUTLCD()
00417 {
00418  //if (!K || K==1) 
00419  //{
00420   DISTX = float ( bufX  );
00421   lcd.locate(0,0);
00422   lcd.printf("x=%+7.2f",DISTX/STX);
00423   lcd.locate(5,2);
00424   if (!R_DZ) 
00425   {
00426     lcd.printf("dia");
00427     DISTZ = 2* float ( bufZ);                   // диаметр: значения координат * 2
00428   }  
00429     else 
00430     { 
00431     lcd.printf("rad"); 
00432     DISTZ = float ( bufZ);
00433     }
00434   lcd.locate(0,1);
00435   lcd.printf("z=%+7.2f",DISTZ/STZ);
00436  //} 
00437   bufZ1=bufZ;
00438   bufX1=bufX;      
00439 } 
00440 //---------------------------
00441 // Подпрограмма прерывания от CPU X в режиме внешнего управления 
00442  void CPUX()
00443  {
00444    if (K==3)
00445    {
00446    if (CDirX) {++bufX; DIRX=1;}                         // вправо
00447     else {--bufX; DIRX=0;}                              // влево
00448    wait_us(2);
00449        STEPZ=0; 
00450        for (int q=30; q>0;  )                              
00451        { --q;++q;--q;} 
00452        STEPZ=1;                       //step - 4 mks
00453     }
00454  }
00455 //----------------------------------------------     
00456 // Подпрограмма прерывания от CPU Z в режиме внешнего управления 
00457  void CPUZ()
00458  {
00459    if (K==3)
00460    {
00461    if (CDirZ) {++bufZ; DIRZ=1;}                         // вправо
00462     else {--bufX; DIRZ=0;}                              // влево
00463    wait_us(2);
00464        STEPZ=0; 
00465        for (int q=30; q>0;  )                              
00466        { --q;++q;--q;} 
00467        STEPZ=1;                       //step - 4 mks
00468     }
00469  }
00470 //----------------------------------------------     
00471 void wanProch ()
00472 {                                       
00473     int k=1;
00474     if (k==1)                                           // врезаемся по Z на hrez мм. 
00475       {  
00476       TimeXold=TimeX0;                                  // предварительно для таймера Х
00477       TimeZ=TimeZ2;                                     // период между шагами 
00478       NvarZ=Nhrez;                                      // кол-во шагов 
00479       lcd.locate(17, 3);   lcd.printf("-hr");
00480       lcd.locate(10, 1);   lcd.printf("vz=%7.2f",SpeedZ2);
00481       FlagZ=1;                                          // данные готовы
00482       while(NvarZ>0){
00483       OUTLCD();OUTfrequencY();              // ждём пока таймер выдаст "NSvar" стробов
00484       if (NvarZ==0) break;} 
00485       FlagZ=0; ++k; 
00486       }
00487       if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD(); 
00488       OUTfrequencY ();
00489     //---------------------------            
00490     if (k==2)                                           // нарезание резьбы один проход 
00491       {
00492       NvarX=NSchagX;                                    // кол-во шагов 
00493       TimeX=(TimeX0);
00494       //lcd.locate(10, 2);   lcd.printf("Tx=%7.6f",TimeXold*1000.0);
00495       lcd.locate(17, 3);   lcd.printf("-LX");
00496       lcd.locate(10, 0);   lcd.printf("vx=%7.2f",Schag);
00497       while(Strob) {}
00498       while(!Strob) {}
00499       FlagX=1;                                          // данные готовы
00500       while (NvarX>0){                                  // ждём пока таймер выдаст "Nvar" стробов
00501        TimeX=TimeX0; 
00502        if (NvarX==0) break;}
00503       OUTLCD();OUTfrequencY();
00504       //lcd.locate(10, 2);   lcd.printf("Tx=%7.6f",TimeXold); 
00505       FlagX=0; ++k; 
00506       }
00507       if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD(); 
00508       OUTfrequencY ();
00509     //---------------------------            
00510     if (k==3)                                           // отводим резец по Z на hrez+swZ мм.
00511       {
00512       TimeZ=TimeZ2;                                     //период между шагами              
00513       NvarZ=NhSZ;                                       // кол-во шагов для отвода резца на безопасное расстояние
00514       lcd.locate(17, 3);   lcd.printf(" Sv");
00515       lcd.locate(10, 1);   lcd.printf("vz=%7.2f",SpeedZ2); 
00516       DZ= !DZ;                                          // смена направления движения по оси
00517       FlagZ=1;                                          // данные готовы
00518       while (NvarZ>0){ 
00519       OUTLCD(); OUTfrequencY();        // ждём пока таймер выдаст "NSvar" стробов
00520        if (NvarZ==0) break;} 
00521       FlagZ=0; ++k;
00522       }
00523       if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD(); 
00524       OUTfrequencY ();
00525     //---------------------------             
00526     if (k==4)                                           // отводим резец по Х к началу резьбы
00527       {
00528       TimeX=TimeX2;                                     // период между шагами 
00529       NvarX=NSchagX;                                    // кол-во шагов 
00530       lcd.locate(17, 3);   lcd.printf(" LX");
00531       lcd.locate(10, 0);   lcd.printf("vx=%7.2f",SpeedX2);    
00532       DX= !DX;                                          // смена направления движения по оси
00533       FlagX=1;                                          // данные готовы
00534       while (NvarX>0){ 
00535       OUTLCD(); OUTfrequencY();        // ждём пока таймер выдаст "NSvar" стробов
00536        if (NvarX==0) break;} 
00537       FlagX=0; ++k;          
00538       DX= !DX;                                          // смена направления движения по оси
00539       }
00540       if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD(); 
00541       OUTfrequencY ();
00542     //---------------------------   
00543     if (k==5)                                           // подводим резец по Z на hrez+swZ мм.
00544       {
00545       TimeZ=TimeZ2;                                     // период между шагами              
00546       NvarZ=NhSZ;                                       // кол-во шагов 
00547       lcd.locate(17, 3);   lcd.printf("-Sv");
00548       lcd.locate(10, 1);   lcd.printf("vz=%7.2f",SpeedZ2);
00549       DZ= !DZ;                                          // смена направления движения по оси
00550       FlagZ=1;                                          // данные готовы
00551       while (NvarZ>0){  
00552       OUTLCD(); OUTfrequencY();       // ждём пока таймер выдаст "NSvar" стробов
00553        if (NvarZ==0) break;} 
00554       FlagZ=0; ++k; 
00555       }
00556       if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD(); 
00557       OUTfrequencY ();          
00558       --ProchodX;
00559  }
00560 //---------------------------------------------- 
00561 // в случае сигнала alarm от ШД
00562     
00563  void Alarm ()                    
00564 {
00565     __disable_irq();
00566     lcd.cls();                                          // Clear LCD
00567     wait_us(20000);
00568     if (AMX) {
00569     lcd.locate(5,1);                
00570     lcd.printf("ALARM X!");}
00571     if (AMZ) {
00572     lcd.locate(5,2);                
00573     lcd.printf("ALARM Z!");}
00574     EStop =0;                                           // для внешнего сомпьютера
00575     while (AMX||AMZ){
00576      if (!AMX&&!AMZ) break;}                            // ждем пока будет устранена авария
00577     NVIC_SystemReset();                                 // рестарт системы
00578 }
00579 //---------------------------------------------- 
00580 void Enc_enable ()
00581 {
00582     EncXA.rise(&IEncXA); EncXA.fall(&IEncXA);       //
00583     EncXB.rise(&IEncXB); EncXB.fall(&IEncXB);       //
00584     EncZA.rise(&IEncZA); EncZA.fall(&IEncZA);       //
00585     EncZB.rise(&IEncZB); EncZB.fall(&IEncZB);       //
00586       
00587 }
00588 
00589 //---------------------------------------------- 
00590 void Enc_disable ()
00591 {
00592     EncXA.rise(NULL); EncXA.fall(NULL);              // запрещаем прерывания от энкодеров
00593     EncXB.rise(NULL); EncXB.fall(NULL);              //
00594     EncZA.rise(NULL); EncZA.fall(NULL);              //
00595     EncZB.rise(NULL); EncZB.fall(NULL);
00596 }
00597 //---------------------------------------------- 
00598 void Jx ()                                            // джойстик X
00599 {
00600     if (!XJR&& XJU) {DX=1; TimeX=TimeX1; DJX=1; }            //если X вправо
00601     if (!XJR&&!XJU) {DX=1; TimeX=TimeX2; DJX=2; }            //если X быстро вправо
00602     if (!XJL&& XJU) {DX=0; TimeX=TimeX1; DJX=1; }            //если X влево
00603     if (!XJL&&!XJU) {DX=0; TimeX=TimeX2; DJX=2; }            //если X быстро влево
00604 }
00605 //---------------------------------------------- 
00606 void Jz ()                                            // джойстик Z
00607 {
00608     if (!ZJR&& ZJU) {DZ=1; TimeZ=TimeZ1; DJZ=1; }            //если Z вправо
00609     if (!ZJR&&!ZJU) {DZ=1; TimeZ=TimeZ2; DJZ=2; }            //если Z быстро вправо
00610     if (!ZJL&& ZJU) {DZ=0; TimeZ=TimeZ1; DJZ=1; }            //если Z влево
00611     if (!ZJL&&!ZJU) {DZ=0; TimeZ=TimeZ2; DJZ=2; }            //если Z быстро влево
00612 }   
00613 //---------------------------------------------- 
00614 void SpX1Z1 ()
00615 {                                            // нормальная установка скоростей
00616     if (N_T)
00617     { 
00618       TimeX1 = 1000000/(STX*SpeedX1);                             // время в мкс между шагами ШДX если шпиндель не вращается
00619       TimeZ1 = 1000000/(STZ*SpeedZ1);                             // время в мкс между шагами ШДZ если шпиндель не вращается 
00620       TimeX2 = 60000000/(SpeedX2*STX);                          // время в мкс между шагами ШДX при скорости Speed (мм/мин)
00621       TimeZ2 = 60000000/(SpeedZ2*STZ);                          // время в мкс между шагами ШДZ при скорости Speed (мм/мин)
00622    }
00623     else 
00624     {
00625       TimeX1 = 20*time1/(STX*SpeedX1);                             // время в мкс между шагами ШДX на 1 оборот шпинделя *20, а то очень быстро.
00626       TimeZ1 = 20*time1/(STZ*SpeedZ1);                             // время в мкс между шагами ШДZ на 1 оборот шпинделя *20, а то очень быстро.
00627       TimeX2 = 60000000/(SpeedX2*STX);                          // время в мкс между шагами ШДX при скорости Speed (мм/мин)
00628       TimeZ2 = 60000000/(SpeedZ2*STZ);                          // время в мкс между шагами ШДZ при скорости Speed (мм/мин)
00629     }
00630 }
00631 //----------------------------------------------     
00632 //----------------------------------------------     
00633 int main() 
00634 {
00635     InTacho.mode(PullUp);                           //
00636     EncXA.mode(PullUp);
00637     EncXB.mode(PullUp);
00638     EncZA.mode(PullUp);
00639     EncZB.mode(PullUp);
00640     
00641     InTacho.fall(&ITacho);                          //
00642     timer_Tacho.start() ;                           //
00643     Enc_enable ();
00644     SpX1Z1 ();
00645     timerX.attach_us(&TickX, (TimeXMax));               //
00646     timerZ.attach_us(&TickZ, (TimeZMax));               //
00647     timer_Gen.attach_us(&Gen, (Gen1));
00648     ZJU.write ( 1);
00649     STEPX=1;
00650     STEPZ=1;
00651     EStop=0;
00652     //__enable_irq(); 
00653   
00654     lcd.cls();                                      // Clear LCD
00655     lcd.locate(3,1);  lcd.printf("Latche control");
00656     lcd.locate(4,3);  lcd.printf("STM32F411ce");
00657     wait_us (2000000);
00658     lcd.cls();
00659     
00660     K=0; k1=0;
00661 //----------------------------------------------                           
00662 //Основной цикл
00663     
00664  while(1) 
00665  {
00666   if (!Up)
00667   {
00668     wait_us(100000);
00669     if (!Up) 
00670     {
00671     --K; 
00672     if (K<0) K=5;
00673     lcd.cls(); 
00674     wait_us(100000);
00675     }
00676    }
00677   if (!Down)
00678   {
00679     wait_us(100000);
00680     if (!Down) 
00681     {
00682     ++K; 
00683     if (K>5) K=0;
00684     lcd.cls(); wait_us(100000);
00685     }
00686    }
00687   if (AMZ||AMX) Alarm ();
00688     
00689   //--------------------------------------------
00690   if (K==0||K==1)                                          // режим ручного управления и ввода координат
00691  {
00692    if (!XJR||!XJL)                                            // джойстик X
00693      { wait_us (50000);  
00694         if (!XJR&&XJU)  { if (DJX!=1) Jx();}  
00695         if (!XJR&&!XJU) { if (DJX!=2) Jx();}
00696         if (!XJL&&XJU)  { if (DJX!=1) Jx();}  
00697         if (!XJL&&!XJU) { if (DJX!=2) Jx();}
00698      }
00699     else  DJX=0; 
00700    //---------------- 
00701    if (!ZJR||!ZJL)                                            // джойстик X
00702      { wait_us (50000);  
00703         if (!ZJR&&ZJU)  { if (DJZ!=1) Jz();}  
00704         if (!ZJR&&!ZJU) { if (DJZ!=2) Jz();}
00705         if (!ZJL&&ZJU)  { if (DJZ!=1) Jz();}  
00706         if (!ZJL&&!ZJU) { if (DJZ!=2) Jz();}
00707      }
00708      else  DJZ=0;       
00709         
00710     //---------------- 
00711     if (K==0)
00712         {
00713         if ((ZJR&&ZJL&&!ZJU)||(XJR&&XJL&&!XJU))         // ни влево, ни вправо, но нажата кнопка на любом 
00714             {                                             // джойстике - можем выставить лимбы энкодеров
00715             DJX=0; DJZ=0;
00716             Enc_disable();
00717             while(!ZJU||!XJU) {if (ZJU&&XJU) { break;}}
00718             Enc_enable();
00719             }
00720         lcd.locate(10, 0); lcd.printf("vx=%7.2f",SpeedX1); 
00721         lcd.locate(10, 1); lcd.printf("vz=%7.2f",SpeedZ1); 
00722         lcd.locate(10, 2); lcd.printf("VX=%7.0f",(SpeedX2)); 
00723         lcd.locate(10, 3); lcd.printf("VZ=%7.0f",(SpeedZ2));
00724         /*lcd.locate(10, 0); lcd.printf("          "); 
00725         lcd.locate(10, 0); lcd.printf("Tx=%7.2f",TimeX);
00726         lcd.locate(10, 1); lcd.printf("          ");  
00727         lcd.locate(10, 1); lcd.printf("Tz=%7.2f",TimeZ); 
00728         //lcd.locate(10, 2); lcd.printf("VX=%7.2f",TimeX2); */
00729         
00730         lcd.locate(0, 2);  lcd.printf("Man ");
00731         k1=0;
00732         }
00733     if (K==1)                                                 //  режим ввода значений X Z
00734         {
00735         lcd.locate(11, 3); 
00736         lcd.printf("Enter X,Z");
00737         if (!ZJU&&ZJR&&ZJL) bufZ=0; 
00738         if (!XJU&&XJR&&XJL) bufX=0;                           // обнуление координат X или Z
00739         }
00740        
00741    OUTLCD(); 
00742    k1=0;
00743    OUTfrequencY ();
00744    
00745  }                   //end K0 K1                             
00746 //----------------------------------------------
00747 // режим: нарезание резьбы
00748                                    
00749  if (K ==2)     
00750   {
00751     if (!k1) 
00752     {
00753      lcd.locate(0, 0);  lcd.printf("Lx%7.2f",Dlrezby); 
00754      lcd.locate(10, 0); lcd.printf("st%6.2f",Schag); 
00755      lcd.locate(0, 1);  lcd.printf("Hr%7.2f",Hrez); 
00756      lcd.locate(10, 1); lcd.printf("hr%6.2f",hrez); 
00757      lcd.locate(0, 2);  lcd.printf("Sf%7.2f",Saw); 
00758      lcd.locate(10, 2); lcd.printf("Bk%6.2f",Back); 
00759   
00760      if (i==0){ lcd.locate(18, 3); lcd.printf("Lx");}
00761      if (i==1){ lcd.locate(18, 3); lcd.printf("st");}     
00762      if (i==2){ lcd.locate(18, 3); lcd.printf("Hr");}     
00763      if (i==3){ lcd.locate(18, 3); lcd.printf("hr");}     
00764      if (i==4){ lcd.locate(18, 3); lcd.printf("Sf");}      
00765      if (i==5){ lcd.locate(18, 3); lcd.printf("Bk");} 
00766      
00767      
00768      SpX1Z1 ();
00769      ProchodX=100*abs(Hrez)/(100*hrez); 
00770      lcd.locate(11, 3);   lcd.printf(" %d ",ProchodX);           // кол-во проходов по Х 
00771      lcd.locate(14, 3);   lcd.printf("thr");
00772      int Hrez1=100*abs(Hrez);  
00773      int hrez1 = 100*hrez;
00774      int chist1 = Hrez1 % hrez1;                                 // остаток в сотках на чистовой проход
00775      chist = float(float(chist1)/100);                          // остаток в мм на чистовой проход
00776      Nchist=int(chist*float(STZ));                               // кол. шагов глубины чистового прохода
00777      OUTfrequencY ();    
00778      if ( Dlrezby>0 ) DX=1; else DX= 0;                          // направление резьбы 1-левая, 0-правая резьба
00779      if ( Hrez>0 ) DZ=1; else DZ= 0;                             // направление врезания 1-внутренняя, 0-наружная резьба
00780      TimeX=TimeX0;                                               // период между шагами по Х
00781      NSchagX=abs(Dlrezby)*STX;                                   // кол-во шагов по Х на всю длину резьбы
00782      NSaw=Saw*STZ;
00783      Nhrez= abs(hrez)*STZ;
00784      NhSZ=NSaw+Nhrez;
00785      NBack=Back*STZ+(abs(Hrez)*STZ);                  //отвод на Back мм
00786      lcd.locate(11, 3);   lcd.printf(" %d ",ProchodX);
00787     }               //end (!k1)---------------------------
00788         
00789     if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD();  
00790     OUTfrequencY ();
00791        
00792     if ((!XJU||!ZJU)&& (k1==0))
00793     {                                                             // begin (k1==0))
00794       while (!XJU||!ZJU) {wait_us(20000); if(XJU&&ZJU) break;}                // ждём отпускания кнопки 
00795       ++k1;
00796       //------------------------------------------------- 
00797       Enc_disable ();                                               // запрещаем прерывания от энкодеров
00798       lcd.cls(); OUTLCD(); OUTfrequencY ();
00799       for(ProchodX; ProchodX>0; )                                   // цикл по кол-ву проходов
00800        {
00801         lcd.locate(11, 3);   lcd.printf(" %d ",ProchodX);
00802         wanProch ();
00803         lcd.locate(11, 3);   lcd.printf(" %d ",ProchodX); 
00804        //while (1){wait_ms(20); if(!XJU||!ZJU) break;}
00805        }
00806     
00807      if (Nchist!=0) 
00808       {
00809         lcd.locate(11, 3);   lcd.printf("ch");     
00810         Nhrez = Nchist;
00811         ProchodX=1;
00812         wanProch (); 
00813       }
00814      if (!ProchodX)                                                 // отвод резца после окончания резьбы  
00815       {                    
00816        TimeZ=TimeZ2;                                                //период между шагами              
00817        NvarZ=NBack;                                                 // кол-во шагов
00818        lcd.locate(17, 3);   lcd.printf(" BK"); 
00819        lcd.locate(10, 1);   lcd.printf("vz=%7.2f",SpeedZ2);
00820        DZ= !DZ;                                                     // смена направления движения по оси
00821        FlagZ=1;                                                     // данные готовы
00822        while (NvarZ>0){
00823          OUTLCD(); OUTfrequencY ();
00824          if (NvarZ==0) break;}                                      // ждём пока таймер выдаст "NSvar" стробов
00825        FlagZ=0; ++k; K=0;
00826        Enc_enable ();                                               // разрешаем прерывания от энкодеров
00827        lcd.cls();k1=0; 
00828       }       
00829     }                           // end k1=0
00830        SpX1Z1 ();                                                      // установка скоростей по умолчанию
00831   }                            // end (K ==2)
00832 //-------------------------------------------------     
00833 // режим: управления от внешнего компьютера
00834 
00835  if (K==3)                                         
00836  {
00837    if (k1==0)
00838     {
00839      lcd.locate(10, 3);   lcd.printf("  CPU    "); 
00840      OUTLCD(); OUTfrequencY ();
00841      CStX.rise(&CPUX);  CStZ.rise(&CPUZ);                       // разрешаем прерывания от CPU
00842      timerX.attach_us(&TickX, (NULL));                             // запрещаем прерывания от timerX
00843      timerZ.attach_us(&TickZ, (NULL));                             // запрещаем прерывания от timerZ
00844      Enc_disable ();                                            // запрещаем прерывания от энкодеров
00845      ++k1; EStop =1; 
00846     }
00847    if (AMZ||AMX) Alarm ();
00848    if (k1==1) 
00849    {
00850      if ((bufZ1!=bufZ) ||(bufX1!=bufX)) OUTLCD(); 
00851      OUTfrequencY();  
00852      if (!Up||!Down)
00853       {
00854        wait_us(100000);    
00855        if (!Up) 
00856         {
00857         --K; 
00858         while(!Up) {wait_us(100000);} 
00859         }
00860        if (!Down) 
00861         {
00862         ++K; 
00863         while(!Down) {wait_us(100000);} 
00864         }
00865        wait_us(100000);
00866        EStop =0; 
00867        ++k1;
00868       }
00869    }       
00870    if (k1==2) 
00871     {
00872      CStX.rise(NULL); CStZ.rise(NULL);                          // запрещаем прерывания от CPU
00873      timerX.attach_us(&TickX, (TimeX1));                           // разрешаем прерывания от timerX
00874      timerZ.attach_us(&TickZ, (TimeZ1));                           // разрешаем прерывания от timerZ
00875      Enc_enable ();                                             // разрешаем прерывания от энкодеров
00876      EStop =0; k1=0; wait_us(100000);
00877     }
00878     SpX1Z1 ();                                                  // утановка скоростей по умолчанию
00879   }                 //end K3
00880 
00881 
00882 //------------------------------------------------- 
00883 // режим: конус
00884   
00885  if (K==4)                                         
00886   {
00887     double xn; 
00888     double zn;
00889     double TimeX3; 
00890     double TimeZ3;
00891     SpX1Z1 ();
00892     OUTLCD(); 
00893     OUTfrequencY ();     
00894     lcd.locate(10, 2);  lcd.printf("angle %+3.0f",angle);
00895     lcd.locate(10, 3);  lcd.printf("cone  deg");
00896     double anglerad = double(angle)*pi/180;
00897     xn=double (sin(anglerad));
00898     zn=double (cos(anglerad));
00899     TimeX3 = 1000000/(STX*SpeedX1*(xn));
00900     TimeZ3 = 1000000/(STZ*SpeedZ1*(zn));
00901     float speedX= float (SpeedX1*float(xn));
00902     float speedZ= float (SpeedZ1*float(zn));
00903     if (angle==0) speedX=0; 
00904     //lcd.locate(10, 0);  lcd.printf("%-+8.3f",TimeX3);
00905     //lcd.locate(10, 1);  lcd.printf("%-+8.3f",TimeZ3);
00906     lcd.locate(10, 0);  lcd.printf("vx %-+3.4f",speedX);
00907     lcd.locate(10, 1);  lcd.printf("vz %-+3.4f",speedZ);
00908    
00909    if (!XJR||!XJL)                                           // джойстик X
00910      { wait_us (50000);  
00911         if (!XJR&&XJU)  { if (DJX!=1) Jx();}  
00912         if (!XJR&&!XJU) { if (DJX!=2) Jx();}
00913         if (!XJL&&XJU)  { if (DJX!=1) Jx();}  
00914         if (!XJL&&!XJU) { if (DJX!=2) Jx();}
00915         while (!XJR||!XJL) {OUTLCD();  OUTfrequencY ();}
00916      }
00917     else  DJX=0; 
00918     OUTLCD();  OUTfrequencY ();    
00919    if (!ZJR&&ZJU||!ZJL&&ZJU)                                 //джойстик Z
00920     {
00921      if (!ZJR&&ZJU)                                          //если Z вперёд (-Z)
00922       { 
00923        DZ=1;                                                 // 
00924        if (xn>0) DX=0;                                       //X в плюс
00925        if (xn<0) DX=1;                                       //X в минус
00926       } 
00927      if (!ZJL&&ZJU)  
00928       { 
00929        DZ=0;                                                //если Z назад
00930        if (xn>0) DX=1;                                       //X в минус
00931        if (xn<0) DX=0;                                       //X в плюс
00932       }
00933       if (angle==90) {DJZ=0;}
00934        else 
00935         { 
00936         TimeZ = TimeZ3; 
00937         DJZ=1;  
00938         }
00939       if (angle==0) {DJX=0;}                                   //запрет движения по X
00940        else 
00941         { 
00942         TimeX=abs(TimeX3); 
00943         DJX=1;  
00944         }
00945       Enc_disable(); 
00946       while (!ZJR||!ZJL)  {OUTLCD(); OUTfrequencY ();} 
00947     }
00948      else 
00949      {
00950         DJZ=0; DJX=0;
00951         Enc_enable();
00952         SpX1Z1 ();                   // утановка скоростей по умолчанию
00953      }
00954     OUTLCD(); OUTfrequencY ();                                     
00955   }                     //end K4
00956 //------------------------------------------------- 
00957 // режим установки скоростей по X и Z
00958   
00959   if (K==5)                                         
00960    {
00961     if (!XJU||!ZJU) N_T=!N_T;
00962     SpX1Z1 ();                   // утановка скоростей по умолчанию
00963     if (N_T)
00964     {
00965     lcd.locate(0, 0); lcd.printf("   mm/s   "); 
00966     }
00967     else
00968     {
00969     lcd.locate(0, 0); lcd.printf("   mm/n   "); 
00970     }
00971     lcd.locate(10, 0); lcd.printf("  mm/min  ");
00972     lcd.locate(0, 1);  lcd.printf("vx=%6.2f",SpeedX1); 
00973     lcd.locate(10, 1); lcd.printf("Vx=%7.2f",SpeedX2); 
00974     lcd.locate(0, 2);  lcd.printf("vz=%6.2f",SpeedZ1); 
00975     lcd.locate(10, 2); lcd.printf("Vz=%7.2f",SpeedZ2); 
00976   
00977     if (i==0){ lcd.locate(12, 3); lcd.printf(" vx ");}
00978     if (i==1){ lcd.locate(12, 3); lcd.printf(" Vx ");}     
00979     if (i==2){ lcd.locate(12, 3); lcd.printf(" vz ");}     
00980     if (i==3){ lcd.locate(12, 3); lcd.printf(" Vz ");}     
00981     OUTfrequencY();
00982      
00983    }                //end K5
00984 //------------------------------------------------- 
00985     //SpX1Z1 ();      
00986  }                  //end while(1)
00987 }                   //end main