#include <avr/pgmspace.h>
#include <util/delay.h>
//////////////////////////////////
// ***** ПАРАМЕТРЫ ЖЕЛЕЗА ***** //

#define ENC_LINE_PER_REV     2000     // Кол-во линий энкодера на 1 оборот шпинделя
#define MOTOR_Z_STEP_PER_REV 200      // Кол-во шагов на оборот винта Z, продольная
#define SCREW_Z              200      // Шаг продольного винта Z в сотках, 2мм
#define McSTEP_Z             4        // Микрошаг, ось Z, продольная
#define MOTOR_X_STEP_PER_REV 400      // Кол-во шагов на оборот винта X, поперечная
#define SCREW_X              125      // Шаг поперечного винта X в сотках, 1.0мм
#define REBOUND_X            200      // Отскок резца в микрошагах, для авторезанья, должен быть больше люфта поперечки
#define REBOUND_Z            200      // Отскок резца в микрошагах, для авторезанья, должен быть больше люфта продольной
#define McSTEP_X             4        // Микрошаг, ось X, поперечная
//
#define THRD_ACCEL           15       // К.деления с которого будем ускоряться на Резьбах, Accel+Ks должен быть < 255
#define FEED_ACCEL          100 //100       // Жесткость разгона на подачах, бОльше значение - короче разгон.
//
#define MIN_FEED             1        // Желаемая минимальная подача  в сотках/оборот, 0.02мм/об
#define MAX_FEED             30       // Желаемая максимальная подача в сотках/оборот, 0.25мм/об
#define MIN_aFEED            10 //20       // Желаемая минимальная подача  в mm/минуту, 20мм/мин
#define MAX_aFEED            160 //120 //500      // Желаемая максимальная подача в mm/минуту, 400мм/мин

// Ускоренные перемещения                              
#define MAX_RAPID_MOTION_Z     27                       // Меньше - бОльшая конечная скорость           //16000000/32/((25+1)*2)/800*60=721rpm
#define MAX_RAPID_MOTION_X     20 //35                       // Меньше - бОльшая конечная скорость           //16000000/32/((75+1)*2)/800*60=~246rpm
#define MIN_RAPID_MOTION    (MAX_RAPID_MOTION_Z + 100)   // Больше - мЕньшая начальная скорость, max 255 //16000000/32/((150+25+1)*2)/800*60=107rpm !!!!ВЛИЯЕТ на минимальное расстояние между упорами
#define REPEAt              (McSTEP_Z * 1)              // Кол-во повторов для постоянной скорости в пределах полного шага    
                                                        // Длительность разгона = 150/2*REPEAT(4)/Microstep(4) = 75 полных шагов цикл ускорения
// Ручной энкодер (100 линий)
#define HC_SCALE_1           1        // 1-e положение, масштаб = 1сотка/тик = 1мм/оборот
#define HC_SCALE_10          10       // 2-e положение, масштаб = 10соток/тик = 10мм/оборот
#define HC_START_SPEED_1     500 //250      // старт РГИ, 250000/(250+1)/800*60/2 = 37rpm
#define HC_MAX_SPEED_1       300 //150      // максимум скорости РГИ, 250000/(150+1)/800*60/2 = 62rpm
#define HC_START_SPEED_10    150      // старт РГИ, 250000/(150+1)/800*60/2 = 62rpm
#define HC_MAX_SPEED_10      35       // максимум скорости РГИ, 250000/(23+1)/800*60/2 = 391rpm
#define HC_X_DIR             1        // 1-поперечка по часовой, 0-против

#define PP_X                 1        // поперечная подача на радиус/диаметр (1-на диаметр , 0-на радиус)
#define PASS_FINISH          3        // для режима "Резьба": Количество чистовых проходов, после автоцикла, по умолчанию

//////////////////////////////////////////////////////////////////////////////////////////////////
#define a  (uint32_t)(ENC_LINE_PER_REV / ((float)MOTOR_Z_STEP_PER_REV * McSTEP_Z * MIN_FEED / SCREW_Z) /2 +0.5)
static_assert(a <= 255, "Неверно задано значение MIN_FEED");
#define b  (uint32_t)(ENC_LINE_PER_REV / ((float)MOTOR_Z_STEP_PER_REV * McSTEP_Z * MAX_FEED / SCREW_Z) /2 + 0.5)
static_assert(b > 1, "Неверно задано значение MAX_FEED");
#define c  250000 / ((uint32_t)MIN_aFEED * MOTOR_Z_STEP_PER_REV * McSTEP_Z / ((uint32_t)60 * SCREW_Z / 100) * 2) -1
static_assert(c <= 65535, "Неверно задано значение MIN_aFEED");
#define d  250000 / ((uint32_t)MAX_aFEED * MOTOR_Z_STEP_PER_REV * McSTEP_Z / ((uint32_t)60 * SCREW_Z / 100) * 2) -1
static_assert(d > 1, "Неверно задано значение MAX_aFEED");

#define e  (uint32_t)(ENC_LINE_PER_REV / ((float)MOTOR_X_STEP_PER_REV * McSTEP_X * MIN_FEED / SCREW_X) /2 +0.5)
static_assert(e <= 255, "Неверно задано значение MIN_FEED");
#define f  (uint32_t)(ENC_LINE_PER_REV / ((float)MOTOR_X_STEP_PER_REV * McSTEP_X * MAX_FEED / SCREW_X) /2 + 0.5)
static_assert(f > 1, "Неверно задано значение MAX_FEED");
#define g  250000 / ((uint32_t)MIN_aFEED * MOTOR_X_STEP_PER_REV * McSTEP_X / ((uint32_t)60 * SCREW_X / 100) * 2) -1
static_assert(g <= 65535, "Неверно задано значение MIN_aFEED");
#define h  250000 / ((uint32_t)MAX_aFEED * MOTOR_X_STEP_PER_REV * McSTEP_X / ((uint32_t)60 * SCREW_X / 100) * 2) -1
static_assert(h > 1, "Неверно задано значение MAX_aFEED");
//////////////////////////////////////////////////////////

#include "Formula.h"
volatile long Rev_Count;
volatile long Rev_Count_F;

// ***** MY CONSTANT *****
#define CW               0
#define CCW              1
#define ON               1
#define OFF              0


// ***** LCD *****
#include <LiquidCrystal.h>
LiquidCrystal lcd(8, 9, 10, 11, 12, 13);
char LCD_Row_1[21];
char LCD_Row_2[21];
char LCD_Row_3[21];
char LCD_Row_4[21];


byte strelka_vlevo[8] = 
{
  0b00000,
  0b00000,
  0b00100,
  0b01000,
  0b11111,
  0b01000,
  0b00100,
  0b00000,
};
byte strelka_vpravo[8] = 
{  
  0b00100,
  0b00010,
  0b11111,
  0b00010,
  0b00100,
  0b00000,
  0b00000,
  0b00000,
};
byte strelka_vverh[8] = 
{
  0b00100,
  0b01110,
  0b10101,
  0b00100,
  0b00100,
  0b00000,
  0b00000,
  0b00000,
};
byte strelka_vniz[8] = 
{
  0b00000,
  0b00000,
  0b00100,
  0b00100,
  0b10101,
  0b01110,
  0b00100,
  0b00000,
};
byte znak_diametr[8] = 
{
  0b00001,
  0b01110,
  0b10011,
  0b10101,
  0b11001,
  0b01110,
  0b10000,
  0b00000
};
byte znak_gradus[8] = 
{
  0b01100,
  0b10010,
  0b10010,
  0b01100,
  0b00000,
  0b00000,
  0b00000,
  0b00000
};

#define Beeper_Init()          DDRH = B01100010;\
                               PORTH = B10011101    // LCD-H5,H6 Buzzer-PH1_Pin16
#define Beeper_On()            PORTH &= ~(1<<1)     // Pin16 0
#define Beeper_Off()           PORTH |= (1<<1)      // Pin16 1



// ***** Stepper Motor *****
#define Motor_Init()           DDRL = B11111011;\
                               PORTL = B0000100

#define Motor_Z_SetPulse()     PORTL &= ~(1<<0)     // Pin49 0
#define Motor_Z_RemovePulse()  PORTL |= (1<<0)      // Pin49 1
#define Motor_Z_InvertPulse()  PORTL ^= (1<<0)      // Pin49
#define Read_Z_State           (PINL & (1<<0))

#define Motor_X_SetPulse()     PORTL &= ~(1<<1)     // Pin48 0
#define Motor_X_RemovePulse()  PORTL |= (1<<1)      // Pin48 1
#define Motor_X_InvertPulse()  PORTL ^= (1<<1)      // Pin48
#define Read_X_State           (PINL & (1<<1))

#define Motor_Z_CW()           PORTL &= ~(1<<6)    // Pin43 0
#define Motor_Z_CCW()          PORTL |= (1<<6)     // Pin43 1

#define Motor_X_CW()           PORTL &= ~(1<<5)    // Pin44 0
#define Motor_X_CCW()          PORTL |= (1<<5)     // Pin44 1

#define Motor_Z_Enable()   do {PORTL |= (1<<4); _delay_ms(250);} while(0)   // Pin45 1   попробуйте задержку увеличить: _delay_ms(120); до 500, может тупо драйвер Ena не успевает отрабатывать
#define Motor_Z_Disable()     {PORTL &= ~(1<<4); _delay_ms(100);}           // Pin45 0

#define Read_Z_Ena_State       (PINL & (1<<4))

#define Motor_X_Enable()   do {PORTL |= (1<<3); _delay_ms(120);} while(0)   // Pin46 1  попробуйте задержку увеличить: _delay_ms(120); до 500, может тупо драйвер Ena не успевает отрабатывать
#define Motor_X_Disable()     {PORTL &= ~(1<<3); _delay_ms(100);}           // Pin46 0
#define Read_X_Ena_State       (PINL & (1<<3))


// ***** Encoder *****
#define ENC_TICK              (ENC_LINE_PER_REV * 2)    // Рабочее кол-во импульсов
#define Encoder_Init()         DDRD = B00000000;\
                               PORTD = B11111111        // подтяжка PIN_21, 20, 19, 18
#define Enc_Read              (PIND & (1<<1))
#define Enc_Ch_A              (PIND & (1<<0))
#define Enc_Ch_B              (PIND & (1<<1))
 


// ***** Hand_Coder *****            // Z/X: Input-E4,E5, подтяжка-E4,E5, X1/X10: Input-J0,J1, подтяжка-J0,J1.
#define Hand_Init()            DDRE = B00000000;\
                               PORTE = B11111111;\
                               DDRJ = B00000000;\
                               PORTJ = B11111111
                                     
#define Hand_Ch_A             (PIND & (1<<2))
#define Hand_Ch_B             (PIND & (1<<3))

#define Hand_Axis_Read        (PINE & B00110000)       // E4,E5
byte Hand_Axis_Old = 0;

#define Hand_Scale_Read        (PINJ & B00000011)      // J0,J1
byte Hand_Scale_Old = 0;


//***** Limit Buttons & LEDs *****
#define Limit_Init()           DDRA = B10101010;\
                               PORTA = B01010101    // IN-A0,A2,A4,A6, OUT-A1,A3,A5,A7, подтяжка

#define Limit_Buttons_Read    (PINA & B01010101)    // PA0 Pin22, PA2 Pin24, PA4 Pin26, PA6 Pin28.
byte Limit_Button_Old = 0;

#define Limit_Rear_LED_On()    PORTA &= ~(1<<1)     // PA1, Pin23 0
#define Limit_Rear_LED_Off()   PORTA |= (1<<1)      // PA1, Pin23 1
#define Limit_Front_LED_On()   PORTA &= ~(1<<3)     // PA3, Pin25 0
#define Limit_Front_LED_Off()  PORTA |= (1<<3)      // PA3, Pin25 1
#define Limit_Right_LED_On()   PORTA &= ~(1<<5)     // PA5, Pin27 0
#define Limit_Right_LED_Off()  PORTA |= (1<<5)      // PA5, Pin27 1
#define Limit_Left_LED_On()    PORTA &= ~(1<<7)     // PA7, Pin29 0
#define Limit_Left_LED_Off()   PORTA |= (1<<7)      // PA7, Pin29 1

#define Limit_Pos_Max          1073741824
#define Limit_Pos_Min         -1073741824


//////////////////
#define Menu_Buttons_Init()    DDRF = B00000000;\
                               PORTF = B11111111;     

#define Buttons_Read           (PINF & B00001111)    // Pin_A0 PF0, Pin_A1 PF1, Pin_A2 PF2, Pin_A3 PF3, Pin_A4 PF4.
byte Button_Old = 0;

#define Button_Sel_Read        (PINF & B00010000)    // Pin_A4 PF4
byte Button_Sel_Old = 0;
bool key_sel_flag = false;

//////////////////
#define Joy_Init()             DDRK = B00000000;\
                               PORTK = B11111111;    // подтяжка PIN_A8, A9, A10, A11, A12 // Submode Sw: A13, A14, A15

#define Joy_Read              (PINK & B00001111)     // PK0 PK1 PK2 PK3
#define Button_Rapid          (PINK & B00010000)     // PK4
byte Joy_Old = 0;

////////////////////
#define Submode_Read          (PINK & B11100000)     // PK5 PK6 PK7
byte Submode_Old = 0;


// ***** Mode *****
#define Mode_Switch_Init()     DDRC = B00000000;\
                               PORTC = B11111111;        // подтяжка PORT_A, ОБЯЗАТЕЛЬНА! внешняя подтяжка к +5 через 1К резисторы
#define Mode_Read             (PINC & B11111111)
byte Mode_Old = 0;


enum Mode
{
  Mode_Thread = 1,
  Mode_Feed,
  Mode_aFeed,
  Mode_Cone_L,
  Mode_Cone_R,
  Mode_Reserve,
  Mode_Sphere,
  Mode_Divider
};

enum Sub_Mode_Thread
{
  Sub_Mode_Thread_Int = 1,
  Sub_Mode_Thread_Man,
  Sub_Mode_Thread_Ext,
};

enum Sub_Mode_Feed
{
  Sub_Mode_Feed_Int = 1,
  Sub_Mode_Feed_Man,
  Sub_Mode_Feed_Ext,
};

enum Sub_Mode_aFeed
{
  Sub_Mode_aFeed_Int = 1,
  Sub_Mode_aFeed_Man,
  Sub_Mode_aFeed_Ext,
};

enum Sub_Mode_Cone
{
  Sub_Mode_Cone_Int = 1,
  Sub_Mode_Cone_Man,
  Sub_Mode_Cone_Ext,
};

enum Sub_Mode_Sphere
{
  Sub_Mode_Sphere_Int = 1,
  Sub_Mode_Sphere_Man,
  Sub_Mode_Sphere_Ext,
};


//***** Ускоренное перемещение *****
#define Timer2_Init()          TCCR2A = (1<<WGM21);\
                               TCCR2B = (1<<CS20)|(1<<CS21); // 16MHz/32 = 500kHz

//***** РГИ перемещение *****
#define Timer3_Init()          TCCR3A = 0;\
                               TCCR3B = (1<<WGM32)|(1<<CS30)|(1<<CS31); // 16MHz/32 = 250kHz

// ***** aFeed *****
#define Timer4_Init()          TCCR4A = 0;\
                               TCCR4B = (1<<WGM42)|(1<<CS40)|(1<<CS41); // 16MHz/32 = 250kHz

// ***** Feed *****
#define Timer5_Init()          TCCR5A = 0;\
                               TCCR5B = (1<<WGM52) | (1<<CS52) | (1<<CS51) | (1<<CS50)


// ***** Cone *****
struct cone_info_type
{
  byte Cs_Div;
  int  Cm_Div;
  char Cone_Print[6];
};
const cone_info_type Cone_Info[] =
{ 
   {  Cs_Div_AB_KM0,  Cm_Div_AB_KM0, " KM0" },        
   {  Cs_Div_AB_KM1,  Cm_Div_AB_KM1, " KM1" },        
   {  Cs_Div_AB_KM2,  Cm_Div_AB_KM2, " KM2" },        
   {  Cs_Div_AB_KM3,  Cm_Div_AB_KM3, " KM3" },        
   {  Cs_Div_AB_KM4,  Cm_Div_AB_KM4, " KM4" },        
   {  Cs_Div_AB_KM5,  Cm_Div_AB_KM5, " KM5" },        
   {  Cs_Div_AB_KM6,  Cm_Div_AB_KM6, " KM6" },        
   {  Cs_Div_AB_1_4,  Cm_Div_AB_1_4, " 1:4" },        
   {  Cs_Div_AB_1_5,  Cm_Div_AB_1_5, " 1:5" },        
   {  Cs_Div_AB_1_7,  Cm_Div_AB_1_7, " 1:7" },        
   {  Cs_Div_AB_1_10,  Cm_Div_AB_1_10, "1:10" },        
   {  Cs_Div_AB_1_16,  Cm_Div_AB_1_16, "1:16" },        
   {  Cs_Div_AB_1_20,  Cm_Div_AB_1_20, "1:20" },        
   {  Cs_Div_AB_1_24,  Cm_Div_AB_1_24, "1:24" },        
   {  Cs_Div_AB_1_30,  Cm_Div_AB_1_30, "1:30" },        
   {  Cs_Div_AB_1_50,  Cm_Div_AB_1_50, "1:50" },        
   {  Cs_Div_AB_7_24,  Cm_Div_AB_7_24, "7:24" },        
   {  Cs_Div_AB_7_64,  Cm_Div_AB_7_64, "7:64" }, 
   
   {  Cs_Div_DEG_60,  Cm_Div_DEG_60, " 60\5" },        
   {  Cs_Div_DEG_59,  Cm_Div_DEG_59, " 59\5" },       
   {  Cs_Div_DEG_58,  Cm_Div_DEG_58, " 58\5" },       
   {  Cs_Div_DEG_57,  Cm_Div_DEG_57, " 57\5" },       
   {  Cs_Div_DEG_56,  Cm_Div_DEG_56, " 56\5" },       
   {  Cs_Div_DEG_55,  Cm_Div_DEG_55, " 55\5" },       
   {  Cs_Div_DEG_54,  Cm_Div_DEG_54, " 54\5" },       
   {  Cs_Div_DEG_53,  Cm_Div_DEG_53, " 53\5" },       
   {  Cs_Div_DEG_52,  Cm_Div_DEG_52, " 52\5" },       
   {  Cs_Div_DEG_51,  Cm_Div_DEG_51, " 51\5" },       
   {  Cs_Div_DEG_50,  Cm_Div_DEG_50, " 50\5" },       
   {  Cs_Div_DEG_49,  Cm_Div_DEG_49, " 49\5" },       
   {  Cs_Div_DEG_48,  Cm_Div_DEG_48, " 48\5" },       
   {  Cs_Div_DEG_47,  Cm_Div_DEG_47, " 47\5" },       
   {  Cs_Div_DEG_46,  Cm_Div_DEG_46, " 46\5" },       
   {  Cs_Div_DEG_45,  Cm_Div_DEG_45, " 45\5" },       
   {  Cs_Div_DEG_44,  Cm_Div_DEG_44, " 44\5" },       
   {  Cs_Div_DEG_43,  Cm_Div_DEG_43, " 43\5" },       
   {  Cs_Div_DEG_42,  Cm_Div_DEG_42, " 42\5" },       
   {  Cs_Div_DEG_41,  Cm_Div_DEG_41, " 41\5" },       
   {  Cs_Div_DEG_40,  Cm_Div_DEG_40, " 40\5" },       
   {  Cs_Div_DEG_39,  Cm_Div_DEG_39, " 39\5" },       
   {  Cs_Div_DEG_38,  Cm_Div_DEG_38, " 38\5" },       
   {  Cs_Div_DEG_37,  Cm_Div_DEG_37, " 37\5" },       
   {  Cs_Div_DEG_36,  Cm_Div_DEG_36, " 36\5" },       
   {  Cs_Div_DEG_35,  Cm_Div_DEG_35, " 35\5" },       
   {  Cs_Div_DEG_34,  Cm_Div_DEG_34, " 34\5" },       
   {  Cs_Div_DEG_33,  Cm_Div_DEG_33, " 33\5" },       
   {  Cs_Div_DEG_32,  Cm_Div_DEG_32, " 32\5" },       
   {  Cs_Div_DEG_31,  Cm_Div_DEG_31, " 31\5" },       
   {  Cs_Div_DEG_30,  Cm_Div_DEG_30, " 30\5" },       
   {  Cs_Div_DEG_29,  Cm_Div_DEG_29, " 29\5" },       
   {  Cs_Div_DEG_28,  Cm_Div_DEG_28, " 28\5" },       
   {  Cs_Div_DEG_27,  Cm_Div_DEG_27, " 27\5" },       
   {  Cs_Div_DEG_26,  Cm_Div_DEG_26, " 26\5" },       
   {  Cs_Div_DEG_25,  Cm_Div_DEG_25, " 25\5" },       
   {  Cs_Div_DEG_24,  Cm_Div_DEG_24, " 24\5" },       
   {  Cs_Div_DEG_23,  Cm_Div_DEG_23, " 23\5" },       
   {  Cs_Div_DEG_22,  Cm_Div_DEG_22, " 22\5" },       
   {  Cs_Div_DEG_21,  Cm_Div_DEG_21, " 21\5" },       
   {  Cs_Div_DEG_20,  Cm_Div_DEG_20, " 20\5" },       
   {  Cs_Div_DEG_19,  Cm_Div_DEG_19, " 19\5" },       
   {  Cs_Div_DEG_18,  Cm_Div_DEG_18, " 18\5" },       
   {  Cs_Div_DEG_17,  Cm_Div_DEG_17, " 17\5" },       
   {  Cs_Div_DEG_16,  Cm_Div_DEG_16, " 16\5" },       
   {  Cs_Div_DEG_15,  Cm_Div_DEG_15, " 15\5" },       
   {  Cs_Div_DEG_14,  Cm_Div_DEG_14, " 14\5" },       
   {  Cs_Div_DEG_13,  Cm_Div_DEG_13, " 13\5" },       
   {  Cs_Div_DEG_12,  Cm_Div_DEG_12, " 12\5" },       
   {  Cs_Div_DEG_11,  Cm_Div_DEG_11, " 11\5" },       
   {  Cs_Div_DEG_10,  Cm_Div_DEG_10, " 10\5" },       
   {  Cs_Div_DEG_09,  Cm_Div_DEG_09, "  9\5" },       
   {  Cs_Div_DEG_08,  Cm_Div_DEG_08, "  8\5" },       
   {  Cs_Div_DEG_07,  Cm_Div_DEG_07, "  7\5" },       
   {  Cs_Div_DEG_06,  Cm_Div_DEG_06, "  6\5" },       
   {  Cs_Div_DEG_05,  Cm_Div_DEG_05, "  5\5" },       
   {  Cs_Div_DEG_04,  Cm_Div_DEG_04, "  4\5" },       
   {  Cs_Div_DEG_03,  Cm_Div_DEG_03, "  3\5" },       
   {  Cs_Div_DEG_02,  Cm_Div_DEG_02, "  2\5" },       
   {  Cs_Div_DEG_01,  Cm_Div_DEG_01, "  1\5" },             
};
#define TOTAL_CONE (sizeof(Cone_Info) / sizeof(Cone_Info[0]))


// ***** Threads *****
struct thread_info_type
{
  byte Ks_Div_Z;
  int  Km_Div_Z;
  byte Ks_Div_X;
  int  Km_Div_X;
  char Thread_Print[7];
  float Step;
  byte Pass;
  char Limit_Print[8];
};
const thread_info_type Thread_Info[] =
{
// метрическая                  
   { Ks_Div_Z_0200,    Km_Div_Z_0200,   Ks_Div_X_0200,    Km_Div_X_0200,   "0.20", 0.200,  4, "980" },                       
   { Ks_Div_Z_0250,    Km_Div_Z_0250,   Ks_Div_X_0250,    Km_Div_X_0250,   "0.25", 0.250,  4, "840" },                        
   { Ks_Div_Z_0300,    Km_Div_Z_0300,   Ks_Div_X_0300,    Km_Div_X_0300,   "0.30", 0.300,  5, "740" },                        
   { Ks_Div_Z_0350,    Km_Div_Z_0350,   Ks_Div_X_0350,    Km_Div_X_0350,   "0.35", 0.350,  5, "680" },                        
   { Ks_Div_Z_0400,    Km_Div_Z_0400,   Ks_Div_X_0400,    Km_Div_X_0400,   "0.40", 0.400,  5, "640" },                        
   { Ks_Div_Z_0450,    Km_Div_Z_0450,   Ks_Div_X_0450,    Km_Div_X_0450,   "0.45", 0.450,  5, "600" },                        
   { Ks_Div_Z_0500,    Km_Div_Z_0500,   Ks_Div_X_0500,    Km_Div_X_0500,   "0.50", 0.500,  6, "560" },                        
   { Ks_Div_Z_0600,    Km_Div_Z_0600,   Ks_Div_X_0600,    Km_Div_X_0600,   "0.60", 0.600,  6, "520" },                        
   { Ks_Div_Z_0700,    Km_Div_Z_0700,   Ks_Div_X_0700,    Km_Div_X_0700,   "0.70", 0.700,  7, "480" },                        
   { Ks_Div_Z_0750,    Km_Div_Z_0750,   Ks_Div_X_0750,    Km_Div_X_0750,   "0.75", 0.750,  7, "460" },                        
   { Ks_Div_Z_0800,    Km_Div_Z_0800,   Ks_Div_X_0800,    Km_Div_X_0800,   "0.80", 0.800,  7, "460" },                        
   { Ks_Div_Z_1000,    Km_Div_Z_1000,   Ks_Div_X_1000,    Km_Div_X_1000,   "1.00", 1.000,  8, "420" },                        
   { Ks_Div_Z_1250,    Km_Div_Z_1250,   Ks_Div_X_1250,    Km_Div_X_1250,   "1.25", 1.250,  9, "400" },                        
   { Ks_Div_Z_1500,    Km_Div_Z_1500,   Ks_Div_X_1500,    Km_Div_X_1500,   "1.50", 1.500, 11, "380" },                        
   { Ks_Div_Z_1750,    Km_Div_Z_1750,   Ks_Div_X_1750,    Km_Div_X_1750,   "1.75", 1.750, 12, "360" },                        
   { Ks_Div_Z_2000,    Km_Div_Z_2000,   Ks_Div_X_2000,    Km_Div_X_2000,   "2.00", 2.000, 13, "360" },                        
   { Ks_Div_Z_2500,    Km_Div_Z_2500,   Ks_Div_X_2500,    Km_Div_X_2500,   "2.50", 2.500, 16, "80" },                       
   { Ks_Div_Z_3000,    Km_Div_Z_3000,   Ks_Div_X_3000,    Km_Div_X_3000,   "3.00", 3.000, 18, "80" },                       
   { Ks_Div_Z_4000,    Km_Div_Z_4000,   Ks_Div_X_4000,    Km_Div_X_4000,   "4.00", 4.000, 23, "80" },                     
// дюймовая                       
   { Ks_Div_Z_0318,    Km_Div_Z_0318,   Ks_Div_X_0318,    Km_Div_X_0318,   " 80", 0.318,  5, "720" },                       
   { Ks_Div_Z_0353,    Km_Div_Z_0353,   Ks_Div_X_0353,    Km_Div_X_0353,   " 72", 0.353,  5, "680" },                       
   { Ks_Div_Z_0397,    Km_Div_Z_0397,   Ks_Div_X_0397,    Km_Div_X_0397,   " 64", 0.397,  5, "640" },                       
   { Ks_Div_Z_0423,    Km_Div_Z_0423,   Ks_Div_X_0423,    Km_Div_X_0423,   " 60", 0.423,  5, "620" },                       
   { Ks_Div_Z_0454,    Km_Div_Z_0454,   Ks_Div_X_0454,    Km_Div_X_0454,   " 56", 0.454,  5, "580" },                       
   { Ks_Div_Z_0529,    Km_Div_Z_0529,   Ks_Div_X_0529,    Km_Div_X_0529,   " 48", 0.529,  6, "540" },                       
   { Ks_Div_Z_0577,    Km_Div_Z_0577,   Ks_Div_X_0577,    Km_Div_X_0577,   " 44", 0.577,  6, "520" },                       
   { Ks_Div_Z_0635,    Km_Div_Z_0635,   Ks_Div_X_0635,    Km_Div_X_0635,   " 40", 0.635,  6, "500" },                       
   { Ks_Div_Z_0706,    Km_Div_Z_0706,   Ks_Div_X_0706,    Km_Div_X_0706,   " 36", 0.706,  7, "480" },                       
   { Ks_Div_Z_0794,    Km_Div_Z_0794,   Ks_Div_X_0794,    Km_Div_X_0794,   " 32", 0.794,  7, "460" },                       
   { Ks_Div_Z_0907,    Km_Div_Z_0907,   Ks_Div_X_0907,    Km_Div_X_0907,   " 28", 0.907,  8, "440" },                       
   { Ks_Div_Z_0941,    Km_Div_Z_0941,   Ks_Div_X_0941,    Km_Div_X_0941,   " 27", 0.941,  8, "420" },                       
   { Ks_Div_Z_0977,    Km_Div_Z_0977,   Ks_Div_X_0977,    Km_Div_X_0977,   " 26", 0.977,  8, "420" },                       
   { Ks_Div_Z_1058,    Km_Div_Z_1058,   Ks_Div_X_1058,    Km_Div_X_1058,   " 24", 1.580,  8, "420" },                       
   { Ks_Div_Z_1155,    Km_Div_Z_1155,   Ks_Div_X_1155,    Km_Div_X_1155,   " 22", 1.155,  9, "400" },                       
   { Ks_Div_Z_1270,    Km_Div_Z_1270,   Ks_Div_X_1270,    Km_Div_X_1270,   " 20", 1.270,  9, "400" },                       
   { Ks_Div_Z_1337,    Km_Div_Z_1337,   Ks_Div_X_1337,    Km_Div_X_1337,   " 19", 1.337, 10, "380" },                       
   { Ks_Div_Z_1411,    Km_Div_Z_1411,   Ks_Div_X_1411,    Km_Div_X_1411,   " 18", 1.411, 10, "380" },                       
   { Ks_Div_Z_1588,    Km_Div_Z_1588,   Ks_Div_X_1588,    Km_Div_X_1588,   " 16", 1.588, 11, "360" },                       
   { Ks_Div_Z_1814,    Km_Div_Z_1814,   Ks_Div_X_1814,    Km_Div_X_1814,   " 14", 1.814, 12, "360" },                       
   { Ks_Div_Z_1954,    Km_Div_Z_1954,   Ks_Div_X_1954,    Km_Div_X_1954,   " 13", 1.954, 13, "360" },                       
   { Ks_Div_Z_2117,    Km_Div_Z_2117,   Ks_Div_X_2117,    Km_Div_X_2117,   " 12", 2.117, 14, "340" },                       
   { Ks_Div_Z_2309,    Km_Div_Z_2309,   Ks_Div_X_2309,    Km_Div_X_2309,   " 11", 2.309, 15, "80" },                        
   { Ks_Div_Z_2540,    Km_Div_Z_2540,   Ks_Div_X_2540,    Km_Div_X_2540,   " 10", 2.540, 16, "80" },                        
   { Ks_Div_Z_2822,    Km_Div_Z_2822,   Ks_Div_X_2822,    Km_Div_X_2822,   "  9", 2.822, 17, "80" },                        
   { Ks_Div_Z_3175,    Km_Div_Z_3175,   Ks_Div_X_3175,    Km_Div_X_3175,   "  8", 3.175, 19, "80" },                        
   { Ks_Div_Z_3629,    Km_Div_Z_3629,   Ks_Div_X_3629,    Km_Div_X_3629,   "  7", 3.629, 21, "80" },                        
   { Ks_Div_Z_4233,    Km_Div_Z_4233,   Ks_Div_X_4233,    Km_Div_X_4233,   "  6", 4.233, 24, "80" },                                     
};
#define TOTAL_THREADS (sizeof(Thread_Info) / sizeof(Thread_Info[0]))
//#define PASS_FINISH   3 // THRD_PS_FN ???

#define Thrd_Accel_Err Thread_Info[0].Ks_Div_Z                 // неверно задано ускорение
//static_assert(Thrd_Accel_Err + THRD_ACCEL <= 255, "Неверно задано значение THRD_ACCEL");


// ***** Interrupts *****
#define INT0_Init()               EICRA |= (1<<ISC00)
#define INT2_Init()               EICRA |= (1<<ISC20)
#define Enable_INT0()             EIMSK |= (1<<INT0)
#define Disable_INT0()            EIMSK &= ~(1<<INT0)

#define Ena_INT_Hcoder()      do {EIFR = (1<<INTF2); EIMSK |= (1<<INT2);} while(0)
#define Disa_INT_Hcoder()         EIMSK &= ~(1<<INT2)

#define Enable_INT_OCR2A()    do {TCNT2 = 0; TIFR2 = (1<<OCF2A); TIMSK2 = (1<<OCIE2A);} while(0)
#define Disable_INT_OCR2A()       TIMSK2 &= ~(1<<OCIE2A)

#define Enable_INT_OCR2B()    do {TCNT2 = 0; TIFR2 = (1<<OCF2B); TIMSK2 = (1<<OCIE2B);} while(0)
#define Disable_INT_OCR2B()       TIMSK2 &= ~(1<<OCIE2B)

#define Enable_INT_OCR3A()    do {TCNT3 = 0; TIFR3 = (1<<OCF3A); TIMSK3 = (1<<OCIE3A);} while(0)
#define Disable_INT_OCR3A()       TIMSK3 &= ~(1<<OCIE3A)

#define Enable_INT_OCR3B()    do {TCNT3 = 0; TIFR3 = (1<<OCF3B); TIMSK3 = (1<<OCIE3B);} while(0)
#define Disable_INT_OCR3B()       TIMSK3 &= ~(1<<OCIE3B)

#define Enable_INT_OCR4A()    do {TCNT4 = 0; TIFR4 = (1<<OCF4A); TIMSK4 = (1<<OCIE4A);} while(0)
#define Disable_INT_OCR4A()       TIMSK4 &= ~(1<<OCIE4A)

#define Enable_INT_OCR4B()    do {TCNT4 = 0; TIFR4 = (1<<OCF4B); TIMSK4 = (1<<OCIE4B);} while(0)
#define Disable_INT_OCR4B()       TIMSK4 &= ~(1<<OCIE4B)

#define Enable_INT_OCR5A()    do {TCNT5 = 0; TIFR5 = (1<<OCF5A); TIMSK5 = (1<<OCIE5A);} while(0)
#define Disable_INT_OCR5A()       TIMSK5 &= ~(1<<OCIE5A)

#define Enable_INT_OCR5B()    do {TCNT5 = 0; TIFR5 = (1<<OCF5B); TIMSK5 = (1<<OCIE5B);} while(0)
#define Disable_INT_OCR5B()       TIMSK5 &= ~(1<<OCIE5B)

////////////////////////////////////////////////////////////
#define Ena_INT_Thrd()        do {Disable_INT_OCR2A();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4A();\
                                  Disable_INT_OCR4B();\
                                  Disable_INT_OCR5A();\
                                  Disable_INT_OCR5B();\
                                  Enable_INT0();} while(0)

#define Ena_INT_Z_Feed()      do {Disable_INT0();\
                                  Disable_INT_OCR2A();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4A();\
                                  Disable_INT_OCR4B();\
                                  Disable_INT_OCR5B();\
                                  Enable_INT_OCR5A();} while(0)

#define Ena_INT_Z_aFeed()     do {Disable_INT0();\
                                  Disable_INT_OCR2A();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4B();\
                                  Disable_INT_OCR5A();\
                                  Disable_INT_OCR5B();\
                                  Enable_INT_OCR4A();} while(0)
                                     
#define Ena_INT_X_Feed()      do {Disable_INT0();\
                                  Disable_INT_OCR2A();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4A();\
                                  Disable_INT_OCR4B();\
                                  Disable_INT_OCR5A();\
                                  Enable_INT_OCR5B();} while(0)

#define Ena_INT_X_aFeed()     do {Disable_INT0();\
                                  Disable_INT_OCR2A();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4A();\
                                  Disable_INT_OCR5A();\
                                  Disable_INT_OCR5B();\
                                  Enable_INT_OCR4B();} while(0)
                                  
#define Ena_INT_aFD()         do {Disable_INT_OCR2A();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4A();\
                                  Enable_INT_OCR4B();\
                                  Disable_INT_OCR5A();\
                                  Disable_INT_OCR5B();\
                                  Disable_INT0();} while(0)
                                     
#define Ena_INT_Z_Rapid()     do {Disable_INT0();\
                                  Disable_INT_OCR2B();\
                                  Disable_INT_OCR4A();\
                                  Disable_INT_OCR4B();\
                                  Disable_INT_OCR5A();\
                                  Disable_INT_OCR5B();\
                                  Enable_INT_OCR2A();} while(0)
                                     
#define Ena_INT_X_Rapid()     do {Disable_INT0();\
                                  Disable_INT_OCR2A();\
                                  Disable_INT_OCR4A();\
                                  Disable_INT_OCR4B();\
                                  Disable_INT_OCR5A();\
                                  Disable_INT_OCR5B();\
                                  Enable_INT_OCR2B();} while(0)
                                                                       

// ***** My Flags *****
typedef struct
{ 
   uint8_t bit0 : 1;
   uint8_t bit1 : 1;
   uint8_t bit2 : 1;
   uint8_t bit3 : 1;
   uint8_t bit4 : 1;
   uint8_t bit5 : 1;
   uint8_t bit6 : 1;
   uint8_t bit7 : 1;
}FLAG;
#define Spindle_Dir        ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit0    // CW-0, CCW-1
#define Motor_Z_Dir        ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit1    // CW-0, CCW-1
#define Joy_Z_flag         ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit2    // On-1, Off-0
#define Step_Z_flag        ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit3    // On-1, Off-0
#define Motor_X_Dir        ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit4    // CW-0, CCW-1
#define Joy_X_flag         ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit5    // On-1, Off-0
#define Step_X_flag        ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit6    // On-1, Off-0
#define Cone_flag          ((volatile FLAG*)_SFR_MEM_ADDR(GPIOR0))->bit7    // On-1, Off-0

bool spindle_flag = OFF;
bool feed_Z_flag = OFF;
bool feed_X_flag = OFF;

bool rapid_step_Z_flag = OFF;
bool rapid_step_X_flag = OFF;
bool rapid_Z_flag = OFF;
bool rapid_X_flag = OFF;

bool limit_Left_flag = OFF;
bool limit_Right_flag = OFF;
bool limit_Front_flag = OFF;
bool limit_Rear_flag = OFF;

bool limit_button_flag = OFF;
bool button_up_flag = OFF;
bool button_down_flag = OFF;
bool button_left_flag = OFF;
bool button_right_flag = OFF;

bool a_flag = false;
bool b_flag = false;
bool c_flag = false;
bool d_flag = false;
bool cycle_flag = false;

bool err_1_flag = false;
bool err_2_flag = false;
bool Complete_flag = false;

bool hand_X = OFF; 
bool hand_Z = OFF;
bool flag_hand_X = OFF;
bool flag_hand_Z = OFF;
bool X_flag = OFF;                    // временный
bool Z_flag = OFF;                    // временный
bool flag_Scale_x1 = OFF;             // возможно только для отладки
bool flag_Scale_x10 = OFF;            // возможно только для отладки
bool control_flag = OFF;
bool flag_j = OFF;

// ***** MY VARIABLES *****
int Tacho_Count = 0;
int Tacho_Count_Old =0;
int Spindle_Count = 0;
long Motor_Z_Pos = 0;
long Motor_X_Pos = 0;

int pin = 20;
float duration = 0;
unsigned long Timeout = 36000;
byte KeyCycle = 0;
int q = 0;
int w = 0; 
int Pass_Fin = 0;
int Thr_Pass_Summ = 0;
int SelectMenu = 0;
float Infeed_ValueX = 0;
float Infeed_ValueZ = 0;
long Size_X = 0;
long Size_Z = 0;
long X_pos = 0;
long Z_pos = 0;

int Enc_Pos = 0;
volatile long Hand_Count = 0;
long Hand_Count_Old = 0;
long Hand_Count_New = 0;
long Hand_Z_Pos = 0;
long Hand_X_Pos = 0;

byte Scale = HC_SCALE_1;

byte Ks_Count = 0;
int Km_Count = 0;
byte Ks_Divisor = 0;
byte tmp_Ks_Divisor = THRD_ACCEL;
int Km_Divisor = 0;
uint16_t Feed_Divisor = 0;
uint16_t aFeed_Divisor = 0;

byte Cs_Count = 0;
int Cm_Count = 0;
byte Cs_Divisor = 0;
int Cm_Divisor = 0;

byte tmp_Accel = THRD_ACCEL;
byte Repeat_Count = 0;

int Brake_Compens = 0;

byte Mode = Mode_Feed;
byte Sub_Mode_Thread = Sub_Mode_Thread_Man;
byte Sub_Mode_Feed = Sub_Mode_Feed_Man;
byte Sub_Mode_aFeed = Sub_Mode_aFeed_Man;
byte Sub_Mode_Cone = Sub_Mode_Cone_Man;
byte Sub_Mode_Sphere = Sub_Mode_Sphere_Man;
byte Thread_Step = 11;
byte Cone_Step = 33;

long Limit_Pos_Left = Limit_Pos_Max;
long Limit_Pos_Right = Limit_Pos_Min;
long Limit_Pos_Front = Limit_Pos_Max;
long Limit_Pos_Rear = Limit_Pos_Min;
volatile long Limit_Pos = 0;
volatile long Limit_Pos_HC = 0;

uint16_t Feed_mm = 0;
uint16_t aFeed_mm = 0;

uint16_t Start_Speed = ENC_LINE_PER_REV / ((uint32_t)MOTOR_Z_STEP_PER_REV * McSTEP_Z * MIN_FEED / SCREW_Z) /FEED_ACCEL;
uint16_t max_OCR5A = ENC_LINE_PER_REV / ((uint32_t)MOTOR_Z_STEP_PER_REV * McSTEP_Z * MIN_FEED / SCREW_Z) /FEED_ACCEL;                                 // Начальная скорость подачи при разгоне/торможении
uint16_t max_OCR4A = (250000 / ((uint32_t)MIN_aFEED * MOTOR_Z_STEP_PER_REV * McSTEP_Z / ((uint32_t)60 * SCREW_Z / 100) * 2) - 1) /FEED_ACCEL;

byte Total_Tooth = 1;
byte Current_Tooth = 1;

//byte Pass_Total = 1;
uint16_t Pass_Total = 1;
//byte Pass_Nr = 1; заменил на int Pass_Nr = 1; добавил ниже строку
//int Pass_Nr = 1;
uint16_t Pass_Nr = 1;
long Null_X_Pos = 0;
long Null_Z_Pos = 0;
int Ap = 0;

int ADC_Feed = 0;
long Sum_ADC = 0;
int ADC_Array[16];
byte x = 0;

long Control_Count = 0;


// Sphere
const int Cutter_Width_array[] = {100, 110, 120, 130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260, 270, 280, 290, 300};
#define TOTAL_CUTTER_WIDTH (sizeof(Cutter_Width_array) / sizeof(Cutter_Width_array[0]))
byte Cutter_Step = 5;
int Cutter_Width = Cutter_Width_array[Cutter_Step];

const int Cutting_Width_array[] = {10, 20, 30, 40, 50, 60, 70, 80, 90, 100};
#define TOTAL_CUTTING_STEP (sizeof(Cutting_Width_array) / sizeof(Cutting_Width_array[0]))
byte Cutting_Step = 4;
int Cutting_Width = Cutting_Width_array[Cutting_Step];

long Sph_R_mm = 1000;
long Sph_R = 0;
long R_Quad = Sph_R_mm * Sph_R_mm;
long Bar_R_mm = 0;
long Bar_R = 0;

#define KEYB_TIMER_FLAG       (TIFR1 & (1<<OCF1A))
#define CLEAR_KEYB_TIMER   do {TCNT1 = 0; (TIFR1 |= (1<<OCF1A));} while(0)

uint16_t max_OCR3A = HC_START_SPEED_1;
uint16_t min_OCR3A = HC_MAX_SPEED_1;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
  //-------- ОТЛАДОЧНАЯ ИНФОРМАЦИЯ ДЛЯ ВЫВОДА В "МОНИТОР ПОРТА" КОЭФФИЦИЕНТОВ РАССЧИТАННЫХ ПО ФОРМУЛЕ ИЗ ФАЙЛА Formula.h    ------------
#ifdef DEBUG_PRINT_THREAD_and_CONE
  //Активируем COM-порт
  Serial.begin(9600);
  //Печатаем исходные переменные на основе которых быдет выполняться расчёт
  Serial.print("ENC_LINE_PER_REV="); Serial.print(ENC_LINE_PER_REV); Serial.print("\n");  // Кол-во линий энкодера на 1 оборот шпинделя
  Serial.print("MOTOR_Z_STEP_PER_REV="); Serial.print(MOTOR_Z_STEP_PER_REV); Serial.print("\n");  // Кол-во шагов на оборот винта Z, продольная
  Serial.print("SCREW_Z="); Serial.print(SCREW_Z); Serial.print("\n");  // Шаг продольного винта Z в сотках, 1.5мм
  Serial.print("McSTEP_Z="); Serial.print(McSTEP_Z); Serial.print("\n");  // Микрошаг, ось Z, продольная
  Serial.print("MOTOR_X_STEP_PER_REV="); Serial.print(MOTOR_X_STEP_PER_REV); Serial.print("\n");  // Кол-во шагов на оборот винта X, поперечная
  Serial.print("SCREW_X="); Serial.print(SCREW_X); Serial.print("\n");  // Шаг поперечного винта X в сотках, 1.0мм
  Serial.print("McSTEP_X="); Serial.print(McSTEP_X); Serial.print("\n");  // Микрошаг, ось X, поперечная

  //Печатаем резьбы
  Serial.print("\nTHREAD Thread_Info:\n");
  for(int i = 0; i < TOTAL_THREADS; i++)
  {
    String Str = String(Thread_Info[i].Step) +" Kz = " +Thread_Info[i].Ks_Div_Z +"," +Thread_Info[i].Km_Div_Z
               +" Kx = " +Thread_Info[i].Ks_Div_X +"," +Thread_Info[i].Km_Div_X +"\n";
    Serial.print(Str);
 }

  //Печатаем Конуса
  Serial.print("\n\nCONE Cone_Info[]:\n");
  for(int j = 0; j < TOTAL_CONE; j++)
  {
    Serial.print(j);
    Serial.print("=");
    Serial.print(Cone_Info[j].Cs_Div);
    Serial.print(",");
    Serial.print(Cone_Info[j].Cm_Div);
    Serial.print("\n");
 }
#endif
  lcd.begin(20, 4);
  lcd.setCursor(0, 0);
  lcd.print("Digital_Feed_7e2_");
  lcd.setCursor(0, 1);
  lcd.print("2004_rus_AUTO"); //Электронная подача версия 7e2
  lcd.setCursor(0, 3);
  lcd.print("----2025.02.20-----");     //версия 7e2                                  
  _delay_ms(300);
  DDRG = B11111111;

  lcd.createChar(1, strelka_vlevo);
  lcd.createChar(2, strelka_vpravo);
  lcd.createChar(3, strelka_vverh);
  lcd.createChar(4, strelka_vniz);
  lcd.createChar(5, znak_gradus);
  lcd.createChar(6, znak_diametr);

  
  TIMSK0 = 0;
  // ***** Timer0 ***** // ***** Тахометр *****
//  TCCR0A = (1<<COM0B0)|(1<<WGM01); // Toggle OC0B on Compare Match // CTC Mode2
//  TCCR0B = (1<<CS00);     // No Prescaler
//  OCR0A = 89; // 1800/10(выходных пульсов)/2-1 = 89
//  TIMSK0 = (1<<OCIE0B);
    
  Encoder_Init();
  Hand_Init();
  Motor_Init();
  
  INT0_Init();
  INT2_Init();

  Timer2_Init();
  OCR2A = MIN_RAPID_MOTION;

  Timer3_Init();
  OCR3A = max_OCR3A;

  Timer4_Init();
  OCR4A = max_OCR4A;
  
  Timer5_Init();
  OCR5A = max_OCR5A;
  
  Ena_INT_Z_Feed();
  
  Limit_Init();
  Limit_Left_LED_Off();
  Limit_Right_LED_Off();
  Limit_Front_LED_Off();
  Limit_Rear_LED_Off();
  Menu_Buttons_Init();
  Joy_Init();
  Mode_Switch_Init();

  Beeper_Init();
  Beeper_Off();
  
  Spindle_Dir = CW;
  Motor_Z_Dir = CW;
  Joy_Z_flag = OFF;
  Step_Z_flag = OFF;
  Motor_X_Dir = CW;
  Joy_X_flag = OFF;
  Step_X_flag = OFF;

  TCCR1A = 0;
  TCCR1B = 0
         |(0<<ICNC1)|(0<<ICES1)
         |(0<<WGM13)|(1<<WGM12)
         |(1<<CS12)|(0<<CS11)|(1<<CS10);
  OCR1A = 625;

  Motor_Z_RemovePulse();
  Motor_X_RemovePulse();
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
  Spindle();
  Read_ADC_Feed();
  if (KEYB_TIMER_FLAG != 0) Menu();
  
  if (Mode == Mode_Divider) Print(); // пока для теста !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//  Print();                         // тоько для теста !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// *****  Thread ***** ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR(INT0_vect)
{
   

   if (!Enc_Ch_A)
   {
      if (!Enc_Ch_B)
      {
         Spindle_Dir = CW;
         if (++Enc_Pos == ENC_TICK)
         {                                           
            Enc_Pos = 0;
            Rev_Count++;
            if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
            else if (Joy_X_flag == ON) {Step_X_flag = ON;}
         }
      }
      else
      {
        Spindle_Dir = CCW;
        if (--Enc_Pos < 0)
        { 
           Enc_Pos = ENC_TICK - 1;
           Rev_Count--;
           if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
           else if (Joy_X_flag == ON) {Step_X_flag = ON;}
        }
      }
   }
   
   else
   {
      if (!Enc_Ch_B) 
      {
         Spindle_Dir = CCW;
         if (--Enc_Pos < 0)
         { 
            Enc_Pos = ENC_TICK - 1;
           Rev_Count--;
            if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
            else if (Joy_X_flag == ON) {Step_X_flag = ON;}
         }
      }
      else 
      {
         Spindle_Dir = CW;
         if (++Enc_Pos == ENC_TICK)
         {                                           
            Enc_Pos = 0;
            Rev_Count++;
            if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
            else if (Joy_X_flag == ON) {Step_X_flag = ON;}
         }
      }
   }  
   
   if (Step_Z_flag == ON)
   {   Motor_Z_RemovePulse();
      if ( (Motor_Z_Dir == CW && Motor_Z_Pos > Limit_Pos) || (Motor_Z_Dir == CCW && Motor_Z_Pos < Limit_Pos) || (!Joy_Z_flag) )
      {
         if (tmp_Ks_Divisor < tmp_Accel)
         {
            Ks_Count++;
            if (Ks_Count > tmp_Ks_Divisor)
            {
               Motor_Z_SetPulse();
               if (Motor_Z_Dir == CW) {Motor_Z_Pos ++;}
               else {Motor_Z_Pos --;}
               Ks_Count = 0;
               if (++Repeat_Count == REPEAt)
               {
                  Repeat_Count = 0;
                  tmp_Ks_Divisor ++;
               }
            }
         }  
         else {Step_Z_flag = OFF;}
      }

      else
      {
         Ks_Count++;
         if (Ks_Count > tmp_Ks_Divisor)
         {
            Motor_Z_SetPulse();
            if (Motor_Z_Dir == CW) {Motor_Z_Pos ++;}
            else {Motor_Z_Pos --;}
         
            if (tmp_Ks_Divisor > Ks_Divisor)
            {
               Ks_Count = 0;
               if (++Repeat_Count == REPEAt)
               {
                  Repeat_Count = 0;
                  tmp_Ks_Divisor --;
               }
            }
            else
            {  
               Km_Count = Km_Count + Km_Divisor;
               if (Km_Count > Km_Divisor)
               {
                  Km_Count = Km_Count - 10000;
                  Ks_Count = 0;
               }
               else {Ks_Count = 1;}
            }
         }
      }
   }
   
   if (Step_X_flag == ON)
   {  Motor_X_RemovePulse();
      if ( (Motor_X_Dir == CW && Motor_X_Pos > Limit_Pos) || (Motor_X_Dir == CCW && Motor_X_Pos < Limit_Pos) || (!Joy_X_flag) )
      {
         if (tmp_Ks_Divisor < tmp_Accel)
         {
            Ks_Count++;
            if (Ks_Count > tmp_Ks_Divisor)
            {
               Motor_X_SetPulse();
               if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
               else {Motor_X_Pos --;}
               Ks_Count = 0;
               if (++Repeat_Count == REPEAt)
               {
                  Repeat_Count = 0;
                  tmp_Ks_Divisor ++;
               }
            }
         }  
         else {Step_X_flag = OFF;}
      }

      else
      {
         Ks_Count++;
         if (Ks_Count > tmp_Ks_Divisor)
         {
            Motor_X_SetPulse();
            if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
            else {Motor_X_Pos --;}
         
            if (tmp_Ks_Divisor > Ks_Divisor)
            {
               Ks_Count = 0;
               if (++Repeat_Count == REPEAt)
               {
                  Repeat_Count = 0;
                  tmp_Ks_Divisor --;
               }
            }
            else
            {  
               Km_Count = Km_Count + Km_Divisor;
               if (Km_Count > Km_Divisor)
               {
                  Km_Count = Km_Count - 10000;
                  Ks_Count = 0;
               }
               else {Ks_Count = 1;}
            }
         }
      }
   }
}


ISR(INT1_vect)
{
   //
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// *****  Tacho ***** ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR (TIMER0_COMPB_vect)                                 // Тахометр
{
   //   
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// *****  Feed & Cone ***** //////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR (TIMER5_COMPA_vect)
{
     if (!Enc_Ch_A)
   {
      if (!Enc_Ch_B)
      {
         Spindle_Dir = CW;
         if (++Enc_Pos == ENC_TICK)
         {                                           
            Enc_Pos = 0;
		      	Rev_Count_F++;
            if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
            else if (Joy_X_flag == ON) {Step_X_flag = ON;}
         }
      }
      else
      {
        Spindle_Dir = CCW;
        if (--Enc_Pos < 0)
        { 
           Enc_Pos = ENC_TICK - 1;
		       Rev_Count_F--;
           if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
           else if (Joy_X_flag == ON) {Step_X_flag = ON;}
        }
      }
   }
   
   else
   {
      if (!Enc_Ch_B) 
      {
         Spindle_Dir = CCW;
         if (--Enc_Pos < 0)
         { 
            Enc_Pos = ENC_TICK - 1;
			      Rev_Count_F--;
            if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
            else if (Joy_X_flag == ON) {Step_X_flag = ON;}
         }
      }
      else 
      {
         Spindle_Dir = CW;
         if (++Enc_Pos == ENC_TICK)
         {                                           
            Enc_Pos = 0;
			Rev_Count_F++;
            if (Joy_Z_flag == ON) {Step_Z_flag = ON;}
            else if (Joy_X_flag == ON) {Step_X_flag = ON;}
         }
      }
   }
   if (Joy_Z_flag == ON) {Motor_X_RemovePulse();}
   
   Tacho_Count = Tacho_Count + (OCR5A+1);
   if (Tacho_Count > ENC_LINE_PER_REV)
   {
      if (Spindle_Dir == CW) {Rev_Count_F++;}
      else if (Spindle_Dir == CCW) {Rev_Count_F--;}	 
      Tacho_Count = Tacho_Count - ENC_LINE_PER_REV;
   }
   
   if ( (Motor_Z_Dir == CW && Motor_Z_Pos > Limit_Pos) || (Motor_Z_Dir == CCW && Motor_Z_Pos < Limit_Pos) || (!feed_Z_flag) )
   {
      if (OCR5A < max_OCR5A)
      {
         Motor_Z_InvertPulse();
         if (!Read_Z_State)
         {
            OCR5A++;
            if (Motor_Z_Dir == CW) {Motor_Z_Pos ++;}
            else {Motor_Z_Pos --;}
         }
      }
      else
      {             
         Step_Z_flag = OFF;
         Step_X_flag = OFF;
      }
   }
   
   else
   {
      Step_Z_flag = ON;
      Motor_Z_InvertPulse();
      if (!Read_Z_State)
      {
         if (Motor_Z_Dir == CW) {Motor_Z_Pos ++;}
         else {Motor_Z_Pos --;}
      
         if (OCR5A > Feed_Divisor) {OCR5A--;}
         else if (OCR5A < Feed_Divisor) {OCR5A ++;}
      }
   }
   
   if (Step_X_flag == ON)
   {
      if (++Cs_Count > Cs_Divisor)
      {
         Motor_X_SetPulse();

         if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
         else {Motor_X_Pos --;}

         Cm_Count = Cm_Count + Cm_Divisor;
         if (Cm_Count > Cm_Divisor)
         {
            Cm_Count = Cm_Count - 10000;
            Cs_Count = 0;
         }
         else {Cs_Count = 1;}
      }
   }
}

////////////////////////////////////////////////////////////
ISR (TIMER5_COMPB_vect)
{
      
   Tacho_Count = Tacho_Count + (OCR5A+1);
   if (Tacho_Count > ENC_LINE_PER_REV)
   {
      if (Spindle_Dir == CW) {Rev_Count_F++;}
      else if (Spindle_Dir == CCW) {Rev_Count_F--;}
      Tacho_Count = Tacho_Count - ENC_LINE_PER_REV;
   }
  
   if ( (Motor_X_Dir == CW && Motor_X_Pos > Limit_Pos) || (Motor_X_Dir == CCW && Motor_X_Pos < Limit_Pos) || (!feed_X_flag) )
   {
      if (OCR5A < max_OCR5A)
      {
         Motor_X_InvertPulse();
         if (!Read_X_State)
         {
            OCR5A++;
            if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
            else {Motor_X_Pos --;}
         }
      }
      else {Step_X_flag = OFF;}
   }
   
   else 
   {
      Step_X_flag = ON;
      Motor_X_InvertPulse();
      {
         if (!Read_X_State)
         {
            if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
            else {Motor_X_Pos --;}
      
            if (OCR5A > Feed_Divisor) {OCR5A--;}
            else if (OCR5A < Feed_Divisor) {OCR5A ++;}
         }
      }
   }

   /////////////////////////////////////////////////////////
   if (Mode == Mode_Sphere)                               // Режим Сфера
   {
      
   }
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ***** Rapid Feed & Rapid Cone ***** ///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR (TIMER2_COMPA_vect)
{
   Motor_X_RemovePulse();
   if ( (Motor_Z_Dir == CW && Motor_Z_Pos > Limit_Pos) || (Motor_Z_Dir == CCW && Motor_Z_Pos < Limit_Pos) || (!rapid_Z_flag) )
   {
      if (OCR2A < MIN_RAPID_MOTION)
      {
         Motor_Z_InvertPulse();
         if (!Read_Z_State)
         {
            if (Motor_Z_Dir == CW) { Motor_Z_Pos ++;}
            else { Motor_Z_Pos --;}

            if (++Repeat_Count == REPEAt)
            {
               Repeat_Count = 0;
               OCR2A ++;
            }
         }
      }
      else
      {
         rapid_step_Z_flag = OFF;
         Step_X_flag = OFF;
      }
   }
  
   else
   {
      rapid_step_Z_flag = ON;
      Motor_Z_InvertPulse();
      if (!Read_Z_State)
      {
         if (Motor_Z_Dir == CW) { Motor_Z_Pos ++;}
         else { Motor_Z_Pos --;}

         if (OCR2A > MAX_RAPID_MOTION_Z)
         {
            if (++Repeat_Count == REPEAt)
            {
               Repeat_Count = 0;
               OCR2A --;
            }
         }
      }
   }
   
   ///////////////////////////////////////////////////////
   if (Step_X_flag == ON)
   {
      if (++Cs_Count > Cs_Divisor)
      {
         Motor_X_SetPulse();
         if (Motor_X_Dir == CW) { Motor_X_Pos ++;}
         else { Motor_X_Pos --;}
         
         Cm_Count = Cm_Count + Cm_Divisor;
         if (Cm_Count > Cm_Divisor)
         {
            Cm_Count = Cm_Count - 10000;
            Cs_Count = 0;
         }
         else {Cs_Count = 1;}
      }
   }
}

//////////////////////////////////////////////////////////
ISR (TIMER2_COMPB_vect)
{
   if ( (Motor_X_Dir == CW && Motor_X_Pos > Limit_Pos) || (Motor_X_Dir == CCW && Motor_X_Pos < Limit_Pos) || (!rapid_X_flag) )
   {
      if (OCR2A < MIN_RAPID_MOTION)
      {
         Motor_X_InvertPulse();
         if (!Read_X_State)
         {
            if (Motor_X_Dir == CW) { Motor_X_Pos ++;}
            else { Motor_X_Pos --;}

            if (++Repeat_Count == REPEAt)
            {
               Repeat_Count = 0;
               OCR2A ++;
            }
         }
      }
      else  {rapid_step_X_flag = OFF;}
   }
  
   else
   {
      rapid_step_X_flag = ON;
      Motor_X_InvertPulse();
      if (!Read_X_State)
      {
         if (Motor_X_Dir == CW) { Motor_X_Pos ++;}
         else { Motor_X_Pos --;}

         if (OCR2A > MAX_RAPID_MOTION_X)
         {
            if (++Repeat_Count == REPEAt)
            {
               Repeat_Count = 0;
               OCR2A --;
            }
         }
      }
   }
}



//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ***** Asynchron Feed ***** ////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR (TIMER4_COMPA_vect)
{

  /* if (Joy_Z_flag == ON) {Motor_X_RemovePulse();}
   
   Tacho_Count = Tacho_Count + (OCR5A+1);
   if (Tacho_Count > ENC_LINE_PER_REV)
   {
      Tacho_Count = Tacho_Count - ENC_LINE_PER_REV;
   }*/
  
   if ( (Motor_Z_Dir == CW && Motor_Z_Pos > Limit_Pos) || (Motor_Z_Dir == CCW && Motor_Z_Pos < Limit_Pos) || (!feed_Z_flag) )
   {
      if (OCR4A < max_OCR4A)
      {
         Motor_Z_InvertPulse();
         if (!Read_Z_State)
         {
            OCR4A ++;
            if (Motor_Z_Dir == CW) {Motor_Z_Pos ++;}
            else                   {Motor_Z_Pos --;}
         }
      }
      else
      {             
         Step_Z_flag = OFF;
         Step_X_flag = OFF;
      }
   }
   
   else
   {
      Step_Z_flag = ON;
      Motor_Z_InvertPulse();
      if (!Read_Z_State)
      {
         if (Motor_Z_Dir == CW) {Motor_Z_Pos ++;}
         else                   {Motor_Z_Pos --;}

         if      (OCR4A > aFeed_Divisor) {OCR4A --;}
         else if (OCR4A < aFeed_Divisor) {OCR4A ++;}
      }
   }
}

//////////////////////////////////////////////////////////
ISR (TIMER4_COMPB_vect)
{
   if ( (Motor_X_Dir == CW && Motor_X_Pos > Limit_Pos) || (Motor_X_Dir == CCW && Motor_X_Pos < Limit_Pos) || (!feed_X_flag) )
   {
      if (OCR4A < max_OCR4A)
      {
         Motor_X_InvertPulse();
         if (!Read_X_State)
         {
            OCR4A ++;
            if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
            else                   {Motor_X_Pos --;}
         }
      }
      else
      {             
         Step_Z_flag = OFF;
         Step_X_flag = OFF;
      }
   }
   
   else 
   {
      Step_X_flag = ON;
      Motor_X_InvertPulse();
      if (!Read_X_State)
      {
         if (Motor_X_Dir == CW) {Motor_X_Pos ++;}
         else                   {Motor_X_Pos --;}

         if      (OCR4A > aFeed_Divisor) {OCR4A --;}
         else if (OCR4A < aFeed_Divisor) {OCR4A ++;}
      }
   }
}



//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ***** HandCoder ***** /////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR(INT2_vect)
{
   if (!Hand_Ch_A)
   {
      if (!Hand_Ch_B) {Hand_Count --;}
   }
   
   else
   {
      if (!Hand_Ch_B) {Hand_Count ++;}
   }

}

/////////////////////////////////////////////
ISR (TIMER3_COMPA_vect)
{   
   if (Motor_Z_Dir == CW)
   {
      if (Motor_Z_Pos < Null_Z_Pos + Hand_Z_Pos)
      {
         Motor_Z_InvertPulse();
         if (!Read_Z_State) 
         {
            Motor_Z_Pos ++;

            if ((Motor_Z_Pos > Limit_Pos_HC) || (hand_Z == OFF))
            {
               if (OCR3A < max_OCR3A) OCR3A ++;
            }
            else if (Motor_Z_Pos < Limit_Pos_HC)
            {
               if (OCR3A > min_OCR3A) OCR3A --;
            }
         }
      }
      else if (Motor_Z_Pos == Hand_Z_Pos)
      {
      //
      }
   }

   else if (Motor_Z_Dir == CCW)
   {
      if (Motor_Z_Pos > Null_Z_Pos + Hand_Z_Pos)
      {
         Motor_Z_InvertPulse();
         if (!Read_Z_State) 
         {
            Motor_Z_Pos --;

            if (Motor_Z_Pos < Limit_Pos_HC  || hand_Z == OFF)
            {
               if (OCR3A < max_OCR3A) OCR3A ++;
            }
            else if (Motor_Z_Pos > Limit_Pos_HC)
            {
               if (OCR3A > min_OCR3A) OCR3A --;
            }
         }
      }
      else if (Motor_Z_Pos == Hand_Z_Pos)
      {
      //
      }
   }
}

//////////////////////////////////////////////////////////
ISR (TIMER3_COMPB_vect)
{   
   if (Motor_X_Dir == CW)
   {
      if (Motor_X_Pos < Null_X_Pos + Hand_X_Pos)
      {
         Motor_X_InvertPulse();
         if (!Read_X_State) 
         {
            Motor_X_Pos ++;

            if ((Motor_X_Pos > Limit_Pos_HC) || (hand_X == OFF))
            {
               if (OCR3A < max_OCR3A) OCR3A ++;
            }
            else if (Motor_X_Pos < Limit_Pos_HC)
            {
               if (OCR3A > min_OCR3A) OCR3A --;
            }
         }
      }
      else if (Motor_X_Pos == Hand_X_Pos)
      {             
      //
      }
   }

   else if (Motor_X_Dir == CCW)
   {
      if (Motor_X_Pos > Null_X_Pos + Hand_X_Pos)
      {
         Motor_X_InvertPulse();
         if (!Read_X_State) 
         {
            Motor_X_Pos --;
            
            if ((Motor_X_Pos < Limit_Pos_HC) || (hand_X == OFF))
            {
               if (OCR3A < max_OCR3A) OCR3A ++;
            }
            else if (Motor_X_Pos > Limit_Pos_HC)
            {
               if (OCR3A > min_OCR3A) OCR3A --;
            }
         }
      }
      else if (Motor_X_Pos == Hand_X_Pos)
      {             
      //
      }
   }
}


// ***** End ***** ///////////////////////////////////////////////////