Переделать С на Ассемблер (микроконтроллер 8535)

[FrenkiFix]

void PIDregulating (void)
{
  static char is_first_call = 1;
  static float U, I, Kp, Ki, Kd, E, Eprev;
  static PIDstep PID_step;
  static struct Timer next_measuring, next_action;
  static struct difTimer next_PID_step;

  // . инициализация
  if (is_first_call)
  {
    is_first_call = 0;
    I = 0;
    Eprev = 0;
    PID_step = ReceivingE;
    // ... инициализация коэффициентов Kp, Ki, Kd
    SetCurrentTime (&next_measuring);
    SetCurrentTime (&next_action);
    // ... задание смещения next_PID_step
  }

  switch (PID_step)
  {
  case ReceivingE:   // получение текущей ошибки
    if (TimeIsLower (&next_measuring))
      break;         // время для измерения не подошло
    AddTime (&next_measuring, &next_PID_step);
    E = ReceivingE ();
    U = 0;

  case ComputeI:    // вычисление интегрального коэф.
    PID_step = ComputeP;
    if (Ki)
    {
      U = I + Ki * E;
      I = U;
      break;
    }

  case ComputeP:    // вычисление пропорционального коэф.
    PID_step = ComputeD;
    if (Kp)
    {
      U += Kp * E;
      break;
    }

  case ComputeD:    // вычисление дифференциального коэф.
    PID_step = MakeAction;
    if (Kd)
    {
      U += Kd * (E - Eprev);
      Eprev = E;
      break;
    }

  case MakeAction:  // воздействие
    CheckU (&U);    // проверка корректности U
    if (TimeIsLower (&next_action))
      break;         // время для воздействия не подошло
    AddTime (&next_action, &next_PID_step);
    MakeAction (U); // воздействие U
    PID_step = ReceivingE;
    break;
  }
}

[FrenkiFix]

[CODE]
.NOLIST
.INCLUDE "8535def.inc"
.LIST
; определить символические имена
.DEF templ = r16
.DEF temph = r17
.DEF saveF = r4
.DEF counT = r21
.DEF rcnt = r22
.EQU Mind = 0x100 ;RAM для кодов индикации
.EQU Madc = 0x090 ;RAM для кода АЦП
.CSEG

; вектора прерываний
.ORG $000
rjmp init ; прерывание по reset
rjmp key_cnt ; прерывание int0
.ORG $009


rjmp adc_cmt ; прерывание АЦП
.ORG $011
;инициализация
init:
ldi templ,low(RAMEND)
ldi temph,high(RAMEND)
out SPH, temph
out SPL, templ ;определить в указателе стека адрес RAMEND
;порты ввода-вывода
ldi templ, 0b11000000
clr temph
out PORTA, temph
out DDRA, templ
ser templ
out PORTB, templ
out DDRB, templ ; порт B на вывод
out PORTD, templ
out DDRD, temph ; порт D на ввод
;внешнее прерывание INT0
ldi templ, (1<<ISC01)|(0<<ISC00)
out MCUCR, templ ; INT0 по падающему фронту
ldi templ, (1<<INT0)
out GIMSK, templ ; разрешение INT0
;прерывание таймера 0
ldi templ, (1<<TOIE0)
out TIMSK, templ ;параметры SPI
ldi templ, 0b11011001
out SPCR, templ ;параметры АЦП
clr templ
out ADMUX, templ ;канал РА0 для АЦП
ldi templ, 0b10001101
out ADCSR, templ
sei ; общее разрешение прерываний
rcall ind_off ; выключение индикации

; бессодержательный бесконечный цикл, может быть заменен любой программой
main: nop
rjmp main

ind_off: ;вывод нулевых байтов в SPI для отключения индикации
clr templ
ldi XL, low(Mind)
ldi XH, high(Mind) ;начальный адрес RAM для кодов SPI
st X+, templ
st X+, templ
st X+, templ
ldi XL, low(Mind) ;восстановление в Х начального адреса
ret

;INT0, кнопки управления
key_cnt: ;сохранение в стеке регистров и флагов
push templ
push temph
push saveF
in saveF, SREG

;обработка сигналов "Стоп", "Пуск"
in templ, PINB
sbrs templ, PB2 ; контроль сигнала СТОП
rjmp cnt_off ; перейти к процедуре СТОП
sbrs templ, PB1 ; контроль сигнала ПУСК
rjmp cnt_on ; перейти к процедуре ПУСК

;если оба сигнала в 1, завершить без изменения режима

key_out: ; завершение с восстановлением из стека флагов и регистров
out SREG, saveF
pop saveF
pop temph
pop templ
reti

cnt_on: ;переход в режим ПУСК
tst rcnt
brne key_out ;завершить по признаку режима ПУСК
sbr rcnt, 0x80 ;установить признак режима ПУСК в rcnt
ldi temph, 0x05
out TCCR0, temph ;запуск таймера циклов контроля
clr counT ;сброс счетчика для формирования цикла
out ADMUX, counT ;канал РА0 для АЦП
sbi ADCSR, ADSC ;запуск АЦП для 1 цикла контроля
rjmp key_out ;завершение процедуры

cnt_off: ;переход в режим СТОП
tst rcnt
breq key_out ;завершить по признаку режима СТОП
clr rcnt ;сбросить признаки режима ПУСК в rcnt
clr temph
out TCCR0, temph ;останов таймера циклов контроля
out TCNT0, temph ;сброс таймера циклов контроля
clr counT ;сброс счетчика для формирования цикла

wait_spi:
cpi XL, low(Mind) ;контроль вывода байтов индикации
breq cnt_end ;продолжение процедуры, если вывод завершен
sei ;разрешить прерывания при ожидании
rjmp wait_spi ;возврат к контролю завершения цикла вывода

cnt_end:
rcall ind_off ;выключить индикацию для СТОП
rjmp key_out ;завершение процедуры
;периодический запуск цикла управления в режиме ПУСК

[FrenkiFix]

cycle:
push saveF
in saveF, SREG
tst rcnt ;контроль режима ПУСК
breq cyc_out ;завершить без запуска для СТОП
cpi counT, 15 ;контроль количества прерываний таймера 0
breq cyc_do ;запуск цикла по 16 прерыванию
inc counT ;инкремент количества прерываний
rjmp cyc_out ;завершить без запуска (<16)

cyc_do:
clr counT ;очистка счетчика прерываний
out ADMUX, counT ;канал РА0 для АЦП
sbi ADCSR, ADSC ;запуск АЦП, начало цикла контроля
;завершение с восстановлением флагов

cyc_out:
out SREG, saveF
pop saveF
reti

;прерывание по завершению преобразования АЦП
adc_cmt:
push templ
push temph
push saveF
in saveF, SREG
tst rcnt
brne make1

make1: ;прием кода
in templ, ADCL ; младший байт кода АЦП
in temph, ADCH ; старший байт кода АЦП
lsr temph
ror templ
lsr temph ; двукратный сдвиг вправо с переносом
ror templ ; для преобразования в 8-битовый формат
in temph, ADMUX ;номер канала ?
tst temph ;
brne tmtr ;перейти к обработке
sbi ADMUX, MUX0 ;установить канал РА1
sbi ADCSR, ADSC ; запустить преобразование в АЦП
sts Madc, templ ; сохранить код канала РА0

tmtr: ;
lds temph, Madc
;считать из RAM код PA0
sub temph, templ ;вычесть коды РА0 и РА1, получив ошибку
rol temph ;сдвинуть влево с переносом вычитания
;анализ кода

[/CODE]

Вот собственно наброски, а что делать дальше хЗ, нужна помощь...