Sterowanie silnikiem BLDC z dysku twardego 5400 obr/min - Atmega8, BS170, kod ASM

INCLUDE "m8def.inc"

.EQU SYS_FREQ = 1 ;CZESTOTLIWOSC ZEGARA PROCKA [ MHZ ]
.EQU xx = 1


Set_Port_B_As_Output:

      ldi R16 ,0xFF
      out DDRB , R16

rjmp FORWARD

walkDelay:
   ldi R18, 1         ;T1 used as delay 2nd count
   ldi R19, 1         ;T2 used as delay 3nd count
delay_1:
   dec R18
   brne delay_1
   dec R19
   brne delay_1
   dec R20          ;temp must be preset as
   brne delay_1          ; delay master count
   ret
         

FORWARD:                     ; OBROT DO PRZODU
   
      ldi R16 , 0b10000011
      out PORTB , R16
      ldi   R20, xx
      rcall walkDelay

      ldi R16 , 0b01000101
      out PORTB , R16
      ldi   R20, xx   
      rcall walkDelay

      ldi R16 , 0b00101001
      out PORTB , R16
      ldi   R20, xx
      rcall walkDelay


      RJMP PETLA

BACKWARD:                     ; OBROT DO TYLU
   
      ldi R16 , 0b00111111
      out PORTB , R16

   
      ldi R16 , 0b01011111
      out PORTB , R16

      ldi R16 , 0b10010111
      out PORTB , R16
      
      RJMP PETLA

STOP:                        ;ZATRZYMANIE SILNIKA
      ldi R16 , 0b00001111
      out PORTB , R16   


      LDI R16 , 0b00000001
      out PORTB , R16
               
      
PETLA:
   
      rjmp FORWARD
   
   rjmp PETLA


;Set_Port_B_Output
;Put_On_PORTB 

#include <avr/io.h>
#define F_CPU 1000000                        //do określenia częstotliwości procka dla "delay"
#include <avr/delay.h>
#include <math.h>

#define LED_ON PORTB|=(1<<PB0)                  //PORTB|=1<<PB1 - wysyła "1" na wyjście portu
#define LED_OFF PORTB&=(~1<<PB0)
#define CONTROL_LED_ON PORTD|=(1<PD0)
#define CONTROL_LED_OFF PORTD&=(~1<PD0)

void init_port(void)
   {
   DDRB = 0xff;
   PORTB = 0x00;
   }
   
void FORWARD(int czas)
   {
//   PORTB&=0b10000001;
//   _delay_ms(czas);
//   PORTB&=0b01000010;
//   _delay_ms(czas);
//   PORTB&=0b00100100;
//   _delay_ms(czas);
   PORTB|=(1<<PB7);
   PORTB&=(0<<PB6);
   PORTB&=(0<<PB5);
   _delay_ms(czas);
   
   PORTB&=(0<<PB7);
   PORTB|=(1<<PB6);
   PORTB&=(0<<PB5);
   _delay_ms(czas);
   
   PORTB&=(0<<PB7);
   PORTB&=(0<<PB6);
   PORTB|=(1<<PB5);
   _delay_ms(czas);
   
   }

void STOP(int czas)
   {
   if (czas <= 20)
      {
      PORTB&=0b11100111;
      _delay_ms(czas);
      }
   }

int main(void)
{
   init_port();
   for(;;)
      {
      FORWARD(10);
      }
   return 0;
}