-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathStartingPoint_Spr17.asm
844 lines (789 loc) · 26.6 KB
/
StartingPoint_Spr17.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
; StartingPoint_Spr17.asm
; This program includes a basic movement API that allows the
; user to specify a desired heading and speed, and the API will
; attempt to control the robot in an appropriate way.
; Also includes several math subroutines.
; This code uses the timer interrupt for the control code.
ORG 0 ; Jump table is located in mem 0-4
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
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 code
;***************************************************************
Main:
OUT RESETPOS ; reset odometer in case wheels moved after programming
; Before enabling the movement control code, set it to
; not start moving immediately.
LOADI 0
STORE DVel ; desired forward velocity
IN THETA
STORE DTheta ; desired heading
; configure timer interrupts to enable the movement control code
LOADI 10 ; 10ms * 10 = 0.1s rate, 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 a CLI &B0010 to disable the interrupt.
; As a quick demo of the movement control, the robot is first
; directed to turn in-place 90 degrees clockwise, then
; turn back to 0 degrees while also moving foward, then turn
; clockwise 90 degrees while moving backwards.
; During all the movements, a short subroutine reads the IR codes
; and displays them to the 7-segs just to demonstrate how those
; values are read.
CALL WaitRemote ; Wait for user input (button 1)
LOADI 0
STORE DVel ; desired forward velocity
LOADI 270
STORE DTheta ; desired heading
; The robot should automatically start moving,
; trying to match these desired parameters.
Turn90:
CALL GetThetaErr
CALL Abs
ADDI -5
JPOS Turn90
CALL WaitRemote ; Wait for user input (button 1)
LOAD FFast ; 500 mm/s
STORE DVel ; desired forward velocity
LOADI 0 ; 5 degrees
STORE DTheta
FArc:
CALL GetThetaErr
CALL Abs
ADDI -5
JPOS FArc
LOAD Zero
STORE DVel
CALL WaitRemote
LOAD RFast
STORE DVel
LOADI 270
STORE DTheta
BArc:
CALL GetThetaErr
CALL Abs
ADDI -5
JPOS BArc
LOAD Zero
STORE DVel
JUMP ForeverDisp
Test1:
CALL IRDisp ; Display the current IR code
CALL GetThetaErr ; get the heading error
CALL Abs
OUT LCD ; useful debug info
ADDI -5 ; check if within 5 degrees
JPOS Test1 ; if not, keep testing
; the robot is now within 5 degrees of 270
LOADI 0 ; new heading
STORE DTheta
LOAD FMid ; forward velocity
STORE DVel
Test2:
CALL IRDisp ; Display the current IR code
CALL GetThetaErr ; get the heading error
CALL Abs
OUT LCD ; useful debug info
ADDI -5 ; check if within 5 degrees
JPOS Test2 ; if not, keep testing
; the robot is now within 5 degrees of 0
LOADI 270 ; new heading
STORE DTheta
LOAD RFast ; fast reverse velocity
STORE DVel
; NOTE: RFast is 500, which is outside the recommended
; range for DVel. You might notice that the robot is
; slow to reach the desired heading.
Test3:
CALL IRDisp ; Display the current IR code
CALL GetThetaErr ; get the heading error
CALL Abs
OUT LCD ; useful debug info
ADDI -5 ; check if within 5 degrees
JPOS Test3 ; if not, keep testing
; the robot is now within 5 degrees of 0
; done
LOADI 0
STORE DVel
ForeverDisp:
CALL IRDisp ; Display the current IR code
JUMP ForeverDisp
IRDisp:
IN IR_HI ; get the high word
OUT SSEG1
IN IR_LO ; get the low word
OUT SSEG2
; OUT IR_HI ; this would reset the value to 0
RETURN
; Wait for user to press 1 on remote
WaitRemote:
CALL IRDisp ; Display IR code
IN IR_LO ; Load most-significant half of IR word (address)
AND HighByte ; Extract most-significant byte
SUB B1 ; Subtract button 1 hex value
JNEG WaitRemote
JPOS WaitRemote
OUT IR_LO
RETURN
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 LEDs
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 ControlMovement
RETI ; return from ISR
; 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 later to get a +/- constant
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 3 ; multiply by another 4
CALL CapValue ; get a +/- max of 50
ADD CMAErr
STORE CMAErr
; For this basic control method, simply take the
; desired forward velocity and add a 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
OUT RVELCMD
LOAD CMAErr
CALL Neg ; left wheel gets negative differential
ADD DVel
CALL CapValue
OUT LVELCMD
RETURN
CMAErr: DW 0 ; holds angle error 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
;***************************************************************
;* Subroutines
;***************************************************************
;*******************************************************************************
; 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 licence 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:
OUT TIMER
Wloop:
IN TIMER
OUT XLEDS ; User-feedback that a pause is occurring.
ADDI -10 ; 1 second at 10Hz.
JNEG Wloop
RETURN
; This subroutine will get the battery voltage,
; and stop program execution if it is too low.
; SetupI2C must be executed prior to this.
BattCheck:
CALL GetBattLvl
JZERO BattCheck ; A/D hasn't had time to initialize
SUB MinBatt
JNEG DeadBatt
ADD MinBatt ; get original value back
RETURN
; If the battery is too low, we want to make
; sure that the user realizes it...
DeadBatt:
LOADI &H20
OUT BEEP ; start beep sound
CALL GetBattLvl ; get the battery level
OUT SSEG1 ; display it everywhere
OUT SSEG2
OUT LCD
LOAD Zero
ADDI -1 ; 0xFFFF
OUT LEDS ; all LEDs on
OUT XLEDS
CALL Wait1 ; 1 second
LOADI &H140 ; short, high-pitched beep
OUT BEEP ; stop beeping
LOAD Zero
OUT LEDS ; LEDs off
OUT XLEDS
CALL Wait1 ; 1 second
JUMP DeadBatt ; repeat forever
; Subroutine to read the A/D (battery voltage)
; Assumes that SetupI2C has been run
GetBattLvl:
LOAD I2CRCmd ; 0x0190 (write 0B, read 1B, addr 0x90)
OUT I2C_CMD ; to I2C_CMD
OUT I2C_RDY ; start the communication
CALL BlockI2C ; wait for it to finish
IN I2C_DATA ; get the returned data
RETURN
; Subroutine to configure the I2C for reading batt voltage
; Only needs to be done once after each reset.
SetupI2C:
CALL BlockI2C ; wait for idle
LOAD I2CWCmd ; 0x1190 (write 1B, read 1B, addr 0x90)
OUT I2C_CMD ; to I2C_CMD register
LOAD Zero ; 0x0000 (A/D port 0, no increment)
OUT I2C_DATA ; to I2C_DATA register
OUT I2C_RDY ; start the communication
CALL BlockI2C ; wait for it to finish
RETURN
; Subroutine to block until I2C device is idle
BlockI2C:
LOAD Zero
STORE Temp ; Used to check for timeout
BI2CL:
LOAD Temp
ADDI 1 ; this will result in ~0.1s timeout
STORE Temp
JZERO I2CError ; Timeout occurred; error
IN I2C_RDY ; Read busy signal
JPOS BI2CL ; If not 0, try again
RETURN ; Else return
I2CError:
LOAD Zero
ADDI &H12C ; "I2C"
OUT SSEG1
OUT SSEG2 ; display error message
JUMP I2CError
;***************************************************************
;* Variables
;***************************************************************
Temp: DW 0 ; "Temp" is not a great name, but can be useful
;***************************************************************
;* Constants
;* (though there is nothing stopping you from writing to these)
;***************************************************************
NegOne: DW -1
Zero: DW 0
One: DW 1
Two: DW 2
Three: DW 3
Four: DW 4
Five: DW 5
Six: DW 6
Seven: DW 7
Eight: DW 8
Nine: DW 9
Ten: DW 10
; Some bit masks.
; Masks of multiple bits can be constructed by ORing these
; 1-bit masks together.
Mask0: DW &B00000001
Mask1: DW &B00000010
Mask2: DW &B00000100
Mask3: DW &B00001000
Mask4: DW &B00010000
Mask5: DW &B00100000
Mask6: DW &B01000000
Mask7: DW &B10000000
LowByte: DW &HFF ; binary 00000000 1111111
LowNibl: DW &HF ; 0000 0000 0000 1111
HighByte: DW &HFF00 ; binary 11111111 00000000
; some useful movement values
OneMeter: DW 961 ; ~1m in 1.04mm units
HalfMeter: DW 481 ; ~0.5m in 1.04mm units
TwoFeet: DW 586 ; ~2ft in 1.04mm units
Deg90: DW 90 ; 90 degrees in odometer units
Deg180: DW 180 ; 180
Deg270: DW 270 ; 270
Deg360: DW 360 ; can never actually happen; for math only
FSlow: DW 100 ; 100 is about the lowest velocity value that will move
RSlow: DW -100
FMid: DW 350 ; 350 is a medium speed
RMid: DW -350
FFast: DW 500 ; 500 is almost max speed (511 is max)
RFast: DW -500
MinBatt: DW 140 ; 14.0V - minimum safe battery voltage
I2CWCmd: DW &H1190 ; write one i2c byte, read one byte, addr 0x90
I2CRCmd: DW &H0190 ; write nothing, read one byte, addr 0x90
; Remote buttons
B1: DW &H2000 ; Remote button 1
;***************************************************************
;* IO address space map
;***************************************************************
SWITCHES: EQU &H00 ; slide switches
LEDS: EQU &H01 ; red LEDs
TIMER: EQU &H02 ; timer, usually running at 10 Hz
XIO: EQU &H03 ; pushbuttons and some misc. inputs
SSEG1: EQU &H04 ; seven-segment display (4-digits only)
SSEG2: EQU &H05 ; seven-segment display (4-digits only)
LCD: EQU &H06 ; primitive 4-digit LCD display
XLEDS: EQU &H07 ; Green LEDs (and Red LED16+17)
BEEP: EQU &H0A ; Control the beep
CTIMER: EQU &H0C ; Configurable timer for interrupts
LPOS: EQU &H80 ; left wheel encoder position (read only)
LVEL: EQU &H82 ; current left wheel velocity (read only)
LVELCMD: EQU &H83 ; left wheel velocity command (write only)
RPOS: EQU &H88 ; same values for right wheel...
RVEL: EQU &H8A ; ...
RVELCMD: EQU &H8B ; ...
I2C_CMD: EQU &H90 ; I2C module's CMD register,
I2C_DATA: EQU &H91 ; ... DATA register,
I2C_RDY: EQU &H92 ; ... and BUSY register
UART_DAT: EQU &H98 ; UART data
UART_RDY: EQU &H98 ; UART status
SONAR: EQU &HA0 ; base address for more than 16 registers....
DIST0: EQU &HA8 ; the eight sonar distance readings
DIST1: EQU &HA9 ; ...
DIST2: EQU &HAA ; ...
DIST3: EQU &HAB ; ...
DIST4: EQU &HAC ; ...
DIST5: EQU &HAD ; ...
DIST6: EQU &HAE ; ...
DIST7: EQU &HAF ; ...
SONALARM: EQU &HB0 ; Write alarm distance; read alarm register
SONARINT: EQU &HB1 ; Write mask for sonar interrupts
SONAREN: EQU &HB2 ; register to control which sonars are enabled
XPOS: EQU &HC0 ; Current X-position (read only)
YPOS: EQU &HC1 ; Y-position
THETA: EQU &HC2 ; Current rotational position of robot (0-359)
RESETPOS: EQU &HC3 ; write anything here to reset odometry to 0
RIN: EQU &HC8
LIN: EQU &HC9
IR_HI: EQU &HD0 ; read the high word of the IR receiver (OUT will clear both words)
IR_LO: EQU &HD1 ; read the low word of the IR receiver (OUT will clear both words)