This repository has been archived by the owner on Aug 13, 2021. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
robot3.bsx
987 lines (775 loc) · 40.7 KB
/
robot3.bsx
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
' {$PBASIC 2.5}
' {$STAMP BS2sx}
'=============================================================================================================
'========== CONDITIONAL COMPILATION ==========================================================================
'=============================================================================================================
' To prevent code from being compiled simply comment out the #DEFINE line
' Comment out line to use pot & joystick values for proto robot
#DEFINE COMPETITION_ROBOT
'=============================================================================================================
'========== WAYPOINTS ========================================================================================
'=============================================================================================================
'---------------- TargetSpeed must be <= 127 --------------------
'starting points at michigan L = (69, 178) R = (69, 63)
'Initial point out of alley
' Long: L = (45,166) R = (45,75) Range 3
' Short: L = (36,162) R = (36,79) Range 12
' Tight: L = (40,157) R = (40,84) Range 12
'-----Hug our side of the ramp and follow through to bottom of ramp. (BBS Routine)
' Pt0 Pt1 Pt2 Pt3 Pt4
List0TargetPositionXRight DATA 40, 66, 125, 196, 0
List0TargetPositionXLeft DATA 40, 66, 125, 196, 0
List0TargetPositionYRight DATA 84, 104, 108, 108, 0
List0TargetPositionYLeft DATA 157, 137, 133, 133, 0
List0TargetOrientation DATA 0, 0, 0, 0
List0TargetSpeed DATA 127, 127, 127, 127
List0TargetFWingPosition DATA 0, 0, 45, 45
List0TargetBWingPosition DATA 0, 0, 90, 90
List0TargetWaypointRange DATA 12, 3, 3, 3
LIST0_NUMBER_WAYPOINTS CON 4
'-----Double hit and follow through. Hit partner's half of stack first. Continue over ramp to our scoring zone.
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5 Pt6
List1TargetPositionXRight DATA 40, 113, 124, 97, 137, 202, 0 '(point 2 was 119)
List1TargetPositionXLeft DATA 40, 113, 124, 97, 137, 202, 0 '(point 2 was 119)
List1TargetPositionYRight DATA 84, 140, 140, 133, 107, 101, 0
List1TargetPositionYLeft DATA 157, 101, 101, 108, 134, 140, 0
List1TargetOrientation DATA 0, 0, 0, 0, 0, 0
List1TargetSpeed DATA 127, 127, 127, 60, 127, 127
List1TargetFWingPosition DATA 0, 0, 45, 45, 45, 45
List1TargetBWingPosition DATA 0, 0, 90, 90, 90, 90
List1TargetWaypointRange DATA 12, 80, 3, 3, 3, 3
LIST1_NUMBER_WAYPOINTS CON 6
'SLOW START POINT => X=33 Yr=79 Yl=162
'-----One on the carpet, one on the top of the ramp(our side of center), one following through to the bottom
' of the other side parking in front of the opponent on same side as start
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5
List2TargetPositionXRight DATA 40, 127, 127, 187, 214
List2TargetPositionXLeft DATA 40, 127, 127, 187, 214
List2TargetPositionYRight DATA 84, 114, 114, 114, 81
List2TargetPositionYLeft DATA 157, 127, 127, 127, 160
List2TargetOrientation DATA 0, 0, 0, 0, 0
List2TargetSpeed DATA 127, 127, 127, 127, 127
List2TargetFWingPosition DATA 0, 0, 45, 45, 45
List2TargetBWingPosition DATA 0, 0, 90, 90, 90
List2TargetWaypointRange DATA 12, 66, 3, 3, 3 'Raul said to move the wings opening 12" sooner
LIST2_NUMBER_WAYPOINTS CON 5
'-----One on the carpet, one on the top of the ramp(our side of center), one following through to the bottom
' of the other side parking in front of the opponent on opposite side as start
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5
'---- Over park Right ---
List3TargetPositionXRight DATA 40, 127, 127, 187, 214
List3TargetPositionXLeft DATA 40, 127, 127, 187, 214
List3TargetPositionYRight DATA 84, 114, 114, 114, 160
List3TargetPositionYLeft DATA 157, 127, 127, 127, 81
List3TargetOrientation DATA 0, 0, 0, 0, 0
List3TargetSpeed DATA 127, 127, 127, 127, 127
List3TargetFWingPosition DATA 0, 0, 45, 45, 45
List3TargetBWingPosition DATA 0, 0, 90, 90, 90
List3TargetWaypointRange DATA 12, 66, 3, 3, 3 'Raul said to move the wings opening 12" sooner
LIST3_NUMBER_WAYPOINTS CON 5
'-----Hit the bins in the center (our side of the center line) and park at the top
' Currently ends up coasting just over the top so it ends up on the other side. Need a brake point.
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5
'---- Over park Right ---
List4TargetPositionXRight DATA 40, 127, 127
List4TargetPositionXLeft DATA 40, 127, 127
List4TargetPositionYRight DATA 84, 114, 114
List4TargetPositionYLeft DATA 157, 127, 127
List4TargetOrientation DATA 0, 0, 0
List4TargetSpeed DATA 127, 127, 127
List4TargetFWingPosition DATA 0, 0, 45
List4TargetBWingPosition DATA 0, 0, 90
List4TargetWaypointRange DATA 12, 60, 3
LIST4_NUMBER_WAYPOINTS CON 3
'List4TargetPositionXRight DATA 45, 45, 0, 50, 3, 11, 60, 223
'List4TargetPositionXLeft DATA 45, 45, 0, 50, 3, 11, 60, 223
'List4TargetPositionYRight DATA 75, 90, 61, 177, 179, 123, 117, 117
'List4TargetPositionYLeft DATA 166, 151, 180, 64, 62, 118, 124, 124
'List4TargetOrientation DATA 0, 0, 0, 0, 0, 0, 0, 0
'List4TargetSpeed DATA 127, 127, 127, 127, 127, 127, 127, 127
'List4TargetFWingPosition DATA 0, 0, 0, 0, 0, 0, 0, 45
'List4TargetBWingPosition DATA 0, 0, 0, 0, 0, 0, 0, 45
'List4TargetWaypointRange DATA 12, 5, 5, 12, 12, 12, 12, 12
'LIST4_NUMBER_WAYPOINTS CON 8
'-----One Point: Single hit and then go back to the center of the ramp on the starting side of the ramp.
' Pt0 Pt1 Pt2 Pt3 Pt4
'List5TargetPositionXRight DATA 40, 127, 127, 54
'List5TargetPositionXLeft DATA 40, 127, 127, 54
'List5TargetPositionYRight DATA 84, 114, 114, 123
'List5TargetPositionYLeft DATA 157, 127, 127, 118
'List5TargetOrientation DATA 0, 0, 0, 0
'List5TargetSpeed DATA 127, 127, 127, 127
'List5TargetFWingPosition DATA 0, 0, 45, 45
'List5TargetBWingPosition DATA 0, 0, 90, 90
'List5TargetWaypointRange DATA 12, 60, 3, 3
'LIST5_NUMBER_WAYPOINTS CON 4
'-----Sweep for bins on our starting side of the field and go up and over the ramp stopping in our scoring zone
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5 Pt6
'List5TargetPositionXRight DATA 0, 50, 3, 11, 60, 223
'List5TargetPositionXLeft DATA 0, 50, 3, 11, 60, 223
'List5TargetPositionYRight DATA 61, 177, 179, 123, 117, 117
'List5TargetPositionYLeft DATA 180, 64, 62, 118, 124, 124
'List5TargetOrientation DATA 0, 0, 0, 0, 0, 0
'List5TargetSpeed DATA 90, 127, 127, 127, 127, 127 'First tested with 80
'List5TargetFWingPosition DATA 0, 0, 0, 0, 0, 45
'List5TargetBWingPosition DATA 0, 0, 0, 0, 0, 45
'List5TargetWaypointRange DATA 5, 12, 12, 12, 12, 12
'LIST5_NUMBER_WAYPOINTS CON 6
'-----Avoid attack under bar, sweep for bins on our starting side of the field and go up and over the ramp stopping in our scoring zone
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5 Pt6
List5TargetPositionXRight DATA 40, 45, 0, 50, 3, 11, 60, 223
List5TargetPositionXLeft DATA 40, 45, 0, 50, 3, 11, 60, 223
List5TargetPositionYRight DATA 84, 81, 95, 177, 179, 123, 117, 117
List5TargetPositionYLeft DATA 157, 160, 146, 64, 62, 118, 124, 124
List5TargetOrientation DATA 0, 0, 0, 0, 0, 0, 0, 0
List5TargetSpeed DATA 127, 127, 127, 127, 127, 127, 127, 127
List5TargetFWingPosition DATA 0, 0, 0, 0, 0, 0, 0, 45
List5TargetBWingPosition DATA 0, 0, 0, 0, 0, 0, 0, 45
List5TargetWaypointRange DATA 12, 5, 5, 12, 12, 12, 12, 12
LIST5_NUMBER_WAYPOINTS CON 8
'-----One Point: Single hit and come back to hit close opponent human player stack
' Pt0 Pt1 Pt2 Pt3 Pt4 Pt5 Pt6
List6TargetPositionXRight DATA 40, 127, 127, 66, 30, 0, 57
List6TargetPositionXLeft DATA 40, 127, 127, 66, 30, 0, 57
List6TargetPositionYRight DATA 84, 114, 114, 114, 70, 63, 122
List6TargetPositionYLeft DATA 157, 127, 127, 127, 171, 178, 119
List6TargetOrientation DATA 0, 0, 0, 0, 0, 0, 0
List6TargetSpeed DATA 127, 127, 127, 60, 127, 127, 127
List6TargetFWingPosition DATA 0, 0, 45, 45, 45, 45, 45
List6TargetBWingPosition DATA 0, 0, 90, 45, 45, 45, 45
List6TargetWaypointRange DATA 12, 66, 3, 5, 7, 3, 5 'Raul said to move the wings opening 12" sooner
LIST6_NUMBER_WAYPOINTS CON 7
'----Simple "45 degree" test that was the first auto software tested
'---- it moves from the duct tape square to the duct tape spots
' Pt0 Pt1 Pt2
'RightList8TargetPositionX DATA 151, 161, 169
'RightList8TargetPositionY DATA 151, 151, 127
'RightList8TargetOrientation DATA 0, 0, 0
'RightList8TargetSpeed DATA 20, 20, 20
'RightList8TargetFWingPosition DATA 0, 0, 0
'RightList8TargetBWingPosition DATA 0, 0, 0
'RightList8TargetWaypointRange DATA 2, 2, 2
'RIGHT_LIST8_NUMBER_WAYPOINTS CON 3
'=============================================================================================================
'========== ARCTAN LOOKUP TABLE ==============================================================================
'=============================================================================================================
'=============================================================================================================
'========== DECLARE VARIABLES ================================================================================
'=============================================================================================================
' Below is a list of declared input and output variables. Comment or un-comment
' the variables as needed. Declare any additional variables required in
' your main program loop. Note that you may only use 26 total variables.
'---------- Misc. MUST BE THE FIRST VARIABLES IN ALL BANKS ---------------------------------------------------
'------PERSISTENT VARIABLES -- VALID IN ALL BANKS ------------------------------------------------------------
'NOTE:In order for these variables to be valid in ALL banks, they must be in the exact same spot in each bank.
' To do this, just always keep these as the first variables in each bank.
'packet_num VAR byte
'delta_t VAR byte
'---------- Aliases for the Pbasic Mode Byte (nvr_PB_mode) -------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Bit 7 of the nvr_PB_mode byte (aliased as comp_mode below) indicates the status
' of the Competition Control, either Enabled or Disabled. This indicates the
' starting and stopping of rounds at the competitions.
' Comp_mode is indicated by a solid "Disabled" LED on the Operator Interface.
' Comp_mode = 1 for Enabled, 0 for Disabled.
'
' Bit 6 of the nvr_PB_mode byte (aliased as auton_mode below) indicates the status
' of the Autonomous Mode, either Autonomous or Normal. This indicates when
' the robot must run on its own programming. When in Autonomous Mode, all
' OI analog inputs are set to 127 and all OI switch inputs are set to 0 (zero).
' Auton_mode is indicated by a blinking "Disabled" LED on the Operator Interface.
' Auton_mode = 1 for Autonomous, 0 for Normal.
'
' Autonomous Mode can be turned ON by setting the RC to Team 0 (zero).
'
' Bit 5 of the nvr_PB_mode byte (aliased as user_display_mode below) indicates when
' the user selects the "User Mode" on the OI. nvr_PB_mode.bit5 is set to 1 in "User Mode".
' When the user selects channel, team number, or voltage, nvr_PB_mode.bit5 is set to 0
' When in "User Mode", the eight Robot Feedback LED are turned OFF.
' Note: "User Mode" is identified by the letter u in the left digit (for 4 digit OI's)
' Note: "User Mode" is identified by decimal places on the right two digits (for 3 digit OI's)
nvr_PB_mode VAR byte
nvr_PB_mode__comp_mode VAR nvr_PB_mode.bit7
nvr_PB_mode__auton_mode VAR nvr_PB_mode.bit6
nvr_PB_mode__user_display_mode VAR nvr_PB_mode.bit5
nvr_manybits VAR byte
nvr_manybits__twitchdone VAR nvr_manybits.bit0
nvr_manybits__crab_broken VAR nvr_manybits.bit1
nvr_manybits__first_loop VAR nvr_manybits.bit2
nvr_manybits__first_auto_loop VAR nvr_manybits.bit3
nvr_manybits__prev_loop_ctr VAR nvr_manybits.bit4
nvr_manybits__prev_loop_dir VAR nvr_manybits.bit5
nvr_manybits__fwing_brake VAR nvr_manybits.bit6
nvr_manybits__bwing_brake VAR nvr_manybits.bit7
nvr_2_3_waypoint_x VAR byte
nvr_2_3_waypoint_y VAR byte
nvr_2_3_waypoint_speed VAR byte
nvr_2_3_auton_bits VAR byte
nvr_2_3_auton_bits__auton_stop VAR nvr_2_3_auton_bits.bit0
'---------- Operator Interface (OI) - Analog Inputs ----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'---------- Operator Interface - Digital Inputs --------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
oi_swA VAR byte 'OI Digital Switch Inputs 1 thru 8
oi_swB VAR byte 'OI Digital Switch Inputs 9 thru 16
'---------- Robot Controller - Digital Inputs ----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
rc_swA VAR byte 'RC Digital Inputs 1 thru 8
'rc_swB VAR byte 'RC Digital Inputs 9 thru 16
'---------- Robot Controller - Digital Outputs ---------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
relayA VAR byte
relayB VAR byte
'THIS MUST MATCH WITH robot2/3A/4 ALSO
waypoint VAR byte
waypoint__side VAR waypoint.BIT7 'Top bit indicates side of the field
waypoint__prog_side VAR waypoint.HIGHNIB 'Program number combined with side is the program to run
waypoint__idx VAR waypoint.LOWNIB 'Bottom 4 bits are the current waypoint index we are at
WAYPOINT_PROG_ONLY_MASK CON %0111 'ANDing prog_side with this will return prog only (mask off "side"...bit7)
waypoint_range VAR byte
num_waypoints VAR byte
'---------- Temporary Variables ------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
tmp1 VAR byte
tmp2 VAR byte
tmp3 VAR byte
tmp4 VAR byte
tmp5 VAR byte
tmp6 VAR byte
tmp7 VAR byte
tmp8 VAR byte
'Keep track of the the user_mode bit (RC request to see user data instead of LEDs)
tmp1_manybits2__force_auto VAR tmp1.bit0 'User requested robot to run auto regardless of auton bit from OI
tmp1_manybits2__prev_user_mode VAR tmp1.bit1 'Previous loop value of the PBMode.user_mode
tmp1_manybits2__many2 VAR tmp1.bit2
tmp1_manybits2__many3 VAR tmp1.bit3
tmp1_manybits2__loop_cnt VAR tmp1.HIGHNIB 'Keep loop count...incrememnted each loop
'=============================================================================================================
'========== DEFINE ALIASES ===================================================================================
'=============================================================================================================
' Aliases are variables which are sub-divisions of variables defined
' above. Aliases don't require any additional RAM.
'---------- Aliases for each OI switch input -----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the digital inputs located on the Operator Interface.
' Ports 1 & 3 have their inputs duplicated in ports 4 & 2 respectively. The
' inputs from ports 1 & 3 may be disabled via the 'Disable' dip switch
' located on the Operator Interface. See Users Manual for details.
fwing_up_sw VAR oi_swA.bit0 'p1_sw_trig
fwing_dn_sw VAR oi_swA.bit1 'p1_sw_top
bwing_up_sw VAR oi_swA.bit2 'p1_sw_aux1
bwing_dn_sw VAR oi_swA.bit3 'p1_sw_aux2
stacker_up_sw VAR oi_swA.bit4 'p3_sw_trig
stacker_dn_sw VAR oi_swA.bit5 'p3_sw_top
scraper_sw VAR oi_swA.bit6 'p3_sw_aux1
crab_ovr_sw VAR oi_swA.bit7 'p3_sw_aux2
p2_sw_trig VAR oi_swB.bit0 'p2_sw_trig
p2_sw_top VAR oi_swB.bit1 'p2_sw_top
p2_sw_aux1 VAR oi_swB.bit2 'p2_sw_aux1
p2_sw_aux2 VAR oi_swB.bit3 'p2_sw_aux2
crab_trig VAR oi_swB.bit4 'p4_sw_trig
crab_top VAR oi_swB.bit5 'p4_sw_top
drive_trig VAR oi_swB.bit6 'p4_sw_aux1
drive_top VAR oi_swB.bit7 'p4_sw_aux2
'---------- Aliases for each RC switch input -----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the digital inputs located on the Robot Controller.
rc_sw1 VAR rc_swA.bit0
rc_sw2 VAR rc_swA.bit1
rc_sw3 VAR rc_swA.bit2
rc_sw4 VAR rc_swA.bit3
rc_sw5 VAR rc_swA.bit4
rc_sw6 VAR rc_swA.bit5
rc_sw7 VAR rc_swA.bit6
rc_sw8 VAR rc_swA.bit7
'---------- Aliases for each RC Relay outputs ----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the relay outputs located on the Robot Controller.
relay1_fwd VAR relayA.bit0
relay1_rev VAR relayA.bit1
relay2_fwd VAR relayA.bit2
relay2_rev VAR relayA.bit3
relay3_fwd VAR relayA.bit4
relay3_rev VAR relayA.bit5
relay4_fwd VAR relayA.bit6
relay4_rev VAR relayA.bit7
relay5_fwd VAR relayB.bit0
relay5_rev VAR relayB.bit1
relay6_fwd VAR relayB.bit2
relay6_rev VAR relayB.bit3
relay7_fwd VAR relayB.bit4
relay7_rev VAR relayB.bit5
relay8_fwd VAR relayB.bit6
relay8_rev VAR relayB.bit7
'=============================================================================
'SHARED Constants (these are used by more than one bank and MUST AGREE in each)
TRUE CON 1
FALSE CON 0
'=============================================================================
'Constants for Scratchpad ram locations [0-62] (63 reserved)
'(these constants are shared and must agree in all banks)
'(I just reserved these in the order they come in the serin stream)
s_oi_swA CON 0
s_oi_swB CON 1
s_rc_swA CON 2
s_drive_x CON 3
s_drive_y CON 4
s_crab_x CON 5
s_crab_y CON 6
s_fwing_oi CON 7
s_bwing_oi CON 8
s_stacker_oi CON 9
s_manybits2 CON 10 'additional status bits
's_packet_num CON 11
s_lcrab_pot CON 12 'left crab pot
s_rcrab_pot CON 13 'right crab pot
s_fwing_pot CON 15 'front wing pot
s_bwing_pot CON 16 'back wing pot
's_p3_y CON 17
s_stacker_pot CON 18 'stacker pot
's_sensor6 CON 19
's_p2_wheel CON 20
's_p1_wheel CON 21
's_sensor7 CON 22
'(...end of possible serin inputs. continue now, with outputs and stored user variables.)
s_stacker_tgt CON 25
's_waypoint_sel CON 26
's_waypoint_flgs CON 27 'flags associated with waypoint
s_curr_pos_x CON 32
s_curr_pos_y CON 33
s_curr_orient CON 34 'current compass value (or theta) of the robot
s_target_orient CON 35 'target compass value of the robot
s_fwing_wp_tgt CON 36
s_bwing_wp_tgt CON 37
s_waypoint CON 38
s_target_heading CON 39
s_relayA CON 40
s_relayB CON 41
s_fwing_tgt CON 42
s_bwing_tgt CON 43
s_rcrab_target CON 44
s_lcrab_target CON 45
s_right_crab CON 46 'crab steering motor speed
s_left_crab CON 47
s_rcrab_init CON 48
s_lcrab_init CON 49
s_lf_cur CON 50 'current speed of drive wheels
s_rf_cur CON 51
s_lb_cur CON 52
s_rb_cur CON 53
s_lf_wheel CON 54 'stored value for left_wheel
s_rf_wheel CON 55
s_lb_wheel CON 56
s_rb_wheel CON 57
s_front_wing CON 58 'current speed of wings
s_back_wing CON 59
s_stacker CON 60 'current speed of stacker
s_bat_volt CON 62
s_banknumber CON 63 'Read-Only
' ...insert any other storage names up to 62 that are useful...
' ____ ___ _ _ ____ _____ _ _ _ _____ ____
' / ___/ _ \| \ | / ___|_ _|/ \ | \ | |_ _/ ___|
' | | | | | | \| \___ \ | | / _ \ | \| | | | \___ \
' | |__| |_| | |\ |___) || |/ ___ \| |\ | | | ___) |
' \____\___/|_| \_|____/ |_/_/ \_\_| \_| |_| |____/
'
'Bank Constants (only used in this bank)
ANALOG_TO_BUTTON_HIGH CON 200 'threshold for analog buttons
DRIVE_DEADZONE CON 5
DRIVE_ACCEL_RATE CON 13
DRIVE_SCL CON 50 'turbo drive speed reduction (percent) [e.g. 50]
DRIVE_CON CON 64 '128 * (1- reduction percent) [e.g. 64]
TWO_AXIS_DEADZONE CON 50
CRAB_MODE_DEADZONE CON 20
CRAB_MANU_SPEED CON 50
CRAB_FAST CON 126 'must be < 127
CRAB_MED CON 80
CRAB_SLOW CON 17
CRAB_POSITION_NEAR CON 2
CRAB_POSITION_CLOSE CON 15
CRAB_POSITION_FAR CON 40
CRAB_RIGHT90 CON 18
CRAB_LEFT90 CON 213
AUTO_DRIVE_FWD CON 0
AUTO_DRIVE_REV CON 1
CC_OUTBAUD CON 240
CC_INBAUD CON 240
#IF COMPETITION_ROBOT #THEN
'=================CHANGE THESE IN BANK3 ALSO!!!!!!!!!!!!!!!!!!!!!
'=================Competition ROBOT
POT_RCRAB_RIGHT_180 CON 9 'Right Pot reading for 0 brads (180 degrees to the right)
POT_RCRAB_LEFT_180 CON 244 'Right Pot reading for 255 brads (180 degrees to the left)
'=================END CHANGE BANK3 WARNING!!!!!!!!!!!!!!!!!!!!!!!
#ELSE
'=================CHANGE THESE IN BANK3 ALSO!!!!!!!!!!!!!!!!!!!!!
'=================Proto ROBOT===============
POT_RCRAB_RIGHT_180 CON 12 'Right Pot reading for 0 brads (180 degrees to the right)
POT_RCRAB_LEFT_180 CON 243 'Right Pot reading for 255 brads (180 degrees to the left)
'=================END CHANGE BANK3 WARNING!!!!!!!!!!!!!!!!!!!!!!!
#ENDIF
CC_CMD_REQ_POSITION CON 180 'Command byte sent to CC to request X,Y,theta
CC_CMD_REQ_X_POS CON 85 'Command byte sent to CC to request just X postion
CC_CMD_REQ_Y_POS CON 170 'Command byte sent to CC to request just Y postion
CC_CMD_REQ_THETA CON 240 'Command byte sent to CC to request just theta postion
CC_CMD_SET_POSITION CON 120 'Command byte sent to CC to set X,Y,theta on the CC if it reset
CC_CMD_SET_WAYPOINT CON 130 'Command byte sent to CC to set our current waypoint
CC_CMD_REQ_WAYPOINT CON 140 'Command byte sent to CC to get our current waypoint
CC_CMD_REQ_PROG_SIDE CON 200 'Command byte sent to CC to get our program number and side
'---- Starting points for robot
'WARNING...MUST CHANGE THIS DEFINE IN robot2/3/4 ALSO
WAYPOINT_RIGHT_SIDE CON 0 'must be a bit, changing this effects AUTO_LIST_XXX_ADD below
WAYPOINT_LEFT_SIDE CON 1
'Remember, left side sets the highest bit of list num (ends up adding 32 to list)
'THESE MUST MATCH WITH robot2 ALSO
AUTO_LIST0 CON 0 '0 for right, 8 for left side
AUTO_LIST1 CON 1 '1 for right, 9 for left side
AUTO_LIST2 CON 2 '2 for right, 10 for left side
AUTO_LIST3 CON 3 '3 for right, 11 for left side
AUTO_LIST4 CON 4 '4 for right, 12 for left side
AUTO_LIST5 CON 5 '5 for right, 13 for left side
AUTO_LIST6 CON 6 '6 for right, 14 for left side
'AUTO_LIST7 CON 7 '7 for right, 15 for left side
'AUTO_LIST8 CON 8 '8 for right, 16 for left side
'AUTO_LIST9 CON 9 '9 for right, 17 for left side
'AUTO_LIST10 CON 10 '10 for right,18 for left side
AUTO_LIST_INVALID CON 7 '7 and 15 equate to invalid list number
AUTO_LIST_RIGHT_ADD CON 0 'Setting right side bit to highest bit in nibble adds 0 to the list number
AUTO_LIST_LEFT_ADD CON 8 'Setting left side bit to highest bit in nibble adds 7 to the list number
AUTO_LIST0_RIGHT CON AUTO_LIST0 + AUTO_LIST_RIGHT_ADD
AUTO_LIST0_LEFT CON AUTO_LIST0 + AUTO_LIST_LEFT_ADD
AUTO_LIST1_RIGHT CON AUTO_LIST1 + AUTO_LIST_RIGHT_ADD
AUTO_LIST1_LEFT CON AUTO_LIST1 + AUTO_LIST_LEFT_ADD
AUTO_LIST2_RIGHT CON AUTO_LIST2 + AUTO_LIST_RIGHT_ADD
AUTO_LIST2_LEFT CON AUTO_LIST2 + AUTO_LIST_LEFT_ADD
AUTO_LIST3_RIGHT CON AUTO_LIST3 + AUTO_LIST_RIGHT_ADD
AUTO_LIST3_LEFT CON AUTO_LIST3 + AUTO_LIST_LEFT_ADD
AUTO_LIST4_RIGHT CON AUTO_LIST4 + AUTO_LIST_RIGHT_ADD
AUTO_LIST4_LEFT CON AUTO_LIST4 + AUTO_LIST_LEFT_ADD
AUTO_LIST5_RIGHT CON AUTO_LIST5 + AUTO_LIST_RIGHT_ADD
AUTO_LIST5_LEFT CON AUTO_LIST5 + AUTO_LIST_LEFT_ADD
AUTO_LIST6_RIGHT CON AUTO_LIST6 + AUTO_LIST_RIGHT_ADD
AUTO_LIST6_LEFT CON AUTO_LIST6 + AUTO_LIST_LEFT_ADD
'AUTO_LIST7_RIGHT CON AUTO_LIST7 + AUTO_LIST_RIGHT_ADD
'AUTO_LIST7_LEFT CON AUTO_LIST7 + AUTO_LIST_LEFT_ADD
'AUTO_LIST8_RIGHT CON AUTO_LIST8 + AUTO_LIST_RIGHT_ADD
'AUTO_LIST8_LEFT CON AUTO_LIST8 + AUTO_LIST_LEFT_ADD
'AUTO_LIST9_RIGHT CON AUTO_LIST9 + AUTO_LIST_RIGHT_ADD
'AUTO_LIST9_LEFT CON AUTO_LIST9 + AUTO_LIST_LEFT_ADD
'AUTO_LIST10_RIGHT CON AUTO_LIST10 + AUTO_LIST_RIGHT_ADD
'AUTO_LIST10_LEFT CON AUTO_LIST10 + AUTO_LIST_LEFT_ADD
'AUTO_LIST11_RIGHT CON AUTO_LIST11 + AUTO_LIST_RIGHT_ADD
'AUTO_LIST11_LEFT CON AUTO_LIST11 + AUTO_LIST_LEFT_ADD
'=================CHANGE THESE IN BANK3 ALSO!!!!!!!!!!!!!!!!!!!!!
'Waypoint option flags
AUTO_FLAGS_DRIVE_FWD CON $80 'Force the robot to drive forward to get to this waypoint
AUTO_FLAGS_DRIVE_REV CON $40 'Force the robot to drive in reverse to get to this waypoint
AUTO_FLAGS_HOLD_POSITION CON $20 'Robot will move on to another waypoint and will instead hold this position
'=================END CHANGE BANK3 WARNING!!!!!!!!!!!!!!!!!!!!!!!
BRADS_TO_OPTIMIZE_CRAB CON 96 'If our crab target is farther away than this many brads then we will actually pick the
' opposite 180 degree target and drive backwards. This should not be lower than
' 63 brads (90 degrees) but can be larger so we can take advantage of momentum so that
' if we are half way between the two target (63 brads from target) we continue to the
' same target, but if we are 96 (130 degrees) from target use the opposite target
'WARNING, THIS MUST MATCH UP WITH THE DEFINE IN robot2...CHANGE IN robot2 also
DEFAULT_WAYPOINT_IDX CON $F 'Default waypoint index...indicates RC doesn't know the waypoint
' and needs to ask the CC sice this means the RC has reset.
'On powerup CC defaults waypoint index to 0 so when the robot is
' first turned on the RC will request the index from the CC. This
' will allow the RC to reset in the middle of auton and the CC will
' tell it what waypoint it was tracking to.
'=============================================================================================================
'========== MAIN CODE ========================================================================================
'=============================================================================================================
'GET s_PB_mode, PB_mode 'PB_mode is now a persistent ram variable across banks.
'===== need to abort if pots are broken
'Always read from the Custom Circuit to get our current orientation/theta
nvr_2_3_auton_bits__auton_stop = 0
Gosub InputData
'If we are in auto mode and we are enabled then just output the waypoint
if(nvr_PB_mode__auton_mode = 1) then
'do Autonomous stuff
if (nvr_2_3_auton_bits__auton_stop = 0) then
Gosub WaypointManager
endif
'continue autonomous code
Run 3
endif
'debug "3Done"
Run 4
'==========================================
'========= InputData Subroutine ===========
'== Inputs: none
'== Outputs: s_curr_pos_x - current X position
'== s_curr_pos_y - current y position
'== s_curr_orient - current orientation
'== Modifys: tmp1, tmp2, tmp3
'== Purpose: get current position data from CC
'==========================================
InputData:
'Serout 16, CC_OUTBAUD, [CC_CMD_REQ_POSITION]
'Serin 16, CC_INBAUD, 10, InputDataNoData, [tmp1, tmp2, tmp3]
'request X
Serout 16, CC_OUTBAUD, [CC_CMD_REQ_X_POS]
Serin 16, CC_INBAUD, 5, InputDataNoData, [tmp1]
'request Y
Serout 16, CC_OUTBAUD, [CC_CMD_REQ_Y_POS]
Serin 16, CC_INBAUD, 5, InputDataNoData, [tmp2]
'request theta
Serout 16, CC_OUTBAUD, [CC_CMD_REQ_THETA]
Serin 16, CC_INBAUD, 5, InputDataNoData, [tmp3]
GET s_waypoint, waypoint
if(waypoint__idx = DEFAULT_WAYPOINT_IDX) then
'The RC has just reset, we must ask the CC for what waypoint we were at before the reset.
' On powerup the CC will default the index to 0, so powerup will work fine.
Serout 16, CC_OUTBAUD, [CC_CMD_REQ_WAYPOINT]
Serin 16, CC_INBAUD, 5, WaypointManagerDone, [tmp3]
waypoint__idx = tmp3
endif
'If we reset in the middle of auton mode, then we won't be able to figure out the autonomous
' program number so it will be INVALID. In this case, ask the CC for that info
if((waypoint__prog_side & WAYPOINT_PROG_ONLY_MASK) = AUTO_LIST_INVALID) then
Serout 16, CC_OUTBAUD, [CC_CMD_REQ_PROG_SIDE]
Serin 16, CC_INBAUD, 5, WaypointManagerDone, [tmp3]
waypoint__prog_side = tmp3
endif
PUT s_waypoint, waypoint
'debug "x ", DEC tmp1, " y ", DEC tmp2, " t ", DEC tmp3, CR
'X,Y,Theta of Zero means the CC has reset and doesn't know where it is. We will
' feed it with the last known data (or in a powerup case, the data for the our start
' position on the field.
if ((tmp1 = 0) AND (tmp2 = 0) AND (tmp3 = 0)) then
GET s_curr_pos_x, tmp1
GET s_curr_pos_y, tmp2
GET s_curr_orient, tmp3
Serout 16, CC_OUTBAUD, [CC_CMD_SET_POSITION, tmp1, tmp2, tmp3]
else
PUT s_curr_pos_x, tmp1
PUT s_curr_pos_y, tmp2
PUT s_curr_orient, tmp3
endif
goto InputDataEnd
InputDataNoData:
'NO CUSTOM CIRCUIT!!!
'Toggle 7
'debug "Serin timeout", CR
nvr_2_3_auton_bits__auton_stop = 1
GET s_target_orient, tmp1 'No custom circuit so set the curr theta to target to prevent theta correction
PUT s_curr_orient, tmp1
InputDataEnd:
return 'from InputData
'================================================
'========= WaypointManager Subroutine ===========
'== Inputs:
'== Outputs:
'== Modifys:
'== Purpose:
'================================================
WaypointManager:
'----- change above in ReadWaypoint also -----
GET s_waypoint, waypoint
select (waypoint__prog_side & WAYPOINT_PROG_ONLY_MASK)
case (AUTO_LIST0)
num_waypoints = LIST0_NUMBER_WAYPOINTS
case (AUTO_LIST1)
num_waypoints = LIST1_NUMBER_WAYPOINTS
case (AUTO_LIST2)
num_waypoints = LIST2_NUMBER_WAYPOINTS
case (AUTO_LIST3)
num_waypoints = LIST3_NUMBER_WAYPOINTS
case (AUTO_LIST4)
num_waypoints = LIST4_NUMBER_WAYPOINTS
case (AUTO_LIST5)
num_waypoints = LIST5_NUMBER_WAYPOINTS
case (AUTO_LIST6)
num_waypoints = LIST6_NUMBER_WAYPOINTS
case else
num_waypoints = 0
endselect
if num_waypoints = 0 then WaypointManagerFinished
if (waypoint__idx = num_waypoints) then WaypointManagerFinished
if (nvr_manybits__crab_broken = 1) then WaypointManagerFinished
Gosub ReadWaypoint
'Once we reach a waypoint with the HOLD_POSITION flag set, don't try to move on...just stay
' at this waypoint until auto finishes.
'GET s_waypoint_flgs, tmp1
'if((tmp1&AUTO_FLAGS_HOLD_POSITION) = AUTO_FLAGS_HOLD_POSITION) then WaypointManagerDone
'If the new waypoint is 0,0 then we are finished
if (nvr_2_3_waypoint_x = 0 and nvr_2_3_waypoint_y = 0) then WaypointManagerFinished
'if nvr_manybits__first_auto_loop = 1 then
' goto WaypointManagerDone
'endif
GET s_curr_pos_x, tmp3
GET s_curr_pos_y, tmp4
'compare current position to waypoint
if (nvr_2_3_waypoint_x > tmp3) then
if (nvr_2_3_waypoint_y > tmp4) then
if ((nvr_2_3_waypoint_x - tmp3) <= waypoint_range) and ((nvr_2_3_waypoint_y - tmp4) <= waypoint_range) then
goto WaypointNextWaypoint
endif
else
if ((nvr_2_3_waypoint_x - tmp3) <= waypoint_range) and ((tmp4 - nvr_2_3_waypoint_y) <= waypoint_range) then
goto WaypointNextWaypoint
endif
endif
else
if (nvr_2_3_waypoint_y > tmp4) then
if ((tmp3 - nvr_2_3_waypoint_x) <= waypoint_range) and ((nvr_2_3_waypoint_y - tmp4) <= waypoint_range) then
goto WaypointNextWaypoint
endif
else
if ((tmp3 - nvr_2_3_waypoint_x) <= waypoint_range) and ((tmp4 - nvr_2_3_waypoint_y) <= waypoint_range) then
goto WaypointNextWaypoint
endif
endif
endif
goto WaypointManagerDone
WaypointNextWaypoint:
'debug "o - WP x: ", DEC nvr_2_3_waypoint_x, " y: ", DEC nvr_2_3_waypoint_y, CR
'debug "c - WP x: ", DEC tmp3, " y: ", DEC tmp4, CR
waypoint__idx = waypoint__idx + 1
PUT s_waypoint, waypoint
'Save the waypoint off to the CC so the CC can tell us our waypoint if we reset
Serout 16, CC_OUTBAUD, [CC_CMD_SET_WAYPOINT, waypoint__idx]
if (waypoint__idx = num_waypoints) then
goto WaypointManagerFinished
else
Gosub ReadWaypoint
goto WaypointManagerDone
endif
WaypointManagerFinished:
GET s_fwing_wp_tgt, tmp1
PUT s_fwing_tgt, tmp1
GET s_bwing_wp_tgt, tmp1
PUT s_bwing_tgt, tmp1
nvr_2_3_auton_bits__auton_stop = 1
WaypointManagerDone:
'Gosub ReadWaypoint
PUT s_waypoint, waypoint
return 'from WaypointManager
'=============================================
'========= ReadWaypoint Subroutine ===========
'== Inputs:
'== Outputs:
'== Modifys:
'== Purpose:
'=============================================
ReadWaypoint:
'----- change above in WaypointManager also -----
GET s_waypoint, waypoint
select (waypoint__prog_side & WAYPOINT_PROG_ONLY_MASK)
case (AUTO_LIST0)
'read waypoint 0
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List0TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List0TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List0TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List0TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List0TargetOrientation + waypoint__idx), tmp4
READ (List0TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List0TargetFWingPosition + waypoint__idx), tmp5
READ (List0TargetBWingPosition + waypoint__idx), tmp6
READ (List0TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List0TargetWaypointFlags + waypoint__idx), tmp8
case (AUTO_LIST1)
'read waypoint 1
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List1TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List1TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List1TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List1TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List1TargetOrientation + waypoint__idx), tmp4
READ (List1TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List1TargetFWingPosition + waypoint__idx), tmp5
READ (List1TargetBWingPosition + waypoint__idx), tmp6
READ (List1TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List1TargetWaypointFlags + waypoint__idx), tmp8
case (AUTO_LIST2)
'read waypoint 2
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List2TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List2TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List2TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List2TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List2TargetOrientation + waypoint__idx), tmp4
READ (List2TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List2TargetFWingPosition + waypoint__idx), tmp5
READ (List2TargetBWingPosition + waypoint__idx), tmp6
READ (List2TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List2TargetWaypointFlags + waypoint__idx), tmp8
case (AUTO_LIST3)
'read waypoint 3
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List3TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List3TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List3TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List3TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List3TargetOrientation + waypoint__idx), tmp4
READ (List3TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List3TargetFWingPosition + waypoint__idx), tmp5
READ (List3TargetBWingPosition + waypoint__idx), tmp6
READ (List3TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List3TargetWaypointFlags + waypoint__idx), tmp8
case (AUTO_LIST4)
'read waypoint 4
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List4TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List4TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List4TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List4TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List4TargetOrientation + waypoint__idx), tmp4
READ (List4TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List4TargetFWingPosition + waypoint__idx), tmp5
READ (List4TargetBWingPosition + waypoint__idx), tmp6
READ (List4TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List4TargetWaypointFlags + waypoint__idx), tmp8
case (AUTO_LIST5)
'read waypoint 5
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List5TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List5TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List5TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List5TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List5TargetOrientation + waypoint__idx), tmp4
READ (List5TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List5TargetFWingPosition + waypoint__idx), tmp5
READ (List5TargetBWingPosition + waypoint__idx), tmp6
READ (List5TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List5TargetWaypointFlags + waypoint__idx), tmp8
case (AUTO_LIST6)
'read waypoint 6
if(waypoint__side = WAYPOINT_RIGHT_SIDE) then
READ (List6TargetPositionXRight + waypoint__idx), nvr_2_3_waypoint_x
READ (List6TargetPositionYRight + waypoint__idx), nvr_2_3_waypoint_y
else
READ (List6TargetPositionXLeft + waypoint__idx), nvr_2_3_waypoint_x
READ (List6TargetPositionYLeft + waypoint__idx), nvr_2_3_waypoint_y
endif
READ (List6TargetOrientation + waypoint__idx), tmp4
READ (List6TargetSpeed + waypoint__idx), nvr_2_3_waypoint_speed
READ (List6TargetFWingPosition + waypoint__idx), tmp5
READ (List6TargetBWingPosition + waypoint__idx), tmp6
READ (List6TargetWaypointRange + waypoint__idx), waypoint_range
' READ (List6TargetWaypointFlags + waypoint__idx), tmp8
endselect
'debug "WP x: ", DEC nvr_2_3_waypoint_x, " y: ", DEC nvr_2_3_waypoint_y, CR
'store waypoint info
PUT s_target_orient, tmp4
PUT s_fwing_tgt, tmp5
PUT s_fwing_wp_tgt, tmp5
PUT s_bwing_tgt, tmp6
PUT s_bwing_wp_tgt, tmp6
PUT s_stacker_tgt, tmp7
'PUT s_waypoint_flgs, tmp8
return 'from ReadWaypoint