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
/
robot2.bsx
1742 lines (1392 loc) · 58 KB
/
robot2.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
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
' {$PBASIC 2.5}
' {$STAMP BS2sx}
'=============================================================================================================
'========== CONDITIONAL COMPILATION ==========================================================================
'=============================================================================================================
' To prevent code from being compiled simply comment out the #DEFINE line
'#DEFINE TWITCH_DEBUG
'#DEFINE TWITCH_TEST
#DEFINE COMPETITION_ROBOT
#DEFINE COMPETITION_BUTTON_BOX
'=============================================================================================================
'========== 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
'---------- Operator Interface (OI) - Analog Inputs ----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
p1_x VAR byte 'Port 1, X-axis on Joystick
pwm5 VAR p1_x
tmp1 VAR p1_x
'p2_x VAR byte 'Port 2, X-axis on Joystick
p3_x VAR byte 'Port 3, X-axis on Joystick
pwm7 VAR p3_x
tmp3 VAR p3_x
p4_x VAR byte 'Port 4, X-axis on Joystick
pwm8 VAR p4_x
tmp4 VAR p4_x
p1_y VAR byte 'Port 1, Y-axis on Joystick
pwm1 VAR p1_y
'p2_y VAR byte 'Port 2, Y-axis on Joystick
p3_y VAR byte 'Port 3, Y-axis on Joystick
pwm3 VAR p3_y
p4_y VAR byte 'Port 4, Y-axis on Joystick
pwm4 VAR p4_y
p1_wheel VAR byte 'Port 1, Wheel on Joystick
pwm9 VAR p1_wheel
p2_wheel VAR byte 'Port 2, Wheel on Joystick
pwm10 VAR p2_wheel
'p3_wheel VAR byte 'Port 3, Wheel on Joystick
'pwm11 VAR p3_wheel
p4_wheel VAR byte 'Port 4, Wheel on Joystick
pwm6 VAR p4_wheel
tmp2 VAR p4_wheel
'p1_aux VAR byte 'Port 1, Aux on Joystick
pwm12 VAR byte
'p2_aux VAR byte 'Port 2, Aux on Joystick
pwm13 VAR byte
'p3_aux VAR byte 'Port 3, Aux on Joystick
p4_aux VAR byte 'Port 4, Aux on Joystick
pwm2 VAR p4_aux
'---------- 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 (RC) - Analog Inputs ------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
sensor1 VAR byte 'RC Analog Input 1, connector pin 2
sensor2 VAR byte 'RC Analog Input 2, connector pin 16
sensor3 VAR byte 'RC Analog Input 3, connector pin 5
sensor4 VAR byte 'RC Analog Input 4, connector pin 19
sensor5 VAR byte 'RC Analog Input 5, connector pin 8
'sensor6 VAR byte 'RC Analog Input 6, connector pin 22
'sensor7 VAR byte 'RC Analog Input 7, connector pin 11
bat_volt VAR byte 'RC Analog Input 8, hardwired to the Battery
'Vin = ((4.7/14.7)* Battery voltage)-0.4
'Binary Battery Voltage = (Vin/5.0 V)*255
'---------- 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
'---------- Temporary Variables ------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
tmp5 VAR byte
'Keep track of the the user_mode bit (RC request to see user data instead of LEDs)
tmp5_manybits2__force_auto VAR tmp5.bit0 'User requested robot to run auto regardless of auton bit from OI
tmp5_manybits2__prev_user_mode VAR tmp5.bit1 'Previous loop value of the PBMode.user_mode
tmp5_manybits2__many2 VAR tmp5.bit2
tmp5_manybits2__many3 VAR tmp5.bit3
tmp5_manybits2__loop_cnt VAR tmp5.HIGHNIB 'Keep loop count...incrememnted each loop
'THESE MUST MATCH WITH robot3/3A ALSO
waypoint_tmp5 VAR tmp5
waypoint_tmp5__side VAR waypoint_tmp5.BIT7 'Top bit indicates side of the field
waypoint_tmp5__prog_side VAR waypoint_tmp5.HIGHNIB 'Program number combined with side is the program to run
waypoint_tmp5__idx VAR waypoint_tmp5.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)
'=============================================================================================================
'========== 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
wing_feedbk_sw VAR oi_swB.bit0 'p2_sw_trig
stack_feedbk_sw 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_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
TRIM_ZONE CON 1
TWITCH_THRESHOLD CON 5 'distance pots must rotate to pass twitch test
'--- Wings
WING_TURBO_UP CON 126 'must be < 127
WING_FAST_UP CON 126 'was 30 'must be < 127
WING_MED_UP CON 25
WING_SLOW_UP CON 20
WING_TURBO_DOWN CON 126
WING_FAST_DOWN CON 50 'must be < 127
WING_MED_DOWN CON 20
WING_SLOW_DOWN CON 15
WING_MANU_SPEED_DN CON 20
WING_MANU_SPEED_UP CON 20
WING_POSITION_NEAR CON 4
WING_POSITION_CLOSE CON 15
WING_POSITION_FAR CON 30
#IF COMPETITION_ROBOT #THEN
'=================Competition ROBOT=========
'--- Back wing
BWING_POT_0_DEG CON 215 '0 Degrees is full up
BWING_POT_90_DEG CON 124 '90 Degrees is straight out
BWING_POT_FULL_DN CON 79 'Full down when on top of the ramp
FWING_POT_0_DEG CON 186 '0 Degrees is full up
FWING_POT_90_DEG CON 94 '90 Degrees is straight out
FWING_POT_FULL_DN CON 53 'Full down when on top of the ramp
BWING_BRAKE_ENGAGED_PWM CON 127 'PWM value to engage the back brake
BWING_BRAKE_DISENGAGED_PWM CON 0 'PWM value to disengage the back brake
FWING_BRAKE_ENGAGED_PWM CON 127 'PWM value to engage the front brake
FWING_BRAKE_DISENGAGED_PWM CON 0 'PWM value to disengage the front brake
#ELSE
'=================Proto ROBOT===============
'--- Back wing
BWING_POT_0_DEG CON 221 '0 Degrees is full up
BWING_POT_90_DEG CON 138 '90 Degrees is straight out
BWING_POT_FULL_DN CON 84 'Full down when on top of the ramp
FWING_POT_0_DEG CON 180 '0 Degrees is full up
FWING_POT_90_DEG CON 96 '90 Degrees is straight out
FWING_POT_FULL_DN CON 44 'Full down when on top of the ramp
BWING_BRAKE_ENGAGED_PWM CON 127 'PWM value to engage the back brake
BWING_BRAKE_DISENGAGED_PWM CON 254 'PWM value to disengage the back brake
FWING_BRAKE_ENGAGED_PWM CON 127 'PWM value to engage the front brake
FWING_BRAKE_DISENGAGED_PWM CON 0 'PWM value to disengage the front brake
'===========================================
#ENDIF
'There should be no need to change these ever...they are just the values for full
' up and down that can be programmed into the autonomous waypoints. All OI values
' are scaled to these and then the intermediate values is scaled to the pot values.
WING_DEG_0_DEG CON 0 'Intermediate Target value for 0 degrees (full up)
WING_DEG_90_DEG CON 90 'Intermediate Target value for 90 degrees
WING_DEG_FULL_DN CON 135 'Intermediate Target value for full down
WING_DEG_TOP_DIFF CON WING_DEG_90_DEG - WING_DEG_0_DEG
WING_DEG_BOTTOM_DIFF CON WING_DEG_FULL_DN - WING_DEG_90_DEG
WING_DEG_DIFF CON WING_DEG_FULL_DN - WING_DEG_0_DEG
WING_BRAKE_ENGAGED_FLAG CON 1 'Flag to show brake needs to be engaged
WING_BRAKE_DISENGAGED_FLAG CON 0 'Flag to show brake needs to be disengaged
#IF COMPETITION_BUTTON_BOX #THEN
'=================Competition Button Box=========
'--- Back wing
BWING_OI_0_DEG CON 185
BWING_OI_90_DEG CON 100
BWING_OI_FULL_DN CON 67
'--- Front wing
FWING_OI_0_DEG CON 170
FWING_OI_90_DEG CON 83
FWING_OI_FULL_DN CON 41
#ELSE
'=================Proto Button Box===============
'--- Back wing
BWING_OI_0_DEG CON 182
BWING_OI_90_DEG CON 99
BWING_OI_FULL_DN CON 61
'--- Front wing
FWING_OI_0_DEG CON 191
FWING_OI_90_DEG CON 114
FWING_OI_FULL_DN CON 87
'================================================
#ENDIF
BWING_OI_TOP_DIFF CON BWING_OI_0_DEG - BWING_OI_90_DEG
BWING_OI_BOTTOM_DIFF CON BWING_OI_90_DEG - BWING_OI_FULL_DN
BWING_OI_FULL_DIFF CON BWING_OI_0_DEG - BWING_OI_FULL_DN
FWING_OI_TOP_DIFF CON FWING_OI_0_DEG - FWING_OI_90_DEG
FWING_OI_BOTTOM_DIFF CON FWING_OI_90_DEG - FWING_OI_FULL_DN
FWING_OI_DIFF CON FWING_OI_0_DEG - FWING_OI_FULL_DN
'--- Stacker
STACKER_MANU_SPEED_DN CON 45
STACKER_MANU_SPEED_UP CON 120
STACKER_OI_MIN CON 6
STACKER_OI_MAX CON 254
STACKER_OI_DIFF CON STACKER_OI_MAX - STACKER_OI_MIN
STACKER_POT_MIN_POS CON 55
STACKER_POT_BIN_GRAB CON 64
STACKER_POT_BIN_RELEASE CON 149
STACKER_POT_MAX_POS CON 160
STACKER_POT_DIFF CON STACKER_POT_BIN_RELEASE - STACKER_POT_BIN_GRAB
STACKER_UP_FAST CON 127 'must be < 127
STACKER_UP_MED CON 55
STACKER_UP_SLOW CON 30
STACKER_DOWN_FAST CON 75 'must be < 127
STACKER_DOWN_MED CON 20
STACKER_DOWN_SLOW CON 15
STACKER_POSITION_NEAR CON 4
STACKER_POSITION_CLOSE CON 10
STACKER_POSITION_FAR CON 15
'---- Starting points for robot
'WARNING...MUST CHANGE THIS DEFINE IN robot3 ALSO
WAYPOINT_RIGHT_SIDE CON 0 'Must be a bit
WAYPOINT_LEFT_SIDE CON 1
STARTING_RIGHT_POS_X CON 69
STARTING_RIGHT_POS_Y CON 63 '128" from center of ramp(127)
STARTING_RIGHT_ORIENT CON 0
STARTING_LEFT_POS_X CON 69
STARTING_LEFT_POS_Y CON 178 '154" from center of ramp(127)
STARTING_LEFT_ORIENT CON 0
STARTING_RIGHT_ORIENT_90 CON 64 'When started in 90 degree orientation for sideways stack hit, use this
STARTING_LEFT_ORIENT_90 CON 64
#IF COMPETITION_BUTTON_BOX #THEN
'=================Competition Button Box=========
'Settings on the pot on the OI that pick between programs
WAYPOINT_OI_SEL0 CON 4 'matches 'diff' define to cover 0
WAYPOINT_OI_SEL1 CON 17
WAYPOINT_OI_SEL2 CON 42
WAYPOINT_OI_SEL3 CON 65
WAYPOINT_OI_SEL4 CON 88
WAYPOINT_OI_SEL5 CON 112
WAYPOINT_OI_SEL6 CON 134
WAYPOINT_OI_SEL7 CON 158
WAYPOINT_OI_SEL8 CON 181
WAYPOINT_OI_SEL9 CON 205
WAYPOINT_OI_SEL10 CON 229
WAYPOINT_OI_DIFF CON 4
#ELSE
'=================Proto Button Box===============
'Settings on the pot on the OI that pick between programs
WAYPOINT_OI_SEL0 CON 4
WAYPOINT_OI_SEL1 CON 26
WAYPOINT_OI_SEL2 CON 49
WAYPOINT_OI_SEL3 CON 72
WAYPOINT_OI_SEL4 CON 94
WAYPOINT_OI_SEL5 CON 117
WAYPOINT_OI_SEL6 CON 139
WAYPOINT_OI_SEL7 CON 161
WAYPOINT_OI_SEL8 CON 184
WAYPOINT_OI_SEL9 CON 206
WAYPOINT_OI_SEL10 CON 230
WAYPOINT_OI_DIFF CON 4
#ENDIF
'Remember, left side sets the highest bit of list num (ends up adding 32 to list)
'THESE MUST MATCH WITH robot3 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
'Going higher than this value on the crab stick will clear the force auton bit
Y_VALUE_TO_STOP_FORCE_AUTON CON 170
'WARNING, THIS MUST MATCH UP WITH THE DEFINE IN robot3...CHANGE IN robot3 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.
'loop_cnt will go from 0 to this number. This can be used to do something special
' based on the current loop...like sending back certain data back to the Palm
LOOP_COUNT_MAX CON 5
'------------------RC To CC Communication Information...In robot3.bsx also -----------------
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_PROG_SIDE CON 190 'Command byte sent to CC to set our program number and side
CC_OUTBAUD CON 240
CC_INBAUD CON 240
'=============================================================================================================
'========== DEFINE CONSTANTS (DO NOT CHANGE) =================================================================
'=============================================================================================================
' Baud rate for communications with User CPU
OUTBAUD CON 20 '(62500, 8N1, Noninverted)
INBAUD CON 20 '(62500, 8N1, Noninverted)
USERCPU CON 4
FPIN CON 1
COMA CON 1
COMB CON 2
COMC CON 3
'=============================================================================================================
'========== MAIN DO LOOP =====================================================================================
'=============================================================================================================
'---------- Blink BASIC RUN LED ---------------
Toggle 7 'Basic Run LED on the RC is toggled ON/OFF every loop.
'GET s_PB_mode, PB_mode 'PB_mode is now a persistent ram variable across banks.
GET s_fwing_tgt, tmp1
'debug "Auto=", dec nvr_PB_mode__auton_mode, "WTrg8=", dec tmp1, " "
if (nvr_PB_mode__auton_mode = 0) then
#IF TWITCH_TEST #THEN
Gosub TwitchTest
#ENDIF
endif
Gosub WriteOutputs
Gosub ReadInputs
'Increment the loop count...if count goes past MAX+1 then reset it back to 0.
' This may be a waste if MAX is the MAX number that loop_cnt can hold
GET s_manybits2,tmp5
tmp5_manybits2__loop_cnt=tmp5_manybits2__loop_cnt+1
if(tmp5_manybits2__loop_cnt=(LOOP_COUNT_MAX+1)) then
tmp5_manybits2__loop_cnt=0
endif
PUT s_manybits2,tmp5
if (nvr_manybits__first_loop = 1 or nvr_PB_mode__user_display_mode = 0) then
'On the first loop (or when not in user display mode) we need to decode the selector knob and also the
' side selector switch. Setting to user_display_mode will lock in the data.
GET s_waypoint, tmp5
'First decode the waypoint selector knob to a waypoint list
'ASSUMES p2_wheel is unchanged from ReadInputs
select (p2_wheel)
case (WAYPOINT_OI_SEL0 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL0 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST0
case (WAYPOINT_OI_SEL1 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL1 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST1
case (WAYPOINT_OI_SEL2 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL2 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST2
case (WAYPOINT_OI_SEL3 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL3 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST3
case (WAYPOINT_OI_SEL4 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL4 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST4
case (WAYPOINT_OI_SEL5 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL5 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST5
case (WAYPOINT_OI_SEL6 - WAYPOINT_OI_DIFF) TO (WAYPOINT_OI_SEL6 + WAYPOINT_OI_DIFF)
waypoint_tmp5__prog_side=AUTO_LIST6
case else
waypoint_tmp5__prog_side=AUTO_LIST_INVALID
endselect
'debug "Wheel=", dec p2_wheel, " Prog=", dec waypoint_tmp5__prog_side
GET s_rc_swA, rc_swA
'Then set the side bit of the prog_side variable to indicate which side of the field
' is being used. Now, prog_side is a unique variable that picks a list that is specific
' to the program number and to the side of the field.
'IMPORTANT...__side is a bit in __prog_side so it should be set after __prog_side
if (rc_sw2 = WAYPOINT_RIGHT_SIDE) then
waypoint_tmp5__side = WAYPOINT_RIGHT_SIDE
PUT s_curr_pos_x, STARTING_RIGHT_POS_X
PUT s_curr_pos_y, STARTING_RIGHT_POS_Y
' if(waypoint_tmp5__prog_side&WAYPOINT_PROG_ONLY_MASK = AUTO_LIST3) then
' PUT s_curr_orient, STARTING_RIGHT_ORIENT_90
' else
PUT s_curr_orient, STARTING_RIGHT_ORIENT
' endif
else
waypoint_tmp5__side = WAYPOINT_LEFT_SIDE
PUT s_curr_pos_x, STARTING_LEFT_POS_X
PUT s_curr_pos_y, STARTING_LEFT_POS_Y
' if(waypoint_tmp5__prog_side&WAYPOINT_PROG_ONLY_MASK = AUTO_LIST3) then
' PUT s_curr_orient, STARTING_LEFT_ORIENT_90
' else
PUT s_curr_orient, STARTING_LEFT_ORIENT
' endif
endif
'Always send the current position to the CC when not in user mode so we properly initialize theta if the program selector knob changes
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]
'Set waypoint index to invalid so we will ask the CC for our current index
waypoint_tmp5__idx = DEFAULT_WAYPOINT_IDX
' debug " Sw=", dec rc_sw2, " Side=", dec waypoint_tmp5__side
'We changed our program so we should store it down to the CC so if we reset then
' the CC will be able to tell us this information.
Serout 16, CC_OUTBAUD, [CC_CMD_SET_PROG_SIDE, waypoint_tmp5__prog_side]
PUT s_waypoint, tmp5
endif
Gosub RunPump
'GET s_PB_mode, PB_mode 'PB_mode is now a persistent ram variable across banks.
Gosub SkidPad
if (nvr_PB_mode__auton_mode = 1) then
'do Autonomous stuff
Gosub BackWingScaling
Gosub BackWingControl
Gosub FrontWingScaling
Gosub FrontWingControl
' Gosub StackerFeedback
else
'do non-Autonomous stuff
' if (limbo mode) then
'limbo
' PUT s_bwing_tgt, BWING_LIMBO_POS
' Gosub BackWingControl
' PUT s_fwing_tgt, FWING_LIMBO_POS
' Gosub FrontWingControl
' PUT s_stacker_tgt, STACKER_POT_MIN_POS
' Gosub StackerHookControl
' Gosub StackerFeedback
Gosub ScraperControl
' else
if (wing_feedbk_sw = 1) then 'feedback button for wings
Gosub BackWingInput
Gosub BackWingScaling
Gosub BackWingControl
Gosub FrontWingInput
Gosub FrontWingScaling
Gosub FrontWingControl
else
Gosub WingManualMode
endif
'THE STACKER HAS LEFT THE BUILDING
' if (stack_feedbk_sw = 1) then 'feedback button for wings
' Gosub StackerInputScaling
' Gosub StackerHookControl
' Gosub StackerFeedback
' else
' Gosub StackerManualMode
' endif
' endif
endif
Run 2
'GET s_PB_mode, PB_mode 'PB_mode is now a persistent ram variable across banks.
if (nvr_PB_mode__auton_mode = 1) then
Run 2
else
Run 3
endif
'===========================================
'========= ReadInputs Subroutine ===========
'===========================================
ReadInputs:
'---------- Serin Command - Get Data from Master uP -----------------------------------------------------
' Construct the "serin" command using the following rules:
' 1) There must be one variable for every input defined in the "Define Constants for Init" section.
' 2) The order must match the order in the EXAMPLE SERIN COMMAND below.
' 3) The total number of all variables may not exceed 26.
' 4) Only use one "Serin" command.
' 5) The Serin command must occupy one line.
'
' If you see a BASIC INIT ERR on the Robot Controller after programming and pressing RESET, then
' there is a problem with the Serin command below. Check the number of variables. A BASIC INIT ERR
' will not occur if you have the variables in the wrong order, however your code will not work correctly.
'
' EXAMPLE SERIN COMMAND
' This example exceed the 26 variable limit and is not on one line:
'
' Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,p2_x,p1_x,p4_x,p3_x,PB_mode,packet_num,sensor1,
' sensor2,p2_y,p1_y,sensor3,sensor4,p4_y,p3_y,sensor5,sensor6,p2_wheel,p1_wheel,
' sensor7,sensor8,p4_wheel,p3_wheel,p2_aux,p1_aux,p4_aux,p3_aux,delta_t,res01]
'
Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,p1_x,p4_x,p3_x,nvr_PB_mode,sensor1,
sensor2,p1_y,sensor3,sensor4,p4_y,sensor5,p2_wheel,
bat_volt,p4_wheel,p4_aux]
'Store ram vars in an un-aliased scratchpad location. This is helpful
'since independent subroutines may want to see the raw input data.
' Mappings for 2003 inputs
'**************************************************************************************************************
'* Analog inputs
'* sensor1 Left crab
'* sensor2 Right crab
'* sensor3 Front wing
'* sensor4 Back wing
'* sensor5 Stacker
'*
'* Digital switches
'* rc_sw1 Air pressure guage
'* rc_sw2 Right / left autonomous selector switch
'*
'* Joystick Analog
'* Input Var Scratchpad RAM Function
'* p4_x s_crab_x Crab X
'* p4_y s_crab_y Crab Y
'* p4_wheel s_drive_x Drive X
'* p4_aux s_drive_y Drive Y
'*
'* Joystick Digital
'* Input Var Alias Function
'* p4_sw_trig crab_trig crab stick trigger
'* p4_sw_top crab_top 90 degree offset
'* p4_sw_aux1 drive_trig Lower skid pad
'* p4_sw_aux2 drive_top Turbo
'*
'* Button Box Analog
'* Input Var Scratchpad RAM Function
'* p1_x s_fwing_oi Front wing presets
'* p1_y s_bwing_oi Back wing presets
'* p2_wheel s_waypoint_sel Waypoint selector knob
'* p3_x s_stacker_oi Stacker presets
'*
'* Button Box Digital
'* Input Var Alias Function
'* p1_sw_trig fwing_up_sw Front wing up
'* p1_sw_top fwing_dn_sw Front wing down
'* p1_sw_aux1 bwing_up_sw Back wing up
'* p1_sw_aux2 bwing_dn_sw Back wing down
'* p2_sw_trig wing_feedbk_sw Wing feedback
'* p2_sw_top stack_feedbk_sw Stacker feedback
'* p2_sw_aux1
'* p2_sw_aux2
'* p3_sw_trig stacker_up_sw Stacker up
'* p3_sw_top stacker_dn_sw Stacker down
'* p3_sw_aux1 scraper_sw Scraper activate/deactivate
'* p3_sw_aux2 crab_ovr_sw Crab manual override
'*
'**************************************************************************************************************
PUT s_oi_swA, oi_swA
GET s_manybits2, tmp5
if(tmp5_manybits2__prev_user_mode = 0 and nvr_PB_mode__user_display_mode = 1) then
'Last loop user display mode was off, this loop it is on...We just entered user display mode
'When the drive button and top are pressed when entering user display mode we want to go to force auto mode
' so that we will be in auto mode right when we are enabled.
if(drive_top = 1 and drive_trig = 1) then
'We have now entered forced auton mode
tmp5_manybits2__force_auto = 1
endif
endif
if(nvr_PB_mode__auton_mode = 1 or nvr_PB_mode__user_display_mode = 0 or p4_y > Y_VALUE_TO_STOP_FORCE_AUTON) then
'When in true auton mode, remove the force_auto bit so we will not be in force_auto
' when auton mode finishes
tmp5_manybits2__force_auto = 0
endif
if(tmp5_manybits2__force_auto = 1) then
'When in forced auto, set auto to 1 regardless of the actual setting
nvr_PB_mode__auton_mode = 1
endif
'Save off current user mode so we know its value next loop
tmp5_manybits2__prev_user_mode = nvr_PB_mode__user_display_mode
PUT s_manybits2, tmp5
PUT s_oi_swB, oi_swB
PUT s_rc_swA, rc_swA
'PUT s_rc_swB, rc_swB
PUT s_drive_x, p4_wheel
PUT s_fwing_oi, p1_x
PUT s_crab_x, p4_x
PUT s_stacker_oi, p3_x
'PUT s_PB_mode, PB_mode 'PB_mode is no- ew a persistent ram variable across banks.
PUT s_lcrab_pot, sensor1
PUT s_rcrab_pot, sensor2
PUT s_drive_y, p4_aux
PUT s_bwing_oi, p1_y
PUT s_fwing_pot, sensor3
PUT s_bwing_pot, sensor4
PUT s_crab_y, p4_y
'PUT s_p3_y, p3_y
PUT s_stacker_pot, sensor5
'PUT s_p1_wheel, p1_wheel
'PUT s_p1_aux, p1_aux
'only save the waypoint selector switch value in non-autonomous mode because in autonomous mode the input will go to 127
'if (nvr_PB_mode__auton_mode = 0) then
' PUT s_waypoint_sel, p2_wheel
'endif
'debug "stack: ", DEC p3_x, " u ", DEC stacker_up_sw, " d ", DEC stacker_dn_sw, CR
'debug "fwingOI: ", DEC p1_x, " bwingOI: ", DEC p1_y, " stack: ", DEC p3_x, CR
'debug "fwing u ", DEC fwing_up_sw, " d ", DEC fwing_dn_sw, " bwing u ", DEC bwing_up_sw, " d ", DEC bwing_dn_sw, CR
'debug "stack u ", DEC stacker_up_sw, " d ", DEC stacker_dn_sw, " scraper ", DEC scraper_sw, " crab manual ", DEC crab_ovr_sw, CR
'debug "wing feedback ", DEC wing_feedbk_sw, " stacker feedback ", DEC stack_feedbk_sw, " p2_sw_aux1 ", DEC p2_sw_aux1, " p2_sw_aux2 ", DEC p2_sw_aux2, CR
'debug "drive x", DEC p4_wheel, " y ", DEC p4_aux, CR
'debug "crab x ", DEC p4_x, " y ", DEC p4_y, CR
'Uncomment this to output the current values of the crab pots
'debug " Lpot ", DEC sensor1, " Rpot ", DEC sensor2, CR
'Uncomment this to output the current values of the wing pots
'debug " Fwing pot ", DEC sensor3, " Bwing pot ", DEC sensor4, " Fwing oi ", DEC p1_x, " Bwing oi " , DEC p1_y,CR
'Uncomment this to output the current values of the stacker pot
'debug " Stacker pot ", DEC sensor5, " Stacker OI=", DEC p3_x, CR
'Uncomment this to output the waypoint selector knowb
'debug " Waypoint selector ", DEC p2_wheel, " switch ", DEC rc_sw2, CR
'...add any other inputs as needed.
'Output to LEDs when Drive Joysticks are centered
' left (crab) joystick
'GET s_crab_y, tmp5
'if (tmp5 > 127-TRIM_ZONE) and (tmp5 < 127+TRIM_ZONE) then Out10 = 1 else Out10 = 0
'GET s_crab_x, tmp5
'if (tmp5 > 127-TRIM_ZONE) and (tmp5 < 127+TRIM_ZONE) then Out12 = 1 else Out12 = 0
' right (drive) joystick
'GET s_drive_y, tmp5
'if (tmp5 > 127-TRIM_ZONE) and (tmp5 < 127+TRIM_ZONE) then Out11 = 1 else Out11 = 0
'GET s_drive_x, tmp5
'if (tmp5 > 127-TRIM_ZONE) and (tmp5 < 127+TRIM_ZONE) then Out13 = 1 else Out13 = 0
return 'from ReadInputs
'=============================================
'========= WriteOutputs Subroutine ===========
'=============================================
WriteOutputs:
' write the output vars with safe values, in case someone forgets
' to update them from memory.
'pwm1 = 127
pwm2 = 127
pwm3 = 127
pwm4 = 127
pwm5 = 127
pwm6 = 127
pwm7 = 127
pwm8 = 127
pwm9 = 127
pwm10 = 127
'pwm11 = 127
pwm12 = 127
pwm13 = 127
'pwm14 = 127
'pwm15 = 127
'pwm16 = 127
relayA = 0
relayB = 0
' Do the final output value GETs before writing to outputs. overwrite
' the safe defaults.
' Don't map functionality to their outputs until this moment. (That
' way if electrical changes their mind, we only need to change it here.)
' Mappings for 2003 outputs
'**************************************************************************************************************
'* color table of relays
'* 1 none air pump
'* 2 red/black front skid plate
'* 3 purple/black back skid plate
'* 4 brown/black scraper
'* 5 orange/black
'* 6 yellow/black
'* 7 grey/black
'* 8 blue/black light
'*
'* color table of pwms
'* 1 none
'* 2 red front left drive
'* 3 purple right crab
'* 4 brown left crab
'* 5 orange front right drive
'* 6 yellow front wing
'* 7 grey back wing
'* 8 green back left drive
'* 9 blue back right drive
'* 10 white stacker
'* 11 black
'* 12 front brake
'* 13 rear brake
'**************************************************************************************************************
GET s_lf_cur, pwm2
GET s_rf_cur, pwm5
GET s_lb_cur, pwm8
GET s_rb_cur, pwm9
'------TURN OFF DRIVE
'pwm2=127
'pwm5=127
'pwm8=127
'pwm9=127
'--------------------
GET s_right_crab, pwm3
GET s_left_crab, pwm4
'-------TURN OFF CRABS-----
'debug "Pots are disabled in robot2.bsx", CR
'pwm3 = 127
'pwm4 = 127
'--------------------------
GET s_front_wing, pwm6
GET s_back_wing, pwm7
'-------TURN OFF WINGS-----
'debug "Wings are disabled in robot2.bsx", CR
#IF COMPETITION_ROBOT #THEN
'=================Competition ROBOT
'pwm6 = 127
'pwm7 = 127
#else
'=================Proto ROBOT