-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPicAstro.asm
1510 lines (1473 loc) · 39.4 KB
/
PicAstro.asm
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
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
list p=16f1825
;**************************************************************
;*
;* Pinbelegung
;* ----------------------------------
;* PORTA: 0 ICSPDAT
;* 1 ISCPCLK
;* 2
;* 3 MCLR
;* 4
;* 5 Key
;* PORTC: 0 AN4 X
;* 1 AN5 Y
;* 2 CTS
;* 3 RTS
;* 4 TX
;* 5 RX
;**************************************************************
; Für RFID Erkennung
;**************************************************************
; Includedatei für den 12F1840 einbinden
;
#include <P16f1825.INC>
;
; Configuration
;
; bis 4 MHz: Power on Timer, no Watchdog, XT-Oscillator
__CONFIG _CONFIG1, _WDTE_OFF & _FOSC_INTOSC & _CLKOUTEN_OFF
__CONFIG _CONFIG2, _PLLEN_OFF & _STVREN_ON
;
;**************************************************************
t_clock equ .96
timer_val equ .50
timeout equ .200
ll_cnt equ .100
#define altitude 0x11
#define azimut 0x10
; Altitude x
#define x_dir altitude
#define up_fast 0x09
#define up_medium 0x07
#define up_slow 0x06
#define up_stop 0x00
#define down_fast 0x19
#define down_medium 0x17
#define down_slow 0x16
#define down_stop 0x00
;Azimut y
#define y_dir azimut
#define right_fast 0x09
#define right_medium 0x07
#define right_slow 0x06
#define right_stop 0x00
#define left_fast 0x19
#define left_medium 0x17
#define left_slow 0x16
#define left_stop 0x00
reg_12 equ 0x20
reg_17 equ 0x21
res_x_cnt equ 0x22
res_y_cnt equ 0x23
cmd_x equ 0x24
cmd_y equ 0x25
rate_x equ 0x26
rate_y equ 0x27
snd_c_x equ 0x28
snd_c_y equ 0x29
snd_r_x equ 0x2a
snd_r_y equ 0x2B
check_sum equ 0x2C
rec_poi equ 0x2D
rec_cnt equ 0x2E
snd_target equ 0x2F
timerrec1 equ 0x30
timerrec2 equ 0x31
rec_mask equ 0x32
key_ll equ 0x33
key_cnt equ 0x34
snd_buf equ 0x38;-3FH
rec_buf1 equ 0x40
rec_buf2 equ 0x50
a_reg equ 0x70
b_reg equ 0x71
c_reg equ 0x72
bitleiste1 equ 0x73
x_adc equ 0x00
snd_lock equ 0x01
rec_quit equ 0x02
rec_echo equ 0x03
rec_ping equ 0x04
buf_ovl equ 0x05
buf_2 equ 0x06
buf_1 equ 0x07
bitleiste2 equ 0x74
key_act equ 0x00
streceive equ 0x75
stseriell equ 0x76
stjoystick equ 0x77
stadc equ 0x78
;
; Bank 2
result_x equ 0x20; -0x27
result_y equ 0x24; -0x2f
ORG 0x000 ; processor reset vector
goto init ; go to beginning of program
ORG 0x004 ; interrupt vector location
interrupt:
btfsc INTCON,TMR0IF
goto timer0
int_t0ex: movlw 0x00 ; switch to bank 0
movwf BSR
btfsc PIR1,RCIF
goto rseriell
int_rvex: movlw 0x01 ; switch to bank 1
movwf BSR
btfss PIE1,TXIE
goto int_txex
int_tsqex: movlw 0x00 ; switch to bank 0
movwf BSR
btfsc PIR1,TXIF
goto tseriell
int_txex: retfie
timer0:
;****************************************
; timer
; timeout after 10ms
;
;****************************************
movlw 0x00 ; switch to bank 0
movwf BSR
movlw t_clock
movwf TMR0
BANKSEL LATA
movlw 0x04
xorwf LATA,f
; movlw 0x00
; bcf PORTA,RA2
;
;****************************************
; Timeout Timerrec1
;****************************************
movlb 0x00
movfw timerrec1
btfsc STATUS,Z
bra t0_rec1_end
sublw 0x01
btfss STATUS,Z
bra t0_rec1_end
bcf bitleiste1,snd_lock
banksel PIE1
bcf PIE1,TXIE
clrf BSR
t0_rec1_end: decf timerrec1,1
;****************************************
; send monitoring Timerrec2
;****************************************
movlb 0x00
movfw timerrec2
btfsc STATUS,Z
bra t0_0count
sublw 0x01
btfss STATUS,Z
bra t0_rec1_2
bcf bitleiste1,snd_lock
; bcf bitleiste1,rec_ping
bcf bitleiste1,rec_echo
bcf bitleiste1,rec_quit
movlw 0x01
movwf BSR
bcf PIE1,TXIE
clrf BSR
t0_rec1_2: decf timerrec2,1
t0_0count:
;****************************************
; sec counter
;****************************************
t0_s_count: decf reg_12,1
btfss STATUS,Z
goto t0_exit
clrf BSR
movlw 0x64
movwf reg_12
;*******************************************
;*10 sec *
;*******************************************
t0_end: incf reg_17,1 ;
movfw reg_17 ;
sublw 0x0a ;
btfss STATUS,Z ;
goto t0_exit ;
clrf reg_17 ;
t0_exit: bcf INTCON,TMR0IF
goto int_t0ex
rseriell:
;*****************************************
;* serial Interrupt receive *
;*****************************************
movlw 0x03
movwf BSR
movlw high jmp_rcv
movwf PCLATH
bcf STATUS,C
; übernahme aus 8051
movf streceive,w
andlw 0x0f
addwf PCL,f
jmp_rcv: goto rcv_0 ; Ruhezustand, warte auf Flagbyte
goto rcv_1 ; Kontrolle der Sendebytes: Flag
goto rcv_2 ; Kontrolle der Sendebytes: Meldung
goto rcv_3
goto rcv_4 ; Empfang der Meldung
goto rcv_5 ; Empfang der Meldung
goto rcv_6 ;
goto rcv_7 ;
;******************************************;
;* 0:idle wait for Flag *;
;******************************************;
rcv_0:
banksel RCREG
movf RCREG,w
sublw 0x3B
btfss STATUS,Z
goto rec_end ; und Ende
movfw RCREG
movlb 0x00
movwf rec_buf1
movlb 0x00
movlw 0x01
movwf streceive
btfss bitleiste1,buf_1
goto rcv_0ex
addlw (rec_buf2-rec_buf1)
bsf bitleiste1,buf_ovl
movwf rec_poi
movlw 0x3B
movwf rec_buf2
goto rec_end
rcv_0ex: movwf rec_poi
goto rec_end
;******************************************;
;* 2 : wait for length *;
;******************************************;
rcv_1: movlb 0x00
clrf FSR0H
movlw rec_buf1
addwf rec_poi,w
incf rec_poi,f
movwf FSR0L
banksel RCREG
movf RCREG,W
movwf INDF0
movlb 0x00
movwf rec_cnt
movlw 0x02
movwf streceive
goto rec_end ; und Ende
;******************************************;
;*2: receive message *;
;******************************************;
rcv_2: movlb 0x00
clrf FSR0H
movlw rec_buf1
addwf rec_poi,w
incf rec_poi,f
movwf FSR0L
banksel RCREG
movf RCREG,W
movwf INDF0
movlb 0x00
decfsz rec_cnt,f
goto rec_end
movlw 0x03
movwf streceive
goto rec_end ; Ende
;******************************************;
;*3: receive CRC *;
;******************************************;
rcv_3: movlb 0x00
clrf FSR0H
movlw rec_buf1
addwf rec_poi,w
incf rec_poi,f
movwf FSR0L
banksel RCREG
movf RCREG,W
movwf INDF0
clrf streceive
btfsc bitleiste1,buf_ovl
goto rcv_3_ex
bsf bitleiste1,buf_1
goto rec_end
rcv_3_ex: bcf bitleiste1,buf_ovl
bsf bitleiste1,buf_2
goto rec_end
;
rcv_4:
rcv_5:
rcv_6: goto rec_end ; und Ende
;******************************************;
;*4: dummies *;
;******************************************;
rcv_7:
rec_end:
rec_ex:
goto int_rvex
;******************************************;
;* End receive *;
;******************************************;
tseriell:
;******************************************;
;* Start sending *;
;******************************************;
ser_send:
movlb 0x00
movlw 0x1F
andwf stseriell,W ; Zustand laden
bcf STATUS,C
brw
jmp_ser: goto ser_0 ; Ruhezustand
goto ser_1 ;
goto ser_2 ;
goto ser_3 ;
goto ser_4 ;
goto ser_5 ; senden Füllzeichn
goto ser_6 ; Senden der Parität
goto ser_7 ; Warten aufs Ende
goto ser_8 ;
goto ser_9 ;
goto ser_A ;
goto ser_B ;
goto ser_C ;
goto ser_D ; Start Ping 10
goto ser_E ;
goto ser_F ;
goto ser_10 ;
;******************************************;
;* idle *;
;******************************************;
ser_0:
banksel PIE1
bcf PIE1,TXIE
goto seriell_ex
;******************************************;
;* 1:x cmd send length *;
;******************************************;
ser_1: movlw 0x03 ;nächster Zustand
movwf stseriell
movlw 0x04 ;Meldungslänge
movwf check_sum
banksel TXREG
movwf TXREG
goto seriell_ex
;******************************************;
;* 2:y cmd send length *;
;******************************************;
ser_2: movlw 0x04 ;nächster Zustand
movwf stseriell
movlw 0x04 ;Meldungslänge
movwf check_sum
banksel TXREG
movwf TXREG
goto seriell_ex ;
;******************************************;
;* 3 x originator *;
;******************************************;
ser_3: movlw 0x05 ;nächster Zustand
movwf stseriell
movlw 0x20 ;Absender
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex
;******************************************;
;* 4: y originator *;
;******************************************;
ser_4: movlw 0x06 ;nächster Zustand
movwf stseriell
movlw 0x20 ;Absender
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex ;
;******************************************;
;* 5:target ALT Controller *;
;******************************************;
ser_5: movlw 0x07 ;nächster Zustand
movwf stseriell
movfw snd_target ;Ziel
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex ;
;******************************************;
;* 6: targe AZM Controller *;
;******************************************;
ser_6: movlw 0x08 ;nächster Zustand
movwf stseriell
movfw snd_target ;Ziel
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex
;******************************************;
;* 7: message ID X *;
;******************************************;
ser_7: movlw 0x09 ;nächster Zustand
movwf stseriell
movf snd_c_x,w ;message ID
addwf check_sum,f ;Chesksum berechnen
banksel TXREG ;
movwf TXREG ;
goto seriell_ex
;******************************************;
;* 12: message ID y *;
;******************************************;
ser_8: movlw 0x0A ;nächster Zustand
movwf stseriell
movf snd_c_y,w ;message ID
addwf check_sum,f ;Chesksum berechnen
banksel TXREG ;
movwf TXREG ;
goto seriell_ex ;
;******************************************;
;* 9 :Rate X *;
;******************************************;
ser_9: movlw 0x0B ;nächster Zustand
movwf stseriell
movf snd_r_x,w ;Speed
addwf check_sum,f ;Chesksum berechnen
banksel TXREG ;
movwf TXREG ;
goto seriell_ex
;******************************************;
;* 16: Rate y *;
;******************************************;
ser_A: movlw 0x0B ;nächster Zustand
movwf stseriell
movf snd_r_y,w ;Speed
addwf check_sum,f ;Chesksum berechnen
banksel TXREG ;
movwf TXREG ;
goto seriell_ex
;******************************************;
;* B: Checksum *;
;******************************************;
ser_B: movlw 0x0C ;nächster Zustand
movwf stseriell
movlw 0xFF ;komplement
xorwf check_sum,w ;Chesksum berechnen
incf WREG,W
banksel TXREG ;
movwf TXREG ;
goto seriell_ex
;******************************************;
;* C: EOT *;
;******************************************;
ser_C:
banksel PIE1
bcf PIE1,TXIE
clrf stseriell
goto seriell_ex
;******************************************;
;* D: Length Ping *;
;******************************************;
ser_D: movlw 0x0E ;nächster Zustand
movwf stseriell
movlw 0x03 ;Meldungslänge
movwf check_sum
banksel TXREG
movwf TXREG
goto seriell_ex
;
;******************************************;
;* E: source Ping *;
;******************************************;
ser_E: movlw 0x0F ;nächster Zustand
movwf stseriell
movlw 0x20 ;Absender
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex
;******************************************;
;* F: target Ping *;
;******************************************;
ser_F: movlw 0x10 ;nächster Zustand
movwf stseriell
movlw 0x10 ;Absender
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex
;******************************************;
;* 10: Ping *;
;******************************************;
ser_10: movlw 0x0B ;nächster Zustand
movwf stseriell
movlw 0x01 ;Absender
addwf check_sum,f ;Chesksum berechnen
banksel TXREG
movwf TXREG
goto seriell_ex
seriell_ex:
movlb 0x00
movlw timer_val
banksel PIR1
btfsc PIR1,RCIF
goto rseriell ; sonst zum Empfangsprogramm
seriell_end:
goto int_txex
init:
; setup clock
banksel OSCCON
; bcf OSCCON,IRCF0
; bsf OSCCON,IRCF1
; bsf OSCCON,IRCF2
; bsf OSCCON,IRCF3
movlw b'01011010'
;b'11110000'
movwf OSCCON
; configure Ports
movlw 0x00 ;
banksel ANSELA
clrf ANSELA
banksel ANSELC
movlw b'00000011'
movwf ANSELC
movlw 0x01 ; switch to Bank 1
movwf BSR
movlw b'11101011' ;port A
banksel TRISA
movwf TRISA
banksel WPUA
movlw b'00100000'
movwf WPUA ; Pull up RA5
movlw b'00101011' ; PortC -/-/RX/TX/RTS/CTS/AN5/AN4
banksel TRISC
movwf TRISC
banksel WPUC
clrf WPUC ; Pull up off
; USART Baudrate
banksel SPBRGL
movlw .12 ; 25 = 19200 bd
MOVWF SPBRGL
banksel SPBRGH
clrf SPBRGH
banksel TXSTA
BSF TXSTA,BRGH ; BRGH=1
banksel BAUDCON
bsf BAUDCON,BRG16 ; BRG16=1
;initialize USART
banksel APFCON0
bcf APFCON0,RXDTSEL ;bcf = RC5
bcf APFCON0,TXCKSEL ; bcf=RC4
banksel TXSTA
bcf TXSTA,SYNC
bsf TXSTA,TXEN
bcf TXSTA,TX9
banksel RCSTA
bCf RCSTA,SYNC
bsf RCSTA,CREN
bsf RCSTA,SPEN
; Timer0 initialize 10ms
banksel OPTION_REG
bcf OPTION_REG,TMR0CS ; Funktion = Timer
bsf OPTION_REG,0 ; Vorteiler 16
bsf OPTION_REG,1 ; 256000/16 = 16000
bcf OPTION_REG,2 ;
bcf OPTION_REG,PSA ;Vorteiler aktivieren
bcf OPTION_REG,7 ;WPUEN
banksel TMR0
movlw t_clock ;
movwf TMR0 ;
; configure ADC
banksel FVRCON ;
movlw B'00000000' ;AD-Referenz aus und 2,048V
;B'10000010'
movwf FVRCON
banksel ADCON1
movlw B'00000000' ;Fosc/64 Vref=Vdd linksbündig
movwf ADCON1
banksel ADCON0
movlw B'00100001' ; chs = Ad4;ADC Stop ADC enabled
movwf ADCON0
; enable/disable Interrupts
banksel INTCON
bsf INTCON,PEIE ;Peripherer Interrupt
bsf INTCON,TMR0IE ; Timerinterrupt freigeben
banksel PIE1
bsf PIE1,RCIE ; Serielle Schnittstelle Empfang und
bcf PIE1,TXIE ; SendeInterrupt erstmal sperren
banksel INTCON
bsf INTCON,GIE
movlb 0x00
clrf stseriell
clrf streceive
clrf bitleiste1
clrf bitleiste2
movlw 0x40
movwf c_reg
clrf FSR0H
movlw 0x01
movwf FSR1H
movlw 0x20
movwf FSR0L
movwf FSR1L
movlw 0x00
init_loop: movwi INDF0++
movwi INDF1++
decfsz c_reg,f
bra init_loop
movlw 0x24
movwf snd_c_y
movwf snd_c_x
banksel ADCON0
bsf ADCON0,1 ;ADC starten
banksel LATC
bsf LATC,0x02
movlb 0x00
bsf bitleiste1,snd_lock
movlw .50
movwf timerrec1
clrf timerrec2
loop:
;*****************************************************
;Main Routine Do-forever-loop, *
;* *
;****************************************************
main_l:
banksel LATA
bsf LATA,RA0
movlw 0x00 ; switch to bank 0
movwf BSR
;*****************************************
;* Start ADC *
;*****************************************
proc_adc:
banksel ADCON0 ;
movf ADCON0,w
btfsc ADCON0,ADGO ; ADC ready?
bra no_adc ; no? then end
btfss bitleiste1,x_adc
bra adc_y
adc_x: movlb 0x00
movlw 0x01 ; Indexregister 1
movwf FSR0H ;initialize
incf res_x_cnt,w
andlw 0x03
movwf res_x_cnt
movwf FSR0L
movlw result_x ; pointer 0 to result
addwf FSR0L,f
banksel ADRESH
movf ADRESH,w
movwf INDF0
banksel ADCON0
movlw B'00010101' ;Registercontent for Ad1
movwf ADCON0
bcf bitleiste1,x_adc
bsf ADCON0,ADGO
goto no_adc ; forward to read Register
adc_y: movlb 0x00
movlw 0x01 ; Indexregister 1
movwf FSR0H ;initialize
incf res_y_cnt,w
andlw 0x03
movwf res_y_cnt
movwf FSR0L
movlw result_y ; pointer to result
addwf FSR0L,f
banksel ADRESH
movf ADRESH,w
movwf INDF0
banksel ADCON0
movlw B'00010001' ;Registercontent Ad1
movwf ADCON0
bsf bitleiste1,x_adc
bsf ADCON0,ADGO
goto no_adc ; forward to read Register
no_adc: movlb 0x00
movlw 0x01 ; address result_X array
movwf FSR0H
movlw result_x
movwf FSR0L
movlw 0x4 ; counter set to 4
movwf c_reg
clrf a_reg ;
clrf b_reg ;delete temporary memory
x_loop: moviw INDF0++ ; load value
addwf a_reg,f ; add
btfsc STATUS,C ;if carry
incf b_reg,f ;then increment
decfsz c_reg,f ;count loop
bra x_loop ;
bcf STATUS,C
;lsrf b_reg,f
;rrf a_reg,f
lsrf b_reg,f
rrf a_reg,f
lsrf b_reg,f
rrf a_reg,w ;div /4
call x_rate_val
movwf a_reg
andlw 0x0F ; only rate
btfsc STATUS,Z
bra x_l_zero
btfsc bitleiste2,key_act; test if key was pressed
addlw 0xFD ;yes, use slower rate
x_l_zero: movwf rate_x ;store rate
btfsc STATUS,Z
bra x_no_cha
swapf a_reg,w
andlw 0x0F
addlw 0x24
movwf cmd_x ; store command
bra y_start
x_no_cha: movlw 0x24
iorwf cmd_x,f
y_start: movlw 0x4 ;now Y ((index should be ok )
movwf c_reg ;
clrf a_reg ; like before
clrf b_reg
y_loop: moviw INDF0++
addwf a_reg,f
btfsc STATUS,C
incf b_reg,f
decfsz c_reg,f
bra y_loop
bcf STATUS,C
lsrf b_reg,f
rrf a_reg,f
lsrf b_reg,f
rrf a_reg,w ;div 4
movwf b_reg
call y_rate_val
movwf a_reg
andlw 0x0F
sublw 0x09
btfsc STATUS,Z
nop
movfw a_reg
andlw 0x0F
btfsc STATUS,Z
bra y_l_zero
btfsc bitleiste2,key_act; test if key was pressed
addlw 0xFD ;yes, use slower rate
y_l_zero: movwf rate_y ;store rate
btfsc STATUS,Z
bra y_no_cha
swapf a_reg,w
andlw 0x0F
addlw 0x24
movwf cmd_y ; store command
bra key_start
y_no_cha: movlw 0x24
iorwf cmd_y,f
key_start:
banksel PORTA
movf PORTA,W
andlw b'00100000' ;RA5
xorwf key_ll ;test for edge
btfsc STATUS,Z
goto key_equal ;
movlb 0x00
movwf key_ll
movlw ll_cnt
movwf key_cnt
goto key_end
key_equal: movf key_cnt,f
btfsc STATUS,Z
goto key_end
decfsz key_cnt,f
goto key_end
movf key_ll,f
btfss STATUS,Z
goto key_end
btfsc bitleiste2,key_act
goto key_deact
bsf bitleiste2,key_act
goto key_end
key_deact: bcf bitleiste2,key_act
key_end:
comm_start:
banksel TRISC
bcf TRISC,4
movlb 0x00
btfsc bitleiste1,snd_lock ; sending process busy?
goto end_send
btfsc bitleiste1,rec_ping ;already ack fo ping received?
goto tst_snd_x
bsf bitleiste1,snd_lock
movlw 0x0D ;start Ping
movwf stseriell
movlw 0x3b
banksel TXREG
movwf TXREG
banksel PIE1
bsf PIE1,TXIE
movlb 0x00
movlw timeout
movwf timerrec1
bra end_send
tst_snd_x: btfsc bitleiste1,rec_quit
goto end_send
btfsc bitleiste1,rec_echo
goto end_send
movf cmd_x,w
subwf snd_c_x,w
btfss STATUS,Z
bra snd_cmd_x
movf rate_x,w
subwf snd_r_x,w
btfsc STATUS,Z
bra tst_snd_y
snd_cmd_x: bsf bitleiste1,snd_lock
movlw 0x01
movwf stseriell
movf rate_x,w
movwf snd_r_x
movf cmd_x,w
movwf snd_c_x
movlw x_dir
movwf snd_target
movlw 0x3B
banksel TXREG
movwf TXREG
banksel PIE1
bsf PIE1,TXIE
movlb 0x00
movlw timer_val
movwf timerrec2
bsf bitleiste1,rec_echo
bsf bitleiste1,rec_quit
bra end_send
tst_snd_y: movlb 0x00
movf cmd_y,w
subwf snd_c_y,w
btfss STATUS,Z
bra snd_cmd_y
movf rate_y,w
subwf snd_r_y,w
btfsc STATUS,Z
bra end_send
snd_cmd_y: bsf bitleiste1,snd_lock
movlw 0x02
movwf stseriell
movf rate_y,w
movwf snd_r_y
movf cmd_y,w
movwf snd_c_y
movlw y_dir
movwf snd_target
movlw 0x3b
banksel TXREG
movwf TXREG
banksel PIE1
bsf PIE1,TXIE
movlb 0x00
movlw timer_val
movwf timerrec2
bsf bitleiste1,rec_echo
bsf bitleiste1,rec_quit
bra end_send
end_send:
rec_start: clrf FSR0H
btfss bitleiste1,buf_1
goto rec_sta2
movlw rec_buf1
movwf FSR0L
movlw 0x7F
movwf rec_mask
bra rec_pars
rec_sta2: btfss bitleiste1,buf_2
goto rec_term
movlw rec_buf2
movwf FSR0L
movlw 0xBF
movwf rec_mask
rec_pars: btfsc bitleiste1,rec_ping ; already Ping ack received?
bra r_p_echo
movlw HIGH ping_response
movwf FSR1H
movlw LOW ping_response
movwf FSR1L
movlw 0x05
movwf c_reg
call rec_comp
btfss STATUS,Z
goto rec_exit
clrf timerrec1
bsf bitleiste1,rec_ping
bcf bitleiste1,snd_lock
goto rec_exit
r_p_echo: moviw ++INDF0
sublw 0x04
btfss STATUS,Z
bra r_p_quit
moviw ++INDF0
sublw 0x20
btfss STATUS,Z
bra r_p_quit
moviw ++INDF0
subwf snd_target
btfss STATUS,Z
bra r_p_quit
movf snd_c_x,w
btfsc snd_target,0 ; Target y?
movf snd_c_y,w ;load with y value
movwf a_reg
moviw ++INDF0
subwf a_reg
btfss STATUS,Z
bra r_p_quit
bcf bitleiste1,rec_echo
bcf bitleiste1,snd_lock
bra rec_exit
r_p_quit: moviw ++INDF0
sublw 0x03
btfss STATUS,Z
bra rec_exit
moviw ++INDF0
subwf snd_target
btfss STATUS,Z
bra rec_exit
moviw ++INDF0
sublw 0x20
btfss STATUS,Z
bra rec_exit
movf snd_c_x,w
btfsc snd_target,0 ; Target y?
movf snd_c_y,w ;load with y value
movwf a_reg
moviw ++INDF0
subwf a_reg
btfss STATUS,Z
bra rec_exit
bcf bitleiste1,rec_quit
clrf timerrec2
rec_exit: movf rec_mask,w
andwf bitleiste1,F
movf bitleiste1,w
andlw 0xC0
btfss STATUS,Z
bra rec_start
rec_term:
banksel ADRESH
movf ADRESH,w
goto loop
rec_comp: moviw INDF0++
movwf a_reg
moviw INDF1++
subwf a_reg,w
btfss STATUS,Z
retlw 0x01
decfsz c_reg,f
bra rec_comp
retlw 0x00
x_rate_val: brw
retlw down_fast ;0x00
retlw down_fast ;0x01
retlw down_fast ;0x02
retlw down_fast ;0x03
retlw down_fast ;0x04
retlw down_fast ;0x05
retlw down_fast ;0x06
retlw down_fast ;0x07
retlw down_fast ;0x08
retlw down_fast ;0x09