; motor functions
-;include "../include/m128def.inc"
-
-.equ MOT_DIR_L = 0x02
-.equ MOT_DIR_R = 0x03
-.equ MOT_PWR_L = 0x04
-.equ MOT_PWR_R = 0x07
-
-;.def tmp = r17
+.equ MOT_1_1 = 0x04
+.equ MOT_1_2 = 0x02
+.equ MOT_2_1 = 0x07
+.equ MOT_2_2 = 0x03
MOTOR_INIT:
; ports -> output
- in tmp,DDRB
- sbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
- sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
- out DDRB,tmp
+ in tmp1,DDRB
+ sbr tmp1,(1<<MOT_1_1)|(1<<MOT_1_2)
+ sbr tmp1,(1<<MOT_2_1)|(1<<MOT_2_2)
+ out DDRB,tmp1
MOTOR_STOP:
; output zero
- in tmp,PORTB
- cbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
- cbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
- out PORTB,tmp
+ in tmp1,PORTB
+ cbr tmp1,(1<<MOT_1_1)|(1<<MOT_1_2)
+ cbr tmp1,(1<<MOT_2_1)|(1<<MOT_2_2)
+ out PORTB,tmp1
ret
MOTOR_FWD:
- in tmp,PORTB
- sbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
- sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
- out PORTB,tmp
+ in tmp1,PORTB
+ sbr tmp1,(1<<MOT_1_1)|(1<<MOT_2_1)
+ cbr tmp1,(1<<MOT_1_2)|(1<<MOT_2_2)
+ out PORTB,tmp1
ret
MOTOR_BWD:
- in tmp,PORTB
- cbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
- sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
- out PORTB,tmp
+ in tmp1,PORTB
+ sbr tmp1,(1<<MOT_1_2)|(1<<MOT_2_2)
+ cbr tmp1,(1<<MOT_1_1)|(1<<MOT_2_1)
+ out PORTB,tmp1
ret
MOTOR_RIGHT:
- in tmp,PORTB
- sbr tmp,(1<<MOT_DIR_L)
- cbr tmp,(1<<MOT_DIR_R)
- sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
- out PORTB,tmp
+ in tmp1,PORTB
+ sbr tmp1,(1<<MOT_2_2)|(1<<MOT_1_1)
+ cbr tmp1,(1<<MOT_2_1)|(1<<MOT_1_2)
+ out PORTB,tmp1
ret
MOTOR_LEFT:
- in tmp,PORTB
- sbr tmp,(1<<MOT_DIR_R)
- cbr tmp,(1<<MOT_DIR_L)
- sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
- out PORTB,tmp
+ in tmp1,PORTB
+ sbr tmp1,(1<<MOT_1_2)|(1<<MOT_2_1)
+ cbr tmp1,(1<<MOT_1_1)|(1<<MOT_2_2)
+ out PORTB,tmp1
ret