-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathSimpleRobotDemo.asm
1194 lines (1110 loc) · 32.3 KB
/
SimpleRobotDemo.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
; This program includes...
; - Robot initialization (checking the battery, stopping motors, etc.).
; - The movement API.
; - Several useful subroutines (ATAN2, Neg, Abs, mult, div).
; - Some useful constants (masks, numbers, robot stuff, etc.)
; This code uses the timer interrupt for the movement control code.
; The ISR jump table is located in mem 0-4. See manual for details.
ORG 0
JUMP Init ; Reset vector
RETI ; Sonar interrupt (unused)
JUMP CTimer_ISR ; Timer interrupt
RETI ; UART interrupt (unused)
RETI ; Motor stall interrupt (unused)
;***************************************************************
;* Initialization
;***************************************************************
Init:
; Always a good idea to make sure the robot
; stops in the event of a reset.
LOAD Zero
OUT LVELCMD ; Stop motors
OUT RVELCMD
STORE DVel ; Reset API variables
STORE DTheta
OUT SONAREN ; Disable sonar (optional)
OUT BEEP ; Stop any beeping (optional)
CALL SetupI2C ; Configure the I2C to read the battery voltage
CALL BattCheck ; Get battery voltage (and end if too low).
OUT LCD ; Display battery voltage (hex, tenths of volts)
WaitForSafety:
; This loop will wait for the user to toggle SW17. Note that
; SCOMP does not have direct access to SW17; it only has access
; to the SAFETY signal contained in XIO.
IN XIO ; XIO contains SAFETY signal
AND Mask4 ; SAFETY signal is bit 4
JPOS WaitForUser ; If ready, jump to wait for PB3
IN TIMER ; We'll use the timer value to
AND Mask1 ; blink LED17 as a reminder to toggle SW17
SHIFT 8 ; Shift over to LED17
OUT XLEDS ; LED17 blinks at 2.5Hz (10Hz/4)
JUMP WaitForSafety
WaitForUser:
; This loop will wait for the user to press PB3, to ensure that
; they have a chance to prepare for any movement in the main code.
IN TIMER ; We'll blink the LEDs above PB3
AND Mask1
SHIFT 5 ; Both LEDG6 and LEDG7
STORE Temp ; (overkill, but looks nice)
SHIFT 1
OR Temp
OUT XLEDS
IN XIO ; XIO contains KEYs
AND Mask2 ; KEY3 mask (KEY0 is reset and can't be read)
JPOS WaitForUser ; not ready (KEYs are active-low, hence JPOS)
LOAD Zero
OUT XLEDS ; clear LEDs once ready to continue
;Main:
OUT RESETPOS ; reset the odometry to 0,0,0
; configure timer interrupt for the movement control code
LOADI 10 ; period = (10 ms * 10) = 0.1s, or 10Hz.
OUT CTIMER ; turn on timer peripheral
SEI &B0010 ; enable interrupts from source 2 (timer)
;JUMP CircleCenter
;***************************************************************
; START FIND CODE
;***************************************************************
Find:
LOADI 1
OUT SSEG1
OUT RESETPOS
LOAD ZERO
ADDI 610
ADDI 609
OUT SONALARM
STORE FOUNDREFLECTOR
LOAD MASK5
OR MASK2
OR MASK3
OUT SONAREN
;LOADI 100
LOAD FMid
STORE DVel
CheckForWall:
LOAD TRAVELED
OUT SSEG2
ADDI -610
ADDI -610
ADDI -610
ADDI -610
ADDI -610
ADDI -610
;ADDI -610
ADDI -150
JPOS TurnAtEnd
FindLoop:
IN SONALARM
AND MASK5
SUB MASK5
JZERO GetClose
CALL CHECKFRONT
JUMP FindLoop
GetClose:
IN DIST4
STORE FOUNDREFLECTOR
GoForward:
CALL CHECKFRONT
IN DIST5
SUB FOUNDREFLECTOR
ADDI 30
JNEG Stop
JUMP GoForward
Stop:
LOADI 0
STORE DVel
IN XPOS
ADD TRAVELED
STORE TRAVELED
;***************************************************************
; END FIND CODE
;***************************************************************
;***************************************************************
; START TURN AND FACE CODE
;***************************************************************
LOADI 2
OUT SSEG1
OUT RESETPOS
IN Theta
STORE StartTheta
CALL TurnRight90
;***************************************************************
; END TURN AND FACE CODE
;***************************************************************
;***************************************************************
LOADI 3
OUT SSEG1
CLI &B0010 ; disable movement API
OUT RESETPOS
; GO UNTIL WITHIN 1 FT CODE
Move1:
LOAD MASK2
OR MASK3
OUT SONAREN
LoopMove1:
; if object within 1.5 ft in front, stop
IN DIST2
ADDI -350
JNEG FinMove1
IN DIST3
ADDI -350
JNEG FINMOVE1
; else, move forward again
LOADI 200
OUT LVELCMD
OUT RVELCMD
JUMP LoopMove1
FinMove1:
;JUMP FinMove1
IN XPOS
STORE linedist ; store odometry distance traveled from line
;ADD LINEDIST
;ADD LINEDIST
;STORE d16sN
SHIFT -1
STORE LINEDIST
LOADI 0
OUT SONAREN ; turn off sensors
;***************************************************************
;START CIRCLE CODE
LOADI 4
OUT SSEG1
OUT RESETPOS
CALL TurnLeft70
;OUT RESETPOS
;circle code from notepad
CLI &B0010
OUT RESETPOS
Circle:
IN THETA
ADDI 35
;ADDI 30
STORE STARTTHETA
CircleLoop:
LOADI 511
OUT LVELCMD
ADDI -210
OUT RVELCMD
IN THETA
SUB STARTTHETA
OUT SSEG2
JZERO CircleEnd
JUMP CircleLoop
CircleEnd:
;JUMP CircleEnd
; END CIRCLE CODE
;***************************************************************
;***************************************************************
;* START RETURN TO LINE CODE
LOADI 5
OUT SSEG1
LOAD ZERO
STORE DTHETA
CALL TurnLeft90
CLI &B0010
OUT RESETPOS
CheckDist:
IN XPOS
OUT SSEG2
SUB linedist
JPOS TurnBack
LOADI 200
OUT LVELCMD
OUT RVELCMD
JUMP CheckDist
TurnBack:
OUT RESETPOS
SEI &B0010
CALL TurnRight90
OUT RESETPOS
LOAD FMid
STORE DVel
LOAD Zero
STORE DTheta
DriveForward:
IN XPOS
ADDI -100
JPOS Find
;JUMP INFLOOP
JUMP DriveForward
;* END RETURN TO LINE CODE
;***************************************************************
;***************************************************************
;* START CENTER CIRCLE CODE
CircleCenter:
LOADI 6
OUT SSEG1
IN XPOS
ADD TRAVELED
STORE TRAVELED
OUT RESETPOS
CALL TurnLeft70
CLI &B0010
OUT RESETPOS
IN THETA
ADDI 15
STORE STARTTHETA
CircleCenterLoop:
LOADI 511
OUT LVELCMD
ADDI -210
OUT RVELCMD
IN THETA
SUB STARTTHETA
OUT SSEG2
JZERO CircleHalfStart
JUMP CircleCenterLoop
CircleHalfStart:
OUT RESETPOS
IN THETA
ADDI 200
STORE STARTTHETA
CircleHalfLoop:
LOADI 511
OUT LVELCMD
ADDI -210
OUT RVELCMD
IN THETA
SUB STARTTHETA
OUT SSEG2
JZERO CircleHalfEnd
JUMP CircleHalfLoop
CircleHalfEnd:
IN YPOS
CALL ABS
ADD TRAVELED
STORE TRAVELED
OUT RESETPOS
CALL TurnLeft90
;CLI &B0010
OUT RESETPOS
SEI &B0010
JUMP Find
;* END CENTER CIRCLE CODE
;***************************************************************
;***************************************************************
; CHECK FRONT CODE
CheckFront:
IN DIST2
ADDI -305
ADDI -150
JNEG EndCheckFront
IN DIST3
ADDI -305
ADDI -150
JNEG EndCheckFront
RETURN
EndCheckFront:
JUMP CircleCenter
; END CHECK FRONT CODE
;***************************************************************
;***************************************************************
; TurnRight90Degrees
TurnRight90:
OUT RESETPOS
IN Theta
STORE StartTheta
LOADI 270
STORE DTHETA
LOADI 0
STORE DVEL
CheckAngleRight90:
IN Theta
SUB StartTheta
ADDI -270
CALL ABS
ADDI -5
JPOS CheckAngleRight90
RETURN
; End TurnRight90Degrees
;***************************************************************
;***************************************************************
; TurnLeft90Degrees
TurnLeft90:
SEI &B0010
OUT RESETPOS
IN Theta
STORE StartTheta
LOADI 80
STORE DTHETA
LOADI 0
STORE DVEL
CheckAngleLeft90:
IN Theta
SUB StartTheta
ADDI -80
CALL Abs
ADDI -5
JPOS CheckAngleLeft90
RETURN
; End TurnLeft90Degrees
;***************************************************************
;***************************************************************
; TurnLeft70Degrees
TurnLeft70:
SEI &B0010
OUT RESETPOS
IN Theta
STORE StartTheta
LOADI 70
STORE DTHETA
LOADI 0
STORE DVEL
CheckAngleLeft70:
IN Theta
SUB StartTheta
ADDI -70
CALL Abs
ADDI -5
JPOS CheckAngleLeft70
RETURN
; End TurnLeft70Degrees
;***************************************************************
;***************************************************************
;* Main code
;***************************************************************
Main:
OUT RESETPOS ; reset the odometry to 0,0,0
; configure timer interrupt for the movement control code
LOADI 10 ; period = (10 ms * 10) = 0.1s, or 10Hz.
OUT CTIMER ; turn on timer peripheral
SEI &B0010 ; enable interrupts from source 2 (timer)
; at this point, timer interrupts will be firing at 10Hz, and
; code in that ISR will attempt to control the robot.
; If you want to take manual control of the robot,
; execute CLI &B0010 to disable the timer interrupt.
CLI &B0010
;LOADI &B00100000
;OUT SONARINT
LOADI 32
OUT SONAREN
LOADI 600
ADDI 619
OUT SONALARM
Check:
;JUMP Turn
;LOAD Zero
;ADDI 45
;STORE DTheta s ; use API to get robot to face 90 degrees
IN DIST5
OUT SSEG1
IN SONALARM
ADDI -32
JZERO InfLoop
JUMP Turn
Turn:
IN DIST5
OUT SSEG1
LOADI 100
OUT LVELCMD
ADDI -200
OUT RVELCMD
JUMP Check
TurnLoop:
IN Theta
ADDI -90
CALL Abs ; get abs(currentAngle - 90)
ADDI -3
JPOS TurnLoop ; if angle error > 3, keep checking
; at this point, robot should be within 3 degrees of 90
LOAD One
STORE DVel ; use API to move forward
JUMP Check
InfLoop:
IN DIST5
OUT SSEG1
JUMP InfLoop
; note that the movement API will still be running during this
; infinite loop, because it uses the timer interrupt, so the
; robot will continue to attempt to match DTheta and DVel
Die:
; Sometimes it's useful to permanently stop execution.
; This will also catch the execution if it accidentally
; falls through from above.
CLI &B1111 ; disable all interrupts
LOAD Zero ; Stop everything.
OUT LVELCMD
OUT RVELCMD
OUT SONAREN
LOAD DEAD ; An indication that we are dead
OUT SSEG2 ; "dEAd" on the sseg
Forever:
JUMP Forever ; Do this forever.
DEAD: DW &HDEAD ; Example of a "local" variable
; Timer ISR. Currently just calls the movement control code.
; You could, however, do additional tasks here if desired.
CTimer_ISR:
;CALL CheckForWall
CALL ControlMovement
RETI ; return from ISR
;***********************************************************
; Reached wall interrupt code
;***********************************************************
CheckForWallI:
IN TRAVELED
OUT SSEG2
ADDI -610
ADDI -610
ADDI -610
ADDI -610
ADDI -610
ADDI -610
;ADDI -610
;ADDI -610
JPOS ReachedEnd
RETURN
ReachedEnd:
ADDI -610
ADDI -610
ADDI -610
ADDI -610
ADDI -610
ADDI -610
;ADDI -610
;ADDI -610
JPOS ReachedStart
CALL TurnAtEnd
RETURN
ReachedStart:
CALL TurnAtEnd
OUT RESETPOS
RETURN
;************************************************************
; End reached wall interrupt code
;************************************************************
;************************************************************
; Turn at end code
;************************************************************
TurnAtEnd:
CALL TurnLeft90
CALL TurnLeft90
LOADI 0
STORE TRAVELED
OUT RESETPOS
JUMP Find
;************************************************************
; End turn at end code
;************************************************************
; Control code. If called repeatedly, this code will attempt
; to control the robot to face the angle specified in DTheta
; and match the speed specified in DVel
DTheta: DW 0
DVel: DW 0
ControlMovement:
LOADI 50 ; used for the CapValue subroutine
STORE MaxVal
CALL GetThetaErr ; get the heading error
; A simple way to get a decent velocity value
; for turning is to multiply the angular error by 4
; and add ~50.
SHIFT 2
STORE CMAErr ; hold temporarily
SHIFT 2 ; multiply by another 4
CALL CapValue ; get a +/- max of 50
ADD CMAErr
STORE CMAErr ; now contains a desired differential
; For this basic control method, simply take the
; desired forward velocity and add the differential
; velocity for each wheel when turning is needed.
LOADI 510
STORE MaxVal
LOAD DVel
CALL CapValue ; ensure velocity is valid
STORE DVel ; overwrite any invalid input
ADD CMAErr
CALL CapValue ; ensure velocity is valid
STORE CMAR
LOAD CMAErr
CALL Neg ; left wheel gets negative differential
ADD DVel
CALL CapValue
STORE CMAL
; ensure enough differential is applied
LOAD CMAErr
SHIFT 1 ; double the differential
STORE CMAErr
LOAD CMAR
SUB CMAL ; calculate the actual differential
SUB CMAErr ; should be 0 if nothing got capped
JZERO CMADone
; re-apply any missing differential
STORE CMAErr ; the missing part
ADD CMAL
CALL CapValue
STORE CMAL
LOAD CMAR
SUB CMAErr
CALL CapValue
STORE CMAR
CMADone:
LOAD CMAL
OUT LVELCMD
LOAD CMAR
OUT RVELCMD
RETURN
CMAErr: DW 0 ; holds angle error velocity
CMAL: DW 0 ; holds temp left velocity
CMAR: DW 0 ; holds temp right velocity
; Returns the current angular error wrapped to +/-180
GetThetaErr:
; convenient way to get angle error in +/-180 range is
; ((error + 180) % 360 ) - 180
IN THETA
SUB DTheta ; actual - desired angle
CALL Neg ; desired - actual angle
ADDI 180
CALL Mod360
ADDI -180
RETURN
; caps a value to +/-MaxVal
CapValue:
SUB MaxVal
JPOS CapVelHigh
ADD MaxVal
ADD MaxVal
JNEG CapVelLow
SUB MaxVal
RETURN
CapVelHigh:
LOAD MaxVal
RETURN
CapVelLow:
LOAD MaxVal
CALL Neg
RETURN
MaxVal: DW 510
;*******************************************************************************
; Mod360: modulo 360
; Returns AC%360 in AC
; Written by Kevin Johnson. No licence or copyright applied.
;*******************************************************************************
Mod360:
; easy modulo: subtract 360 until negative then add 360 until not negative
JNEG M360N
ADDI -360
JUMP Mod360
M360N:
ADDI 360
JNEG M360N
RETURN
;*******************************************************************************
; Abs: 2's complement absolute value
; Returns abs(AC) in AC
; Neg: 2's complement negation
; Returns -AC in AC
; Written by Kevin Johnson. No licence or copyright applied.
;*******************************************************************************
Abs:
JPOS Abs_r
Neg:
XOR NegOne ; Flip all bits
ADDI 1 ; Add one (i.e. negate number)
Abs_r:
RETURN
;******************************************************************************;
; Atan2: 4-quadrant arctangent calculation ;
; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ;
; Original code by Team AKKA, Spring 2015. ;
; Based on methods by Richard Lyons ;
; Code updated by Kevin Johnson to use software mult and div ;
; No license or copyright applied. ;
; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ;
; To use: store dX and dY in global variables AtanX and AtanY. ;
; Call Atan2 ;
; Result (angle [0,359]) is returned in AC ;
; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ;
; Requires additional subroutines: ;
; - Mult16s: 16x16->32bit signed multiplication ;
; - Div16s: 16/16->16R16 signed division ;
; - Abs: Absolute value ;
; Requires additional constants: ;
; - One: DW 1 ;
; - NegOne: DW 0 ;
; - LowByte: DW &HFF ;
;******************************************************************************;
Atan2:
LOAD AtanY
CALL Abs ; abs(y)
STORE AtanT
LOAD AtanX ; abs(x)
CALL Abs
SUB AtanT ; abs(x) - abs(y)
JNEG A2_sw ; if abs(y) > abs(x), switch arguments.
LOAD AtanX ; Octants 1, 4, 5, 8
JNEG A2_R3
CALL A2_calc ; Octants 1, 8
JNEG A2_R1n
RETURN ; Return raw value if in octant 1
A2_R1n: ; region 1 negative
ADDI 360 ; Add 360 if we are in octant 8
RETURN
A2_R3: ; region 3
CALL A2_calc ; Octants 4, 5
ADDI 180 ; theta' = theta + 180
RETURN
A2_sw: ; switch arguments; octants 2, 3, 6, 7
LOAD AtanY ; Swap input arguments
STORE AtanT
LOAD AtanX
STORE AtanY
LOAD AtanT
STORE AtanX
JPOS A2_R2 ; If Y positive, octants 2,3
CALL A2_calc ; else octants 6, 7
CALL Neg ; Negatge the number
ADDI 270 ; theta' = 270 - theta
RETURN
A2_R2: ; region 2
CALL A2_calc ; Octants 2, 3
CALL Neg ; negate the angle
ADDI 90 ; theta' = 90 - theta
RETURN
A2_calc:
; calculates R/(1 + 0.28125*R^2)
LOAD AtanY
STORE d16sN ; Y in numerator
LOAD AtanX
STORE d16sD ; X in denominator
CALL A2_div ; divide
LOAD dres16sQ ; get the quotient (remainder ignored)
STORE AtanRatio
STORE m16sA
STORE m16sB
CALL A2_mult ; X^2
STORE m16sA
LOAD A2c
STORE m16sB
CALL A2_mult
ADDI 256 ; 256/256+0.28125X^2
STORE d16sD
LOAD AtanRatio
STORE d16sN ; Ratio in numerator
CALL A2_div ; divide
LOAD dres16sQ ; get the quotient (remainder ignored)
STORE m16sA ; <= result in radians
LOAD A2cd ; degree conversion factor
STORE m16sB
CALL A2_mult ; convert to degrees
STORE AtanT
SHIFT -7 ; check 7th bit
AND One
JZERO A2_rdwn ; round down
LOAD AtanT
SHIFT -8
ADDI 1 ; round up
RETURN
A2_rdwn:
LOAD AtanT
SHIFT -8 ; round down
RETURN
A2_mult: ; multiply, and return bits 23..8 of result
CALL Mult16s
LOAD mres16sH
SHIFT 8 ; move high word of result up 8 bits
STORE mres16sH
LOAD mres16sL
SHIFT -8 ; move low word of result down 8 bits
AND LowByte
OR mres16sH ; combine high and low words of result
RETURN
A2_div: ; 16-bit division scaled by 256, minimizing error
LOADI 9 ; loop 8 times (256 = 2^8)
STORE AtanT
A2_DL:
LOAD AtanT
ADDI -1
JPOS A2_DN ; not done; continue shifting
CALL Div16s ; do the standard division
RETURN
A2_DN:
STORE AtanT
LOAD d16sN ; start by trying to scale the numerator
SHIFT 1
XOR d16sN ; if the sign changed,
JNEG A2_DD ; switch to scaling the denominator
XOR d16sN ; get back shifted version
STORE d16sN
JUMP A2_DL
A2_DD:
LOAD d16sD
SHIFT -1 ; have to scale denominator
STORE d16sD
JUMP A2_DL
AtanX: DW 0
AtanY: DW 0
AtanRatio: DW 0 ; =y/x
AtanT: DW 0 ; temporary value
A2c: DW 72 ; 72/256=0.28125, with 8 fractional bits
A2cd: DW 14668 ; = 180/pi with 8 fractional bits
;*******************************************************************************
; Mult16s: 16x16 -> 32-bit signed multiplication
; Based on Booth's algorithm.
; Written by Kevin Johnson. No licence or copyright applied.
; Warning: does not work with factor B = -32768 (most-negative number).
; To use:
; - Store factors in m16sA and m16sB.
; - Call Mult16s
; - Result is stored in mres16sH and mres16sL (high and low words).
;*******************************************************************************
Mult16s:
LOADI 0
STORE m16sc ; clear carry
STORE mres16sH ; clear result
LOADI 16 ; load 16 to counter
Mult16s_loop:
STORE mcnt16s
LOAD m16sc ; check the carry (from previous iteration)
JZERO Mult16s_noc ; if no carry, move on
LOAD mres16sH ; if a carry,
ADD m16sA ; add multiplicand to result H
STORE mres16sH
Mult16s_noc: ; no carry
LOAD m16sB
AND One ; check bit 0 of multiplier
STORE m16sc ; save as next carry
JZERO Mult16s_sh ; if no carry, move on to shift
LOAD mres16sH ; if bit 0 set,
SUB m16sA ; subtract multiplicand from result H
STORE mres16sH
Mult16s_sh:
LOAD m16sB
SHIFT -1 ; shift result L >>1
AND c7FFF ; clear msb
STORE m16sB
LOAD mres16sH ; load result H
SHIFT 15 ; move lsb to msb
OR m16sB
STORE m16sB ; result L now includes carry out from H
LOAD mres16sH
SHIFT -1
STORE mres16sH ; shift result H >>1
LOAD mcnt16s
ADDI -1 ; check counter
JPOS Mult16s_loop ; need to iterate 16 times
LOAD m16sB
STORE mres16sL ; multiplier and result L shared a word
RETURN ; Done
c7FFF: DW &H7FFF
m16sA: DW 0 ; multiplicand
m16sB: DW 0 ; multipler
m16sc: DW 0 ; carry
mcnt16s: DW 0 ; counter
mres16sL: DW 0 ; result low
mres16sH: DW 0 ; result high
;*******************************************************************************
; Div16s: 16/16 -> 16 R16 signed division
; Written by Kevin Johnson. No licence or copyright applied.
; Warning: results undefined if denominator = 0.
; To use:
; - Store numerator in d16sN and denominator in d16sD.
; - Call Div16s
; - Result is stored in dres16sQ and dres16sR (quotient and remainder).
; Requires Abs subroutine
;*******************************************************************************
Div16s:
LOADI 0
STORE dres16sR ; clear remainder result
STORE d16sC1 ; clear carry
LOAD d16sN
XOR d16sD
STORE d16sS ; sign determination = N XOR D
LOADI 17
STORE d16sT ; preload counter with 17 (16+1)
LOAD d16sD
CALL Abs ; take absolute value of denominator
STORE d16sD
LOAD d16sN
CALL Abs ; take absolute value of numerator
STORE d16sN
Div16s_loop:
LOAD d16sN
SHIFT -15 ; get msb
AND One ; only msb (because shift is arithmetic)
STORE d16sC2 ; store as carry
LOAD d16sN
SHIFT 1 ; shift <<1
OR d16sC1 ; with carry
STORE d16sN
LOAD d16sT
ADDI -1 ; decrement counter
JZERO Div16s_sign ; if finished looping, finalize result
STORE d16sT
LOAD dres16sR
SHIFT 1 ; shift remainder
OR d16sC2 ; with carry from other shift
SUB d16sD ; subtract denominator from remainder
JNEG Div16s_add ; if negative, need to add it back
STORE dres16sR
LOADI 1
STORE d16sC1 ; set carry
JUMP Div16s_loop
Div16s_add:
ADD d16sD ; add denominator back in
STORE dres16sR
LOADI 0
STORE d16sC1 ; clear carry
JUMP Div16s_loop
Div16s_sign:
LOAD d16sN
STORE dres16sQ ; numerator was used to hold quotient result
LOAD d16sS ; check the sign indicator
JNEG Div16s_neg
RETURN
Div16s_neg:
LOAD dres16sQ ; need to negate the result
CALL Neg
STORE dres16sQ
RETURN
d16sN: DW 0 ; numerator
d16sD: DW 0 ; denominator
d16sS: DW 0 ; sign value
d16sT: DW 0 ; temp counter
d16sC1: DW 0 ; carry value
d16sC2: DW 0 ; carry value
dres16sQ: DW 0 ; quotient result
dres16sR: DW 0 ; remainder result
;*******************************************************************************
; L2Estimate: Pythagorean distance estimation
; Written by Kevin Johnson. No license or copyright applied.
; Warning: this is *not* an exact function. I think it's most wrong
; on the axes, and maybe at 45 degrees.
; To use:
; - Store X and Y offset in L2X and L2Y.
; - Call L2Estimate
; - Result is returned in AC.
; Result will be in same units as inputs.
; Requires Abs and Mult16s subroutines.
;*******************************************************************************
L2Estimate:
; take abs() of each value, and find the largest one
LOAD L2X
CALL Abs
STORE L2T1
LOAD L2Y
CALL Abs
SUB L2T1
JNEG GDSwap ; swap if needed to get largest value in X
ADD L2T1
CalcDist:
; Calculation is max(X,Y)*0.961+min(X,Y)*0.406
STORE m16sa
LOADI 246 ; max * 246
STORE m16sB
CALL Mult16s
LOAD mres16sH
SHIFT 8
STORE L2T2
LOAD mres16sL
SHIFT -8 ; / 256
AND LowByte
OR L2T2
STORE L2T3
LOAD L2T1
STORE m16sa
LOADI 104 ; min * 104
STORE m16sB
CALL Mult16s
LOAD mres16sH
SHIFT 8
STORE L2T2
LOAD mres16sL
SHIFT -8 ; / 256
AND LowByte
OR L2T2
ADD L2T3 ; sum
RETURN
GDSwap: ; swaps the incoming X and Y
ADD L2T1
STORE L2T2
LOAD L2T1
STORE L2T3
LOAD L2T2
STORE L2T1
LOAD L2T3
JUMP CalcDist
L2X: DW 0
L2Y: DW 0
L2T1: DW 0
L2T2: DW 0
L2T3: DW 0
; Subroutine to wait (block) for 1 second
Wait1: