Не, видео классное! Я не правильно спросил. Имел ввиду что хотелось в файлике на цифры посмотреть. А синусоиду в теме как регулировать на видео видел. То есть тестовая траектория только синус или может быть произвольная?

Похоже погорячился я с энкодерами. При таких скоростях расчета пид к
ак минимум в 10 раз точнее нужны. Но тогда и частота сканирования под 100кГц Не зря интуиция подсказывала что просто не будет. Как бы выкрутиться?

taran_ob писал(а):Имел ввиду что хотелось в файлике на цифры посмотреть

Сделаю.

taran_ob писал(а):То есть тестовая траектория только синус или может быть произвольная?

Генератор может давать синус и меандр. + любую последовательность, которую ты ручками запишешь... в виде колонки цифр...

Ale писал(а):Я бы остановился на периоде передачи данных из игры порядка 5..10 мс (что дает нам в идеале возможность ощущать пятой точкой почти звуковые колебания до 100 (50) Гц при наезде на поребрики/бордюры).

Вот о чем подумал, как то пытался сделать гравировальную головку для станка из привода позиционирования головок харда. Так вот при 30-50 Гц она уже практически не стучала, при том что веса у нее никакого. То есть, сомневаюсь, что платформа будет эффективно справляться с таким заданием, по крайней мере с резиновоременным приводом. Да и редуктор в вайпере сдохнет быстро. Может лучше в твою программу для компа внедрить алгоритм-фильтр который будет отслеживать гармонические колебания и по их амплитуде (или частоте) слать данные на контроллер вместе с координатой. А контроллер в свою очередь формирует отдельный канал с ШИМ для вибромотора. Если на каждой оси по вибро, то разнеся их справа - слева получим стерео эффект наезда на гравий и тд. или ощущение наезда одной парой колес. В таком случае нет смысла так часто слать координаты в контроллер, 20Гц ИМХО достаточно, 100-200Гц для контура стабилизации скорости тоже, частота сканирования энкодера на 45-50имп 33кГц. Тайменги для ПИДа становятся уже более реальными и без ухищрений.

taran_ob писал(а):Вот о чем подумал, как то пытался сделать гравировальную головку для станка из привода позиционирования головок харда. Так вот при 30-50 Гц она уже практически не стучала, при том что веса у нее никакого. То есть, сомневаюсь, что платформа будет эффективно справляться с таким заданием, по крайней мере с резиновоременным приводом. Да и редуктор в вайпере сдохнет быстро.

Думаю, ты прав... Я тут пошел на поводу у Таноса... Хотя давно понятно, что не стоит Он развивает свой продукт как раз в сторону увеличения скорости передачи координат. Я с ним поспорил на буржуйском сайте, а тут чего-то ступил... В общем, давай пересмотрим, ну, скажем на те же 33мс. Уж 15 Гц (небольшой амплитуды) можно попытаться получить от платформы.

taran_ob писал(а):Может лучше в твою программу для компа внедрить алгоритм-фильтр который будет отслеживать гармонические колебания и по их амплитуде (или частоте) слать данные на контроллер вместе с координатой. А контроллер в свою очередь формирует отдельный канал с ШИМ для вибромотора.

Типа как на руле сделано . Усложним механику, как мне кажется. Хотя с точки зрения программ и контроллеров никаких сложностей нет.

Или может аппаратно решить проблему?
Аппаратный подход 1. Модернизировать инкрементный энкодер под интерфейс шаг/направление (заводские такие тоже есть) импульсов на 100-200. Счетный импульс завести на ногу таймера, по переполнению увеличивать множитель, направление энкодера на внешнее прерывание в котором сохранять значение таймера и тд. При этом здорово разгрузить контроллер.
Аппаратный подход 2. Энкодер оставить такой как есть 15-30имп, сканировать по таймеру. И завести с него сигнал еще на модуль захвата, по которому организовать контур стабилизации скорости - измерять время между импульсами. Но при этом теряем 16бит таймер - шимить придется 8бит.
Аппаратный подход 3 пересесть на камень с аппаратным декодером квадратур. Но хмега слишком монстроподобная, а в пиках я не шарю.

Спасибо!

Ale писал(а):Генератор может давать синус и меандр. + любую последовательность, которую ты ручками запишешь... в виде колонки цифр...

Ага, буду разбираться...

Ale писал(а):Он развивает свой продукт как раз в сторону увеличения скорости передачи координат.

Думаю, этим он компенсирует не эффективность регулятора! Так можно и до релейного управления добраться

Ale писал(а):В общем, давай пересмотрим, ну, скажем на те же 33мс.

Ок, хороший компромисс!

Ale писал(а):Усложним механику, как мне кажется.

А че, просто жёстко прикрутить пару (один) моторчик к раме(креслу)... В любом случае как опция, по желанию... Я делал вибро, мотор из струйника (по 3$ в мастерской купил 5шт принтеров), толстая шайба эксцентриком. Там, кстати, и енкодеры готовые, только слишком точные...

Ale писал(а):Типа как на руле сделано

А у меня в руле нет вибро , но мотор обратной связи есть

Ale писал(а):
taran_ob писал(а):Может лучше в твою программу для компа внедрить алгоритм-фильтр который будет отслеживать гармонические колебания и по их амплитуде (или частоте) слать данные на контроллер вместе с координатой. А контроллер в свою очередь формирует отдельный канал с ШИМ для вибромотора.

Типа как на руле сделано . Усложним механику, как мне кажется. Хотя с точки зрения программ и контроллеров никаких сложностей нет

Идея сильнейшая! Очень поддерживаю!

PS У нас на форуме такое проскакивало если не ошибаюсь, но вместо вибро мотора должен был быть низкочастотный динамик. Не знаю чем закончилось ....

Я тут было отвлекся на другой проект... постепенно возвращаюсь...
Собрал простенький макет для проверки позиционирования, позже фото выложу. Почти написал тестовую прошивку. Протокол, как и в оригинале - 5ти байтные посылки: 1 заголовок (А), 2 идентификатор контроллера(В для широковещания и 1,2,3 и тд для индивидуальных посылок), 3 команда, 4 старший байт данных, 5 младший байт данных. Ответ (если необходим) в том же формате. Сохранил поддержку приема координат сразу для 2х осей, без ответа, для совместимости с софтом от Ale. Для настройки и отображения графика рассогласования попросил коллегу (очень не удобно Ale отвлекать от своего проекта) написать программку - пишет.
Имеется команда для приема ШИМ вибромотора. Установка ограничения тока мотора программная-контроллер формирует шим для опоры компаратора тока, думаю тему можно развить для получения небольшого пикового запаса мощности.
Поддержки потенциометра не будет, разве что подключить вместо концевиков в качестве задатчика траектории при тестировании (настройке).
По поводу регулятора. Пока отошел от ранее предложенного - боюсь не смогу достаточно быстро опрашивать энкодер, да и делать новые, более разрядные, пока лень. Остановился на таком алгоритме: энкодер на 15 импульсов подключен на INT0 INT1 по любому изменению уровня для отслеживания квадратуры (15*4=60 отсчетов за оборот). Там же происходит подсчет 2МГц тактов таймера2 между импульсами квадратур для вычисления скорости вращения двигателя которая стабилизируется по ПИ закону с частотой 100-200Гц. Заданная же, скорость рассчитывается по рассогласованию позиции. В результате, как и раньше говорил, движение от задания к заданию с постоянной скоростью.
Надеюсь, скоро будут тесты в железе.

Приступаю к отладке

Симпатично! И да, удачи на испытаниях

Мужчины, нужна ваша помощь!
Пока занимаюсь отладкой, хочется услышать ваши замечания по поводу функционала который я пытаюсь вложить.

Работа контроллера начинается с цикла калибровки, в котором определяется диапазон позиции и максимальная скорость импульсов с энкодера. Сначала мотор вращается влево с макс. ШИМ/2 до левого концевика. Затем текущая позиция обнуляется и меняется направление. После достижения правого концевика, производится расчет диапазона позиции с учетом программного лимита. Далее направление опять меняется и устанавливается максимальный ШИМ. После достижения точки половины диапазона, определяется максимальная скорость. Контроллер переходит рабочий режим - цикл стабилизации позиции с заданием от ПК. По камманде, см. ниже, контроллер может перейти в режимы, которые могут помочь при настройке: стабилизация скорости и управление ШИМом. Так как программа для конфигурирования еще не написана, то включил дополнительные режимы с управлением от потенциометра и отправкой контролируемых параметров в ASCII коде для приема в любом терминале. Кратковременным нажатием, во время включения, кнопки DEF, осуществляется вход и циклическое переключение дополнительных режимов. Удерживанием в течении 5с - контроллер примет значения констант записанные во флеше. В принципе, во флеш можно записать ранее настроенные константы и закоротить DEF, чтоб не использовать EEPROM, так как часто в сети люди жалуются на её работу в импульсных схемах.

Протокол обмена. Контроллер принимает запрос и отправляет ответ, если предусмотрен, в едином формате - 5 байт:
1 байт - заголовок, по умолчанию - 0х41
2 байт - идентификатор. Запросы с "широковещательным" идентификатором (по умолчанию 0х42), принимают все контроллеры в сети. Запрос с "личным" идентификатор (любой байт заданный в настройках, по умолчанию 0х01 и 0х02 для 1 и 2 оси соответственно) принимает только один, соответствующий ему, контроллер в сети.
3 байт - команда. Перечень ниже.
4 байт - старший байт данных
5 байт - младший байт данных

Команды:

RESET_CONTROLLER Перезагрузить контроллер. Без ответа. Широк. и индив. запрос.
SAVE_CONST_TO_EEPROM Записать константы находящиеся в оперативной памяти контроллера в еепром. Контроллер отправляет эхо для подтверждения принятия запроса.
LOAD_DEFAULT_CONST Загрузить константы "по умолчанию" в оперативную память. Контроллер отправляет эхо для подтверждения принятия запроса.

Команды ниже относятся к записи и чтению оперативной памяти.

WRITE_NORM_POSITION Записать нормированную (0-256) позицию, без ответа. При широк. запросе координата в мл. байте. При индив. - в мл. байте координата, в ст. байте ШИМ 1125Гц, для коммутации интеллектуального мосфета который может управлять вибромотором (при поддержке управляющего софта) или чего то еще.
WRITE_POSITION Записать позицию в размерности квадратур энкодера из диапазона 0-32767. Ответ - текущее значение.
WRITE_VELOCITY Записать +-скорость (частоту), следования квадратур для режима настройки в котором осуществляется стабилизация скорости по ПИ закону. Знак определяет направление. С энкодером на N=15 эта частота совпадает с частотой вращения двигателя Fдвиг=Fэнк*4N/60 в об/мин. Ответ - текущая скорость.
WRITE_PWM_MOTOR Записать ШИМ из диапазона (задается командами ниже) от -pwm_max до -pwm_min и от +pwm_min до +pwm_max, для режима настройки в котором можно выполнить проверку моста или отладить ограничение тока мотора. Знак определяет направление. Ответ - текущая скорость.
WRITE_PWM_VIBRO Записать ШИМ виброматора. Ответ - текущее значение.

WRITE_KOEF_PROP Записать пропорциональны коэффициент контура стабилизации скорости. Ответ - текущее значение.
WRITE_KOEF_INT Записать интегральный коэффициент контура стабилизации скорости.
WRITE_KOEF_VEL Записать коэффициент пропорциональности ошибки позиции и скорости необходимой для устранения ошибки за фиксированное время (пока задал 10 мс), для контура стабилизации позиции. Ответ - текущее значение.
WRITE_PWM_MIN Записать мин шим мотора. Ответ - текущее значение.
WRITE_PWM_MAX Записать макс шим мотора. Ответ - текущее значение.
WRITE_SOFT_LIMIT Записать размер (число квадратур) мертвой зоны до концевика. Ответ - текущее значение.
WRITE_PWM_REF Записать шим для формирования опорного напряжения компаратора обратной связи по току. Пока только для ограничения тока двигателя. Ответ - текущее значение.
WRITE_CONFIG Записать режим управления: калибровка положения и максимальной скорости, стабилизация позиции, стабилизация скорости, регулировка ШИМ. Дополнительные режимы с управлением от потенциометра: стабилизация позиции, стабилизация скорости, регулировка ШИ. В дополнительных режимах заданный и контролируемый параметр передается с частотой 10Гц в ASCII коде для приема данных любым терминалом.
WRITE_USART_BAUND Записать скорость порта. Ответ - текущее значение.
WRITE_ID Записать идентификатор контроллера. Ответ - текущее значение.

READ_POSITION
READ_VELOCITY
READ_PWM_MOTOR
READ_PWM_VIBRO

READ_KOEF_PROP
READ_KOEF_INT
READ_KOEF_VEL
READ_PWM_MIN
READ_PWM_MAX
READ_SOFT_LIMIT
READ_PWM_REF
READ_CONFIG
READ_POSITION_RANGE Прочитать диаппазон позиции определенный в цикле калибровки.
READ_KOEF_NORM Прочитать коэффициент нормирования позиций 0-255 к диапазону позиций энкодера. Определяется в цикле калибровки.
READ_VELOCITY_MAX Прочитать максимальную скорость при заданном макс. ШИМ. Определяется в цикле калибровки.

Как то так

Сейчас код выглядит так

Код: Выделить всё

//Файлы/////////////////////////////////////////////////////////////////////////////////
#include <mega88.h>
#include <delay.h>
#include <stdio.h>

//Макросы/////////////////////////////////////////////////////////////////////////////////
#define BYTEH(word) (char)(word>>8)
#define BYTEL(word) (char)(word)
#define WORD(byteh,bytel) ((int)byteh<<8)|bytel
#define BYTE(nibbleh,nibblel) (nibbleh<<4)|nibblel
#define CLRBIT(byte, mask) byte&=~mask
#define SETBIT(byte, mask) byte|=mask
#define ABS(var) ((var) < 0 ? -(var) : (var))

//Команды///////////////////////////////////////////////////////////////////////////////////               
#define WRITE_NORM_POSITION             0xFF

#define RESET_CONTROLLER                0x01
#define SAVE_CONST_TO_EEPROM            0x02
#define LOAD_DEFAULT_CONST              0x03

#define WRITE_POSITION                  0x11
#define WRITE_VELOCITY                  0x12
#define WRITE_PWM_MOTOR                 0x13
#define WRITE_PWM_VIBRO                 0x14

#define WRITE_KOEF_PROP                 0x21
#define WRITE_KOEF_INT                  0x22
#define WRITE_KOEF_VEL                  0x23
#define WRITE_PWM_MIN                   0x24
#define WRITE_PWM_MAX                   0x25
#define WRITE_SOFT_LIM                  0x26
#define WRITE_PWM_REF                   0x27
#define WRITE_CONFIG                    0x28

#define WRITE_USART_BAUND               0x31       
#define WRITE_ID                        0x32       

#define READ_POSITION                   0x41
#define READ_VELOCITY                   0x42
#define READ_PWM_MOTOR                  0x43
#define READ_PWM_VIBRO                  0x44

#define READ_KOEF_PROP                  0x51
#define READ_KOEF_INT                   0x52
#define READ_KOEF_VEL                   0x53
#define READ_PWM_MIN                    0x54
#define READ_PWM_MAX                    0x55
#define READ_SOFT_LIM                   0x56
#define READ_PWM_REF                    0x57
#define READ_CONFIG                     0x58

#define READ_POSITION_RANGE             0x61
#define READ_KOEF_NORM                  0x62
#define READ_VELOCITY_MAX               0x63

//Регистр UCSR0A////////////////////////////////////////////////////////////////////////////////////
#define PARITY_ERROR                    0b00000100
#define DATA_OVERRUN                    0b00001000
#define FRAMING_ERROR                   0b00010000
#define DATA_REGISTER_EMPTY             0b00100000
#define RX_COMPLETE                     0b10000000

//Регистр reg_flag///////////////////////////////////////////////////////////////////////////////////
#define FLAG_CALIB_COMPL                0b00000001
#define FLAG_CALIB_ERROR                0b00000010
#define FLAG_EN_CALC_VEL_GET            0b00000100
#define FLAG_MOTOR_OVL                  0b00001000
#define FLAG_REQUEST_NEW                0b01000000
#define FLAG_RX_BUF_OVF                 0b10000000

//Регистр reg_ctrl//////////////////////////////////////////////////////////////////////////////////
#define CTRL_CALIB_POS                  0b00000000
#define CTRL_USART_POS_VEL_PWM          0b00000001
#define CTRL_USART_VEL_PWM              0b00000010
#define CTRL_USART_PWM                  0b00000100
#define CTRL_ADC_POS_VEL_PWM            0b00001000
#define CTRL_ADC_VEL_PWM                0b00010000
#define CTRL_ADC_PWM                    0b00100000

//Порты ввода-вывода///////////////////////////////////////////////////////////////////////////////////
#define PORT_PWM                        PORTB   
#define MASK_PWM                        0b00000110
#define MASK_PWMA                       0b00000010
#define MASK_PWMB                       0b00000100

#define PORT_DEF                        PINC   
#define MASK_DEF                        0b00001000

#define PORT_LIM                        PINC   
#define MASK_LIM                        0b00110000
#define MASK_LIM_L                      0b00010000
#define MASK_LIM_R                      0b00100000

#define PORT_ENC                        PIND   
#define MASK_ENC                        0b00001100
#define PORT_ENC_SHIFT                  ((PORT_ENC&MASK_ENC)>>2)

#define PORT_TXEN                       PORTD   
#define MASK_TXEN                       0b00010000

#define PORT_VIBRO                      PORTD   
#define MASK_VIBRO                      0b00100000

#define PORT_REF                        PORTD   
#define MASK_REF                        0b01000000

#define PORT_OVL                        PIND   
#define MASK_OVL                        0b10000000

#define PIN_POT                         0x02   

//Операции с регистрами///////////////////////////////////////////////////////////////////////////////
#define DO_CON_OCR1AB                    TCCR1A|=0b10100000;
#define DO_DISCON_OCR1AB                 TCCR1A&=0b01011111;

#define DO_CON_OCR1A                     TCCR1A&=0b11011111; TCCR1A|=0b10000000;
#define DO_CON_OCR1B                     TCCR1A&=0b01111111; TCCR1A|=0b00100000;

#define DO_CON_OCR0B                     TCCR0A|=0b00100000;
#define DO_DISCON_OCR0B                  TCCR0A&=0b11011111;   

//Системные константы///////////////////////////////////////////////////////////////////////////////
#define FREQ_CLK                        18432000    //Hz
#define FREQ_TIM0                       288000      //Hz
#define FREQ_TIM2                       2304000     //Hz

#define FREQ_CALC                       100         //Hz
#define DIV_CALC                        11          //DIV_CALC=FREQ_TIM0/256*FREQ_CALC

#define FREQ_SEND_ADC                   10          //Hz
#define DIV_SEND_ADC                    10          //DIV_CALC=FREQ_CALC/FREQ_SEND_ADC

#define MESSAGE_SIZE                    5
#define RX_BUFFER_SIZE                  32
#define TX_BUFFER_SIZE                  32

#define HEADER                          0x41
#define ID_INDIV_DOF1                   0x01
#define ID_INDIV_DOF2                   0x02

#define ADC_VREF_TYPE                   0x20

//Умолчания///////////////////////////////////////////////////////////////////////////////
#define DEFAULT_K_PROP          50      //кэфф пропорциональный скорости
#define DEFAULT_K_INT           50      //кэфф интегральный скорости
#define DEFAULT_K_VEL           100     //кэфф пропорциональный скорости (FREQ_CALC  скорость единичного перемещения при заданной частоте обновления координаты)
#define DEFAULT_PWM_MIN         0       //минимальный шим мотора
#define DEFAULT_PWM_MAX         1000    //максимальный шим мотора
#define DEFAULT_SOFT_LIM        100     //число шагов до концевика мертвой зоны
#define DEFAULT_PWM_REF         0       //0x007F//50% соответствует 2,5V/5=0.5V  U=I*R  U=10 A*0,1 Omh = 1V
#define DEFAULT_CONFIG          0x0000  //reg_flag reg_ctrl=CTRL_CALIB_POS         
#define DEFAULT_BAUND           (FREQ_CLK/(16*57600))-1     //57600 скорость СОМ порта
#define DEFAULT_ID              0x4201  //0x42 широковещание, 0x01 индивидуальный

//Константы в EEPROM//////////////////////////////////////////////////////////////////////////
///*
eeprom int eep_k_prop           @0x10;                     
eeprom int eep_k_int            @0x12;                     
eeprom int eep_k_vel            @0x14;                     
eeprom int eep_pwm_min          @0x16;                     
eeprom int eep_pwm_max          @0x18;                     
eeprom int eep_soft_lim         @0x1A;                     
eeprom int eep_pwm_ref          @0x1C;
eeprom int eep_config           @0x1E;
eeprom int eep_baund            @0x20;                     
eeprom int eep_id               @0x22;                                           
//*/
 
unsigned int const_k_prop;             
unsigned int const_k_int;               
unsigned int const_k_vel;               
unsigned int const_pwm_min;             
unsigned int const_pwm_max;             
unsigned int const_soft_lim;             
unsigned int const_pwm_ref;                         
unsigned int const_config;             
unsigned int const_baund;               
unsigned int const_id;

//Константы/////////////////////////////////////////////////////////////////////////////////
unsigned char const_id_bdc;
unsigned char const_id_indiv;

unsigned char reg_flag;
unsigned char reg_ctrl;

unsigned int const_pos_range;
unsigned int const_k_norm;
unsigned int const_vel_max;

unsigned int const_adc_norm_vel;
unsigned int const_adc_norm_pwm;

unsigned char const_adc_pwm_min;
unsigned char const_adc_pwm_max;

//Переменные///////////////////////////////////////////////////////////////////////////////
unsigned char count_tim0_ovf;
unsigned char count_tim2_ovf;


unsigned char rx_wr_index, rx_rd_index, rx_counter;
unsigned char tx_wr_index, tx_rd_index, tx_counter;
char rx_buffer[RX_BUFFER_SIZE];
char tx_buffer[TX_BUFFER_SIZE];

unsigned char rx_id;
unsigned char rx_command;
unsigned char rx_data_h;
unsigned char rx_data_l;

unsigned char var_tim2_ovf;
unsigned char var_tcnt2;

signed int var_pos_set;
signed int var_pos_get;
signed int var_vel_set;
signed int var_vel_get;
signed int var_pwm_motor;
unsigned int var_pwm_vibro;

//Процедуры и функции////////////////////////////////////////////////////////////////////////////
//Инициализировать контроллер
void ini(void)
{
//UBRR0L=BYTEL(const_baund);
//OCR2A=BYTEL(const_pwm_ref);
//UBRR0L=(char)(FREQ_CLK/(16*const_baund*100))-1;
//TCCR0B=PRE_DIV_CLK_TIM0;
//OCR0A=(char)((FREQ_CLK/2*mas_div[PRE_DIV_CLK_TIM0]*FREQ_ENC_SCAN)-1);

// Crystal Oscillator division factor: 1
#pragma optsize-
CLKPR=0x80;
CLKPR=0x00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif

// Input/Output Ports initialization
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=Out Func1=Out Func0=In
// State7=P State6=P State5=P State4=P State3=P State2=0 State1=0 State0=P
PORTB=0xF9;
DDRB=0x06;

// Port C initialization
// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State6=P State5=P State4=P State3=P State2=T State1=T State0=T
PORTC=0x78;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=Out Func5=Out Func4=Out Func3=In Func2=In Func1=Out Func0=In
// State7=P State6=0 State5=0 State4=0 State3=P State2=P State1=0 State0=P
PORTD=0x8D;
DDRD=0x72;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 288,000 kHz
// Mode: Fast PWM top=0xFF
// OC0A output: Non-Inverted PWM
// OC0B output: Non-Inverted PWM
TCCR0A=0xA3;
TCCR0B=0x03;
TCNT0=0x00;
OCR0A=0x00;
OCR0B=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 18432,000 kHz
// Mode: Fast PWM top=0x03FF
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xA3;
TCCR1B=0x09;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 2304,000 kHz
// Mode: Normal top=0xFF
// OC2A output: Disconnected
// OC2B output: Disconnected
ASSR=0x00;
TCCR2A=0x00;
TCCR2B=0x02;
TCNT2=0x00;
OCR2A=0x00;
OCR2B=0x00;

// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Any change
// INT1: On
// INT1 Mode: Any change
// Interrupt on any change on pins PCINT0-7: Off
// Interrupt on any change on pins PCINT8-14: Off
// Interrupt on any change on pins PCINT16-23: Off
EICRA=0x05;
EIMSK=0x03;
EIFR=0x03;
PCICR=0x00;

// Timer/Counter 0 Interrupt(s) initialization
TIMSK0=0x01;

// Timer/Counter 1 Interrupt(s) initialization
TIMSK1=0x00;

// Timer/Counter 2 Interrupt(s) initialization
TIMSK2=0x01;

// ADC initialization
// ADC Clock frequency: 576,000 kHz
// ADC Voltage Reference: AREF pin
// ADC Auto Trigger Source: ADC Stopped
// Only the 8 most significant bits of
// the AD conversion result are used
// Digital input buffers on ADC0: On, ADC1: On, ADC2: On, ADC3: On
// ADC4: On, ADC5: On
DIDR0=0x00;
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0x85;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART0 Mode: Asynchronous
// USART Baud Rate: 57600
UCSR0A=0x00;
UCSR0B=0xD8;
UCSR0C=0x06;
UBRR0H=0x00;
UBRR0L=0x13;

// Global enable interrupts
#asm("sei")
}

//Конфигурировать контроллер
void config(void)
{
OCR0A=BYTEL(const_pwm_ref);
UBRR0L=BYTEL(const_baund);
reg_ctrl=BYTEL(const_config);
reg_flag=BYTEH(const_config);
const_id_bdc=BYTEH(const_id);
const_id_indiv=BYTEL(const_id);
}

//Сбросить контроллер
void resetchip(void)
{
#asm("cli")

DO_DISCON_OCR1AB;
CLRBIT(PORT_PWM, MASK_PWMA);
CLRBIT(PORT_PWM, MASK_PWMB);   

count_tim0_ovf=0;
count_tim2_ovf=0;

rx_wr_index=0;
rx_rd_index=0;
rx_counter=0;
tx_wr_index=0;
tx_rd_index=0;
tx_counter=0;

rx_id=0;
rx_command=0;
rx_data_h=0;
rx_data_l=0;

var_tim2_ovf=0;
var_tcnt2=0;

var_pos_set=0;
var_pos_get=0;
var_vel_set=0;
var_vel_get=0;
var_pwm_motor=0;
var_pwm_vibro=0;

const_pos_range=0;
const_k_norm=0;
const_vel_max=0;

#asm("rjmp   0")
}

//Cохранить константы в EEPROM
void saveeep(void)
{
///*
eep_k_prop=const_k_prop;
eep_k_int=const_k_int;
eep_k_vel=const_k_vel;
eep_pwm_min=const_pwm_min;
eep_pwm_max=const_pwm_max;
eep_soft_lim=const_soft_lim;
eep_pwm_ref=const_pwm_ref;
eep_config=const_config;
eep_baund=const_baund;
eep_id=const_id;
//*/
}

//Прочитать константы по умолчанию
void loaddef(void)
{
const_k_prop=DEFAULT_K_PROP;
const_k_int=DEFAULT_K_INT;
const_k_vel=DEFAULT_K_VEL;
const_pwm_min=DEFAULT_PWM_MIN;
const_pwm_max=DEFAULT_PWM_MAX;
const_soft_lim=DEFAULT_SOFT_LIM;
const_pwm_ref=DEFAULT_PWM_REF;
const_config=DEFAULT_CONFIG;
const_baund=DEFAULT_BAUND;
const_id=DEFAULT_ID;
}

//Загрузить константы из EEPROM
void loadeep(void)
{
///*
const_k_prop=eep_k_prop;
const_k_int=eep_k_int;
const_k_vel=eep_k_vel;
const_pwm_min=eep_pwm_min;
const_pwm_max=eep_pwm_max;
const_soft_lim=eep_soft_lim;
const_pwm_ref=eep_pwm_ref;
const_config=eep_config;
const_baund=eep_baund;
const_id=eep_id;
//*/
}

//Организовать буфер приема
void rxfifo(void)
{
static char status,data;
status=UCSR0A;
data=UDR0;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
   {
   rx_buffer[rx_wr_index++]=data;
   if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
   if (++rx_counter == RX_BUFFER_SIZE)
      {
      rx_counter=0;
      SETBIT(reg_flag, FLAG_RX_BUF_OVF);
      }
   }
}

//Организовать буфер отправки
void txfifo(void)
{
if (tx_counter)
   {
   --tx_counter;
   UDR0=tx_buffer[tx_rd_index++];
   if (tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
   }
else CLRBIT(PORT_TXEN, MASK_TXEN);
}

/*
//Получить байт из буфера приема USART
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}

//Отправить байт в буфер отправки USART
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSR0A & DATA_REGISTER_EMPTY)==0))
   {
   tx_buffer[tx_wr_index++]=c;
   if (tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
   ++tx_counter;
   }
else
   {
   SETBIT(PORT_TXEN, MASK_TXEN);
   UDR0=c;
   };
#asm("sei")
}
*/

//Получить результат АЦП
unsigned char read_adc(unsigned char adc_input)
{
ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
//delay_us(10);   // Delay needed for the stabilization of the ADC input voltage
ADCSRA|=0x40;   // Start the AD conversion
while ((ADCSRA & 0x10)==0);  // Wait for the AD conversion to complete
ADCSRA|=0x10;
return ADCH;
}

//Принять запрос из буфера приема 
void request(void) //на выходе rx_id, rx_command, rx_data_h, rx_data_l
{
if (rx_counter>=MESSAGE_SIZE)                                   //если в буфере приема накопилось достаточно данных, то 
    {   
    while ((rx_counter>0)&&(getchar()!=HEADER)) {};             //считать байты из буфера пока не найден байт HEADER
    if (rx_counter>=MESSAGE_SIZE-1)                             //если после этого осталось достаточно данных, то
        {
        rx_id=getchar();                                        //считать байт идентификатора
        rx_command=getchar();                                   //считать код команды
        rx_data_h=getchar();                                    //считать ст. байт данных
        rx_data_l=getchar();                                    //считать мл. байт данных
        if ((rx_id==const_id_bdc)||(rx_id==const_id_indiv))  //если считанный байт идентификатора совпадает с личным или широковещательным идентификатором, то
            SETBIT(reg_flag, FLAG_REQUEST_NEW);          //установить флаг нового запроса
        }
    }
}

//Обработать запрос
void task(unsigned char id, unsigned char command, unsigned char data_h, unsigned char data_l)
{
if (id==const_id_bdc)
    {
    switch (command)
        {
        case WRITE_NORM_POSITION:        //Записать координату для 2х осей
            {
            switch (const_id_indiv)
                {
                case ID_INDIV_DOF1:
                    {
                    var_pos_set=data_h*const_k_norm;
                    break;
                    };   
                case ID_INDIV_DOF2:
                    {
                    var_pos_set=data_l*const_k_norm;
                    break;
                    };
                };
            break;
            };         
               
        case RESET_CONTROLLER:    //Выполнить сброс контроллера
            {
            resetchip();
            break;
            };                   
        };       
    }
if (id==const_id_indiv)
    {
    switch (command)
        {
        //////////////////////////////////////////////////////////////////////   
        case WRITE_NORM_POSITION:
            {
            var_pos_set=data_l*const_k_norm;
            var_pwm_vibro=data_h;
            OCR0B=var_pwm_vibro;
            break;
            };
        case WRITE_POSITION: 
            {
            putchar(HEADER); putchar(id); putchar(READ_POSITION); putchar(BYTEH(var_pos_get)); putchar(BYTEL(var_pos_get));
            var_pos_set=WORD(data_h,data_l);
            break;
            };
        case WRITE_VELOCITY:
            {
            putchar(HEADER); putchar(id); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
            var_vel_set=WORD(data_h,data_l);
            break;
            };
        case WRITE_PWM_MOTOR: 
            {
            putchar(HEADER); putchar(const_id_indiv); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
            var_pwm_motor=WORD(data_h,data_l);
            break;
            };
        case WRITE_PWM_VIBRO:
            {
            putchar(HEADER); putchar(id); putchar(READ_PWM_VIBRO); putchar(0x00); putchar(var_pwm_vibro);
            var_pwm_vibro=data_l; OCR0B=var_pwm_vibro;
            break;
            };

        //////////////////////////////////////////////////////////////////////   
        case READ_POSITION:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(var_pos_get)); putchar(BYTEL(var_pos_get));
            break;
            };
        case READ_VELOCITY:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
            break;
            };
        case READ_PWM_MOTOR:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(var_pwm_motor)); putchar(BYTEL(var_pwm_motor));
            break;
            };

        case READ_PWM_VIBRO:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(0x00); putchar(BYTEL(var_pwm_vibro));
            break;
            };

        //////////////////////////////////////////////////////////////////////   
        case WRITE_KOEF_PROP:
            {
            putchar(HEADER); putchar(id); putchar(READ_KOEF_PROP); putchar(BYTEH(const_k_prop)); putchar(BYTEL(const_k_prop));
            const_k_prop=WORD(data_h, data_l);
            break;
            };
        case WRITE_KOEF_INT:
            {
            putchar(HEADER); putchar(id); putchar(READ_KOEF_INT); putchar(BYTEH(const_k_int)); putchar(BYTEL(const_k_int));
            const_k_int=WORD(data_h, data_l);
            break;
            };
        case WRITE_KOEF_VEL:
            {
            putchar(HEADER); putchar(id); putchar(READ_KOEF_VEL); putchar(BYTEH(const_k_vel)); putchar(BYTEL(const_k_vel));
            const_k_vel=WORD(data_h, data_l);
            break;
            };
        case WRITE_PWM_MIN:
            {
            putchar(HEADER); putchar(id); putchar(READ_PWM_MIN); putchar(BYTEH(const_pwm_min)); putchar(BYTEL(const_pwm_min));
            const_pwm_min=WORD(data_h, data_l);
            break;
            };
        case WRITE_PWM_MAX:
            {
            putchar(HEADER); putchar(id); putchar(READ_PWM_MAX); putchar(BYTEH(const_pwm_max)); putchar(BYTEL(const_pwm_max));
            const_pwm_max=WORD(data_h, data_l);
            break;
            };
        case WRITE_SOFT_LIM:
            {
            putchar(HEADER); putchar(id); putchar(READ_SOFT_LIM); putchar(BYTEH(const_soft_lim)); putchar(BYTEL(const_soft_lim));
            const_soft_lim=WORD(data_h, data_l);
            break;
            };
        case WRITE_PWM_REF:
            {
            putchar(HEADER); putchar(id); putchar(READ_PWM_REF); putchar(0x00); putchar(BYTEL(const_pwm_ref));
            const_pwm_ref=data_l; OCR0A=const_pwm_ref;
            break;
            };
        case WRITE_CONFIG:
            {
            putchar(HEADER); putchar(id); putchar(READ_CONFIG); putchar(reg_flag); putchar(reg_ctrl);
            const_config=WORD(data_h, data_l);
            break;
            };
        case WRITE_USART_BAUND:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(0x00); putchar(UBRR0L);
            const_baund=data_l;
            break;
            };
        case WRITE_ID:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(const_id_bdc); putchar(const_id_indiv);
            const_id=WORD(data_h, data_l);
            break;
            };

        ////////////////////////////////////////////////////////////////////
        case READ_KOEF_PROP:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_prop)); putchar(BYTEL(const_k_prop));
            break;
            };
        case READ_KOEF_INT:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_int)); putchar(BYTEL(const_k_int));
            break;
            };
        case READ_KOEF_VEL:
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_vel)); putchar(BYTEL(const_k_vel));
            break;
            };
        case READ_PWM_MIN:             
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pwm_min)); putchar(BYTEL(const_pwm_min));
            break;
            };
        case READ_PWM_MAX:             
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pwm_max)); putchar(BYTEL(const_pwm_max));
            break;
            };
        case READ_SOFT_LIM:             
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_soft_lim)); putchar(BYTEL(const_soft_lim));
            break;
            };
        case READ_PWM_REF:             
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pwm_ref)); putchar(BYTEL(const_pwm_ref));
            break;
            };
        case READ_CONFIG:       
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(reg_flag); putchar(reg_ctrl);
            break;
            };
        case READ_POSITION_RANGE:     
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pos_range)); putchar(BYTEL(const_pos_range));
            break;
            };
        case READ_KOEF_NORM:         
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_norm)); putchar(BYTEL(const_k_norm));
            break;
            };
        case READ_VELOCITY_MAX:         
            {
            putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_vel_max)); putchar(BYTEL(const_vel_max));
            break;
            };

        //////////////////////////////////////////////////////////////////////
        case RESET_CONTROLLER:
            {
            if (WORD(data_h, data_l)&0xFFFF)
                {
                putchar(HEADER); putchar(id); putchar(command); putchar(0xFF); putchar(0xFF);
                resetchip();
                };           
            break;
            };                   
        case SAVE_CONST_TO_EEPROM:
            {
            if (WORD(data_h, data_l)&0xFFFF)
                {
                putchar(HEADER); putchar(id); putchar(command); putchar(0xFF); putchar(0xFF);
                saveeep();
                resetchip();
                };
            break;
            };
        case LOAD_DEFAULT_CONST:
            {
            if (WORD(data_h, data_l)&0xFFFF)
                {           
                putchar(HEADER); putchar(id); putchar(command); putchar(0xFF); putchar(0xFF);
                loaddef();
                config();
                };
            break;
            };   
        }   
    };
}

//Организовать связь по RS485
void simlink485(void)
{
if (tx_counter==0)                                          //если буфер отправки пуст, то
    {
    request();                                              //проверить буфер приема на наличие нового запроса
    if (reg_flag&FLAG_REQUEST_NEW)                          //если новый запрос, то
        {
        task(rx_id, rx_command, rx_data_h, rx_data_l);      //выполнить задание
        CLRBIT(reg_flag, FLAG_REQUEST_NEW);                 //сбросить флаг нового запроса
        };
    };
}

//Сканировать компаратор
void ovl(void)
{
if (PORT_OVL&MASK_OVL) {#asm("cli") OCR1AH=0; OCR1AL=0; OCR1BH=0; OCR1BL=0; #asm("sei")};
}

//Сканировать концевики
void limit(void)
{
if (PORT_LIM&MASK_LIM)
    {
    DO_DISCON_OCR1AB;//отключить OCR1A и отключить OCR1B
    CLRBIT(PORT_PWM, MASK_PWMA);
    CLRBIT(PORT_PWM, MASK_PWMB);     
    while(1){#asm("cli");};
    }
}

//Сканировать енкодер 
void encoder(void)   //на выходе: var_position_get, var_tcnt2, var_count_tim2_ovf
{
static unsigned char enc_last, enc_now, enc_dir_last, enc_dir_now;

enc_now=PORT_ENC_SHIFT;                                     //прочитать состояние энкодера и сдвинуть в начало байта
enc_dir_now=(enc_last&1)^((enc_now&2)>>1);                       //определить направление вращения
enc_last=enc_now;                                               //текущее состояние энкодера стало прошлым
if (!enc_dir_now) var_pos_get++;                                //обновить текущую позицию в зависимости от направления вращения
else  var_pos_get--;


if ((enc_dir_last==enc_dir_now)&&(reg_flag&FLAG_EN_CALC_VEL_GET))  //если направление вращения не поменялось, то
    {
    var_tcnt2=TCNT2;                                            //присвоить глобальной переменной значение таймера2
    var_tim2_ovf=count_tim2_ovf;                                //присвоить глобальной переменной число переполнений таймера2
    };
enc_dir_last=enc_dir_now;                                       //текущее направление вращения стало прошлим
CLRBIT(reg_flag, FLAG_EN_CALC_VEL_GET);
count_tim2_ovf=0;                                               //сбросить счетчик переполнений таймера2
TCNT2=0;                                                        //сбросить таймер2

SETBIT(EIFR, 0b00000011);                                       //сбросить флаги внешних прерываний
SETBIT(TIFR2, 0b00000001);                                      //сбросить флаг переполнения таймера2
}

//Расчитать задание скорости
signed int calcvelset(signed int pos_set, signed int pos_get, unsigned int k_vel, unsigned int vel_max)
{
static unsigned int vel_set;
vel_set=k_vel*(pos_set-pos_get);                     
if (vel_set<vel_max) return vel_set;
else return vel_max;
}

//Расчитать фактическую скорость
unsigned int calcvelget(unsigned char count, unsigned char count_ovf)
{
return FREQ_TIM2/(count+(long int)count_ovf*255);  //VEL=FREQ_CLC/Ncount частота следования квадратур энкодера
}

//Расчитать ШИМ
signed int calcpwm(signed int vel_set, signed int vel_get, unsigned int k_prop, unsigned int k_int, unsigned int pwm_min, unsigned int pwm_max)
{
static signed int pwm;
static signed int vel_error;
static signed int vel_sum_error;

vel_error=ABS(vel_set)-vel_get;                                                                   
if ((pwm>pwm_min)&&(pwm<pwm_max)) vel_sum_error=vel_sum_error+vel_error;                       
pwm=(((long int)k_prop*vel_error)+((long int)k_int*vel_sum_error))/1000;    //расчет воздействия

if ((pwm>pwm_min)&&(pwm<pwm_max))
    {
    if (vel_set>0) return pwm;
    else return -pwm;   
    }
else
    {
    if (pwm>=pwm_max)
        {
        if (vel_set>0) return pwm_max;
        else return -pwm_max;   
        }
    else return 0;
    }
}

//Установить ШИМ
void setmotor(signed int pwm)
{
static unsigned int pwm_abs;

pwm_abs=ABS(pwm);
if (pwm>0) {#asm("cli") OCR1BH=0; OCR1BL=0; OCR1AH=BYTEH(pwm_abs); OCR1AL=BYTEL(pwm_abs); #asm("sei")}
else {#asm("cli") OCR1AH=0; OCR1AL=0; OCR1BH=BYTEH(pwm_abs); OCR1BL=BYTEL(pwm_abs); #asm("sei")};
}

//Выполнять калибровку
void calib(void)
{
static unsigned char step;
switch (step)
    {
    case 0:                                   
        {
        step=1;
        var_pos_get=0;
        var_pwm_motor=-(const_pwm_max/2);
        setmotor(var_pwm_motor);
        break;
        };
    case 1:                                   
        {
        if (PORT_LIM&MASK_LIM_L)
            {
            step=2;
            var_pos_get=0;
            var_pwm_motor=const_pwm_max/2;
            setmotor(var_pwm_motor);
            delay_ms(100);
            };
        break;
        };
    case 2:                                   
        {
        if (PORT_LIM&MASK_LIM_R)
            {
            step=3;
            var_pwm_motor=0;
            setmotor(var_pwm_motor);
            delay_ms(100);
            const_pos_range=var_pos_get-2*const_soft_lim;
            const_k_norm=const_pos_range/256;
            var_pos_get=const_pos_range+const_soft_lim;
            };
        break;
        };
    case 3:                                   
        {
        step=4;
        var_pwm_motor=-(const_pwm_max);
        setmotor(var_pwm_motor);
        break;
        };
    case 4:                                   
        {
        if (var_pos_get<(const_pos_range/2))
            {
            step=0;
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            if (var_vel_get>const_vel_max) const_vel_max=var_vel_get;               
            var_pwm_motor=0;
            setmotor(var_pwm_motor);
            SETBIT(reg_flag, FLAG_CALIB_COMPL);     //установить флаг "калибровка завершена"
            }
        else
            {
            if (PORT_LIM&MASK_LIM_L)
                {
                step=0;
                var_pwm_motor=0;
                setmotor(var_pwm_motor);
                };
            };                 
        break;
        };
    };
}

//Задать позицию потенциометром
signed int adcposset(unsigned char pin, unsigned int k_norm)
{
return k_norm*read_adc(pin);
}

//Задать скорость потенциометром
signed int adcvelset(unsigned char pin, unsigned int k_norm_vel)
{
return k_norm_vel*(read_adc(pin)-127);
}

//Задать ШИМ потенциометром
signed int adcpwm(unsigned char pin, unsigned int k_norm_pwm, unsigned char adc_pwm_min, unsigned char adc_pwm_max)
{
static unsigned int adc;
adc=read_adc(pin);

if ((adc>adc_pwm_min)&&(adc<adc_pwm_max)) return 0;
else return k_norm_pwm*(adc-127);
}

//Прерывания///////////////////////////////////////////////////////////////////////////////
//Внешнее прерывание при изменении состояния канала А энкодера
interrupt [EXT_INT0] void ext_int0_isr(void)
{
encoder();
}

//Внешнее прерывание при изменении состояния канала В энкодера
interrupt [EXT_INT1] void ext_int1_isr(void)
{
encoder();
}

//Прерывание по переполнению таймера0 (1125Гц), для задачи частоты расчета ШИМ
interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{
count_tim0_ovf++;
if (!EIFR) SETBIT(reg_flag, FLAG_EN_CALC_VEL_GET);     //если внешних прерываний не было, то разрешить захват таймера2 для расчета скорости
}

//Прерывание по переполнению таймера2 (9000Гц), для расчета скорости вращения энкодера
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{
if (count_tim2_ovf<254) count_tim2_ovf++;
}

//Прерывание от USART по приему байта
interrupt [USART_RXC] void usart_rx_isr(void)
{
rxfifo();
if (!EIFR) SETBIT(reg_flag, FLAG_EN_CALC_VEL_GET);     //если внешних прерываний не было, то разрешить захват таймера2 для расчета скорости 
}

//Прерывание от USART после передачи байта
interrupt [USART_TXC] void usart_tx_isr(void)
{
txfifo();
if (!EIFR) SETBIT(reg_flag, FLAG_EN_CALC_VEL_GET);     //если внешних прерываний не было, то разрешить захват таймера2 для расчета скорости
}

//Программа//////////////////////////////////////////////////////////////////////////////
void main(void)
{
static char count_calc;
static char ctrl_after_calib;

ini();                                                          //Выполнить инициализацию контроллера
loadeep();                                                      //Загрузить константы из EEPROM
config();                                                       //Выполнить конфигурирование

if (!(PORT_DEF&MASK_DEF))                                       //Если при включении нажата кнопка DEF, то управление от ADC
    {
    delay_ms(100);
    if (!(PORT_DEF&MASK_DEF))
        ctrl_after_calib=CTRL_ADC_POS_VEL_PWM;                 
    }
else  ctrl_after_calib=CTRL_USART_POS_VEL_PWM;                  //иначе управление от USART

if (!(PORT_DEF&MASK_DEF))                                       //Если DEF нажата более 2с, то
    {
    delay_ms(2000);
    if (!(PORT_DEF&MASK_DEF))
        {
        loaddef();                                              //использовать константы "по умолчанию"
        config();                                               //выполнить конфигурирование
        ctrl_after_calib=CTRL_USART_POS_VEL_PWM;                //управление от USART
        };
    };
                                                     
while(1)                                                        //Бесконечный программный цикл, переключение режимов контроллера через регистр reg_ctrl
    {           
    while (reg_ctrl==CTRL_CALIB_POS)                            //Цикл калибровки
        {
        ovl();
        calib();
        if (reg_flag&FLAG_CALIB_COMPL)
            {
            var_pos_set=const_pos_range/2;
            reg_ctrl=ctrl_after_calib;
            //расчет констант для управления от ADC
            const_adc_norm_vel=const_vel_max/128;
            const_adc_norm_pwm=const_pwm_max/128;
            const_adc_pwm_min=127-(const_pwm_min/const_adc_norm_pwm);
            const_adc_pwm_max=127+(const_pwm_min/const_adc_norm_pwm);           
            };
        };

    while (reg_ctrl==CTRL_USART_POS_VEL_PWM)                    //Стабилизация принятой по USART позиции по скорости
        {
        simlink485();
        ovl();
        limit();
        if (count_tim0_ovf==DIV_CALC)                       
            {   
            count_tim0_ovf=0;
            var_vel_set=calcvelset(var_pos_set, var_pos_get, const_k_vel, const_vel_max);
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
            setmotor(var_pwm_motor);
            };         
        };
           
    while (reg_ctrl==CTRL_USART_VEL_PWM)                        //Стабилизация принятой по USART скорости
        {
        simlink485();       
        ovl();
        limit();
        if (count_tim0_ovf==DIV_CALC)
            {   
            count_tim0_ovf=0;
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
            setmotor(var_pwm_motor);
            };       
        };
           
    while (reg_ctrl==CTRL_USART_PWM)                            //Стабилизация принятой по USART ШИМ
        {
        simlink485();
        ovl();
        limit();
        if (count_tim0_ovf==DIV_CALC)
            {   
            count_tim0_ovf=0;
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            setmotor(var_pwm_motor);
            };       
        };       
       
    while (reg_ctrl==CTRL_ADC_POS_VEL_PWM)                      //Стабилизация позиции заданной через ADC (для выхода из цикла требуется сброс контроллера)
        {
        ovl();
        limit();
        if (count_tim0_ovf==DIV_CALC)
            {   
            count_tim0_ovf=0;
            var_pos_set=adcposset(PIN_POT, const_k_norm);
            var_vel_set=calcvelset(var_pos_set, var_pos_get, const_k_vel, const_vel_max);
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
            setmotor(var_pwm_motor);
            if (count_calc++==DIV_SEND_ADC)
                {
                count_calc=0;
                printf("Ps %d",var_pos_set); printf("  Pg %d",var_pos_get); printf("\n\r");
//                putchar(HEADER); putchar(const_id_bdc); putchar(WRITE_POSITION); putchar(BYTEH(var_pos_set)); putchar(BYTEL(var_pos_set));
//                putchar(HEADER); putchar(const_id_bdc); putchar(READ_POSITION); putchar(BYTEH(var_pos_get)); putchar(BYTEL(var_pos_get));           
                }
            else
                {
                if (!(PORT_DEF&MASK_DEF))
                    {
                    while (!(PORT_DEF&MASK_DEF)){};
                    delay_ms(100);
                    reg_ctrl=CTRL_ADC_VEL_PWM;
                    };           
                };
            };         
        };
           
    while (reg_ctrl==CTRL_ADC_VEL_PWM)                          //Стабилизация скорости заданной через ADC (для выхода из цикла требуется сброс контроллера)
        {       
        ovl();
        limit();
        if (count_tim0_ovf==DIV_CALC)
            {   
            count_tim0_ovf=0;
            var_vel_set=adcvelset(PIN_POT, const_adc_norm_vel);
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
            setmotor(var_pwm_motor);
            if (count_calc++==DIV_SEND_ADC)
                {
                count_calc=0;
                printf("Vs %d",var_vel_set); printf("  Vg %d",var_vel_get); printf("\n\r");
//                putchar(HEADER); putchar(const_id_bdc); putchar(WRITE_VELOCITY); putchar(BYTEH(var_vel_set)); putchar(BYTEL(var_vel_set));
//                putchar(HEADER); putchar(const_id_bdc); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
                }
            else
                {
                if (!(PORT_DEF&MASK_DEF))
                    {
                    while (!(PORT_DEF&MASK_DEF)){};
                    delay_ms(100);
                    reg_ctrl=CTRL_ADC_PWM;
                    };           
                };
            };       
        };
           
    while (reg_ctrl==CTRL_ADC_PWM)                              //Стабилизация ШИМ заданной через ADC (для выхода из цикла требуется сброс контроллера)
        {
        ovl();
        limit();
        if (count_tim0_ovf==DIV_CALC)
            {   
            count_tim0_ovf=0;
            var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
            var_pwm_motor=adcpwm(PIN_POT, const_adc_norm_pwm, const_adc_pwm_min, const_adc_pwm_max);
            setmotor(var_pwm_motor);
            if (count_calc++==DIV_SEND_ADC)
                {
                count_calc=0;
                printf("PWM %d",var_pwm_motor); printf("  Vg %d",var_vel_get); printf("\n\r");
//                putchar(HEADER); putchar(const_id_bdc); putchar(WRITE_PWM_MOTOR); putchar(BYTEH(var_pwm_motor)); putchar(BYTEL(var_pwm_motor));
//                putchar(HEADER); putchar(const_id_bdc); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
                }
            else
                {
                if (!(PORT_DEF&MASK_DEF))
                    {
                    while (!(PORT_DEF&MASK_DEF)){};
                    delay_ms(100);
                    reg_ctrl=CTRL_ADC_POS_VEL_PWM;
                    };           
                };               
            };       
        };   
    }
}