-
Notifications
You must be signed in to change notification settings - Fork 0
/
servo_isr.inc
154 lines (137 loc) · 3.99 KB
/
servo_isr.inc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
;**************************************************************
;* Servo_isr.inc
;* -------------------------------------------------------
;* Macro zur Verwendung in der ISR zur Ansteuerung eines
;* Servo's.
;*
;**************************************************************
;
; M.Zimmermann 22.05.2020
;
;**************************************************************
;
; wird verwendet von: DSA-2.inc, ServoAnsteurungCore.inc
;
;**************************************************************
servo_nr = 0
IsrForServo macro
servo_nr++ ; increment count of instantations
; irgendeine Richtung für Servo gewählt?
btfsc IMove_L_#v(servo_nr)
goto SERVO_ok_#v(servo_nr)
btfsc IMove_R_#v(servo_nr)
goto SERVO_ok_#v(servo_nr)
; '''''''''''''''''''''''''''''''''''''''''''
; Nein: keine Richtung gesetzt = LED ein/aus
btfss Blink_LED
goto switch_to_#v(3-servo_nr)_no_power
decfsz counter_#v(servo_nr),F ; 0,5s vorbei?
goto switch_to_#v(3-servo_nr)_no_power
movlw LOOP_LED_#v(servo_nr)
movwf counter_#v(servo_nr)
btfsc LED_flg_#v(servo_nr)
goto LED_#v(servo_nr)_inv
bsf LED_LI_#v(servo_nr)
bcf LED_RE_#v(servo_nr)
bsf LED_flg_#v(servo_nr)
goto switch_to_#v(3-servo_nr)_no_power
LED_#v(servo_nr)_inv:
bcf LED_LI_#v(servo_nr)
bsf LED_RE_#v(servo_nr)
bcf LED_flg_#v(servo_nr)
goto switch_to_#v(3-servo_nr)_no_power
; '''''''''''''''''''''''''''''''''''''''''''
; Richtung vorhanden: Normalbetrieb
SERVO_ok_#v(servo_nr): ; SERVO ansteuern
bcf T1CON, TMR1ON ; Timer 1 stoppen
movlw .255
movwf TMR1H
#if LONG_PULS==0
clrc
rrf ist_pos_#v(servo_nr), 0 ; geteilt durch 2
addlw OFFSET_05 ; + 0,5ms
skpnc
decf TMR1H, 1
#else
movfw ist_pos_#v(servo_nr)
#endif
addlw OFFSET_05 ; + 0,5ms
xorlw .255
movwf TMR1L
skpnc
decf TMR1H, 1
bsf T1CON, TMR1ON ; Timer 1 starten
bsf SERVO_#v(servo_nr); Impuls ein
#if USE_IBN==1
btfsc IBN_Modus
goto mov_ziel_#v(servo_nr)
; Zielposition vorgeben
movfw srvpos_#v(servo_nr)_l
btfss IMove_L_#v(servo_nr)
movfw srvpos_#v(servo_nr)_r
movwf ziel_pos_#v(servo_nr)
mov_ziel_#v(servo_nr):
#endif
movfw ziel_pos_#v(servo_nr)
; Position über Rampe anfahren
decfsz inc__20_1, F
goto SERVO_istpos#v(servo_nr)
movfw inc__20fix_1
movwf inc__20_1
movfw ziel_pos_#v(servo_nr)
subwf ist_pos_#v(servo_nr),0 ; w(ziel_pos) = ist_pos - ziel_pos
skpnz
goto reached_#v(servo_nr)
; Position noch nicht erreicht
bcf LED_LI_#v(servo_nr)
bcf LED_RE_#v(servo_nr)
bcf EndPos_SV#v(servo_nr)
bcf PowOffTimeStart
movfw inc__dec_1 ; w enthält jetzt den Rampensummand
skpnc
goto decrement_#v(servo_nr)
increment_#v(servo_nr):
; C=1 -> Wert erhöhen
addwf ist_pos_#v(servo_nr),0
skpnc ; mehr als 255 geht nicht
goto reached_#v(servo_nr)
movwf ist_pos_#v(servo_nr)
subwf ziel_pos_#v(servo_nr),0
skpnz
goto reached_#v(servo_nr)
skpc
goto reached_#v(servo_nr)
movfw ist_pos_#v(servo_nr)
goto SERVO_time#v(servo_nr)
decrement_#v(servo_nr):
; C=0 -> Wert erniedrigen
subwf ist_pos_#v(servo_nr),0
skpnz
goto reached_#v(servo_nr) ; weniger als 0 geht nicht
skpc
goto reached_#v(servo_nr) ; weniger als 0 geht nicht
movwf ist_pos_#v(servo_nr)
subwf ziel_pos_#v(servo_nr),0
skpnz
goto reached_#v(servo_nr) ; weniger als 0 geht nicht
skpnc
goto reached_#v(servo_nr) ; weniger als 0 geht nicht
SERVO_istpos#v(servo_nr):
movfw ist_pos_#v(servo_nr)
goto SERVO_time#v(servo_nr)
reached_#v(servo_nr):
movfw ziel_pos_#v(servo_nr)
movwf ist_pos_#v(servo_nr) ; ziel_pos -> ist_pos
; LED einschalten
btfsc IMove_L_#v(servo_nr)
goto set_L_#v(servo_nr)
bsf LED_RE_#v(servo_nr)
bsf EndPos_SV#v(servo_nr)
goto SERVO_time#v(servo_nr)
set_L_#v(servo_nr):
bsf LED_LI_#v(servo_nr)
bsf EndPos_SV#v(servo_nr)
SERVO_time#v(servo_nr):
bsf Power_ON_#v(servo_nr) ; ISR wurde für diesen Servo wenigstens einmal durchlaufen
switch_to_#v(3-servo_nr)_no_power:
endm