-
Notifications
You must be signed in to change notification settings - Fork 0
/
allmotors.sms
1088 lines (911 loc) · 21.9 KB
/
allmotors.sms
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
#define OFFSET_X 1 'Offset guider x stage
#define OFFSET_Y 2 'Offset guider y stage
#define OFFSET_FOCUS 3 'Offset guider focus stage
#define OFFSET_MIRRORS 4
#define OFFSET_FWHEEL 5
#define FWHEEL_LOWER 6
#define FWHEEL_UPPER 7
'This is so the numbers are kept in one place
#define QOFFSET_X "1" 'Offset guider x stage
#define QOFFSET_Y "2" 'Offset guider y stage
#define QOFFSET_FOCUS "3" 'Offset guider focus stage
#define QOFFSET_MIRRORS "4"
#define QOFFSET_FWHEEL "5"
#define QFWHEEL_LOWER "6"
#define QFWHEEL_UPPER "7"
#define OFFSET_X_OK B(12, OFFSET_X)
#define OFFSET_Y_OK B(12, OFFSET_Y)
#define OFFSET_FOCUS_OK B(12, OFFSET_FOCUS)
#define OFFSET_MIRRORS_OK B(12, OFFSET_MIRRORS)
#define OFFSET_FWHEEL_OK B(12, OFFSET_FWHEEL)
#define FWHEEL_LOWER_OK B(12, FWHEEL_LOWER)
#define FWHEEL_UPPER_OK B(12, FWHEEL_UPPER)
#define HEAD_NODE hhh
#define HOME 100
#define MAXVii 0 ' Index of the MAXV in array
#define ACCELii 1 ' Index of accel in array
#define MAXV aa
#define ACCEL bb
#define DECEL cc
#define NEG_LIMIT_RT Bm ' Real time negative limit
#define POS_LIMIT_RT Bp ' Real time positive limit
#define NEG_LIMIT_HIST Bl ' Historical negative limit
#define POS_LIMIT_HIST Br ' Historical positive limit
#define NEG_MSWITCH_POS nnn 'Position of negative mechanical limit switch
#define POS_MSWITCH_POS ppp 'Position of positive mechanical limit switch
#define READY W(0) & 1
#define DEBUG ddd
#define EEP_SOFTWARE_LIMIT 0
#define NEG_SLIMIT dd
#define POS_SLIMIT ee
#define SET_HOMED_BIT US(0) US(16+CADDR):HEAD_NODE
#define GET_HOMED_BIT B(12,0)
#define TURN_ON_HOMING_IRLED OS(4) OS(5)
#define TURN_OFF_HOMING_IRLED OR(4) OR(5)
#define GET_HOMING_SENSOR IN(0)
#define GET_FILTER_SENSOR IN(1)
#define RETRACT_SOLENOID GOSUB(410):FWHEEL_UPPER
#define RELEASE_SOLENOID GOSUB(411):FWHEEL_UPPER
#define HOME_SENSOR IN(0)
#define FILTER_SENSOR IN(1)
#define SET_NODE_READY US(14)
#define GET_NODE_READY B(12,14)
' User bit 8 on the upper filter wheel
' is our lock for filterwheel motion
#define SET_FWHEEL_MOVING US(8):FWHEEL_UPPER
#define UNSET_FWHEEL_MOVING UR(8):FWHEEL_UPPER
#define SET_WATCH_FWHLS US(9):FWHEEL_UPPER
#define UNSET_WATCH_FWHLS UR(9):FWHEEL_UPPER
#define GET_WATCH_FWHLS B(12,9):FWHEEL_UPPER
#define GET_FWHEEL_MOVING B(12,8):FWHEEL_UPPER
#define ISMOVING B(0,2)
#define MOTORON B(0,1) !| 1
#define HEAD_NODE_SETUP_DONE gg
#define FILTER_DIST hh ' Number of counts between filters
#define NEG_SLIMIT_RT Bms
#define POS_SLIMIT_RT Bps
#define NEG_SLIMIT_HIST Bls
#define POS_SLIMIT_HIST Brs
#define FILTER_NUMBER f
#define HOME_OFFSET kk
#define HOME_INACTIVE_STATE ll
#define HOME_ACTIVE_STATE mm
#define HOME_DIRECTION oo
#define SLIMITS_ENABLED B(1,10)
#define MOTORII pp
#define DUMMYVAR qq
#define DUMMYVAR1 rr
#define DUMMYVAR2 ss
#define DUMMYVAR3 tt
#define DUMMYVAR4 uu
#define OTHER_FWHEEL vv
#define IS_MAIN_FWHEEL ww
#define UMIRROR_POS xx
#define CENTER_FIELD_POS yy
#define CALIB_POS zz
BAUD115200
ECHO_OFF
DEBUG = 1
IF DEBUG PRINT("BEGINNING PROGRAM", #13) ENDIF
GOSUB1
SWITCH CADDR
CASE OFFSET_X
' We use the defualt servo constants for Linear starges
BREAK
CASE OFFSET_Y
BREAK
CASE FWHEEL_LOWER
IF DEBUG == 1 PRINT("SETTING UP FWHEEL_LOWER", #13) ENDIF
EIGN(2)
EIGN(3)
SLD
ZS
KP=8000
KI=10
KD=9000
KL=40
WAIT=500
F
IS_MAIN_FWHEEL = 1
BREAK
CASE FWHEEL_UPPER
IF DEBUG == 1 PRINT("SETTING UP FWHEEL_LOWER", #13) ENDIF
EIGN(2)
EIGN(3)
SLD
ZS
KP=8000
KI=10
KD=9000
KL=40
WAIT=500
F
IS_MAIN_FWHEEL = 1
BREAK
CASE OFFSET_FWHEEL
IF DEBUG == 1 PRINT("SETTING UP OFFSET_FWHEEL", #13) ENDIF
EIGN(2) ' Diable the negative limit
EIGN(3) ' Disable the positive limit
SLD ' Disable the software limits
ZS ' Clear the limits
KP=8000'Set the proportional gain
KI=10 ' Set the Integrator gain
KD=9000 ' Set the differential gain
KL=40 ' Set the integrator clip
WAIT=500 ' We must wait half a second or F won't work
F 'Apply servo values.
BREAK
ENDS
END
'Maps the home subroutine to the can address
' This way the command is the same from the
' user point of view.
' For example if the user wants to home OFFSET_X
' and FWHEEL_UPPER they simply use:
' GOSUB(104):1 GOSUB(104):7
C104
SWITCH CADDR
CASE OFFSET_X
GOSUB(103)
BREAK
CASE OFFSET_Y
GOSUB(103)
BREAK
CASE OFFSET_FOCUS
GOSUB(101)
BREAK
CASE OFFSET_MIRRORS
GOSUB(103)
BREAK
CASE OFFSET_FWHEEL
GOSUB(102)
BREAK
CASE FWHEEL_LOWER
GOSUB(102)
BREAK
CASE FWHEEL_UPPER
GOSUB(102)
BREAK
ENDS
RETURN
C103 ' Home assuming the home sensor is only active at the index (edge detection)
' This subroutine is only used for the linear stages.
'Algorithm:
'
' 1. Disable software limits so we can get to the limit switch.
' 2. Turn on the home opitcal sensor
' 3. Using velocity mode, move towared the positive limit.
' 4. When limit is reached, move toward home sensor.
' 5. When home is reached, store home position and ramp down to stop.
' 6. Using positin mode, move to stored home position.
' 7. Set the homed bit
' 8. Enable software limits
' 9. Turn off the home optical sensor
SWITCH CADDR ' Bail if I am the wrong motor
CASE OFFSET_FWHEEL
END
BREAK
CASE FWHEEL_UPPER
END
BREAK
CASE FWHEEL_LOWER
END
BREAK
ENDS
IF READY == 1 ' We don't home if the motor isn't ready.
IF DEBUG == 1 PRINT("HOMING MOTOR", #13) ENDIF
IF SLIMITS_ENABLED
eee = 1
ELSE
eee = 0
ENDIF
IF eee == 1
SLD 'Disable software limits
ENDIF
TURN_ON_HOMING_IRLED
WAIT=100
IF HOME_SENSOR == HOME_ACTIVE_STATE ' We are in the home state go to the limit
MV
VT=HOME_DIRECTION*MAXV/4
G TWAIT
IF eee SLE ENDIF ' let C110 know about slimits
WAIT=100
GOSUB110
PRT=-HOME_DIRECTION*20000
G TWAIT
ENDIF
MV
VT=HOME_DIRECTION*MAXV/4
G
IF DEBUG == 1 PRINT("HEADING TO HOME", #13) ENDIF
WHILE 1
' Wait till homing sensor state changes
' and record position of home sensor
IF HOME_SENSOR == HOME_ACTIVE_STATE
IF DEBUG == 1 PRINT("HOME SENSOR TRIPPED", #13) ENDIF
b=PA
BREAK
ELSEIF NEG_LIMIT_RT ' oops we hit a limit
IF DEBUG == 1 PRINT("Hit limit first moving back", #13) ENDIF
GOSUB(110) ' move out of the limit
MP
PRT=20000
G TWAIT ' Move passed the home sensor
MV
VT=HOME_DIRECTION*MAXV/4 ' start again
G
ENDIF
LOOP
X TWAIT
' Move to home sensor position
MP
PT=b
G TWAIT
O=0 ' Set home sensor position to 0
SET_HOMED_BIT
TURN_OFF_HOMING_IRLED
OFF
VT=MAXV
IF eee == 1 ' Leave limits as we found them.
SLE
ENDIF
IF DEBUG == 1 PRINT("MOTOR HOMED", #13) ENDIF
ELSE
IF DEBUG == 1 PRINT("MOTOR NOT READY") ENDIF
ENDIF
RETURN
C102 ' Home assuming the home sensor is only active at the index (edge detection)
' This subroutine is only used for the filter wheel stages
'Algorithm:
' 1. Turn on home optical sensor
' 2. Using velocity mode, move towared in the home sensor.
' 3. When home is reached, store home position and ramp down to stop.
' 4. Using positin mode, move to stored home position.
' 5. Set the homed bit
' 6. Enable software limits
' 7. Turn off home optical sensor
SWITCH CADDR ' Bail if this isn't a filter wheel
CASE OFFSET_X
END
BREAK
CASE OFFSET_Y
END
BREAK
CASE OFFSET_FOCUS
END
BREAK
CASE OFFSET_MIRRORS
END
BREAK
ENDS
IF DEBUG == 1 PRINT("FILTER HOME CALLED", #13) ENDIF
IF READY == 1 ' We don't home if the motor isn't ready.
AT=ACCEL
IF DEBUG == 1 PRINT("HOMING MOTOR", #13) ENDIF
TURN_ON_HOMING_IRLED
IF IS_MAIN_FWHEEL
GOSUB(499):FWHEEL_LOWER
GOSUB(499):FWHEEL_UPPER
RETRACT_SOLENOID
ENDIF
MV
VT=MAXV/2
G
DUMMYVAR1=10
WHILE 1
' Wait till homing sensor state changes
' and record position of home sensor
IF IS_MAIN_FWHEEL
'GOSUB(499):FWHEEL_LOWER
'GOSUB(499):FWHEEL_UPPER
IF DEBUG PRINT("CHECKING FWHEELS", #13) ENDIF
ENDIF
IF HOME_SENSOR == HOME_ACTIVE_STATE
'IF DEBUG == 1 PRINT("HOME SENSOR TRIPPED", #13) ENDIF
IF FILTER_SENSOR == HOME_ACTIVE_STATE
'IF DEBUG == 1 PRINT("FILTER SENSOR TRIPPED", #13) ENDIF
b=PA
BREAK
ENDIF
ENDIF
LOOP
X TWAIT
IF DEBUG == 1 PRINT("HOME IS AT ", b, #13) ENDIF
' Move to home sensor position
MP
VT=MAXV
PT=b+HOME_OFFSET
G TWAIT
O=0 ' Set origin at the home sensor position
SET_HOMED_BIT
IF IS_MAIN_FWHEEL
GOSUB498
ELSE
OFF
ENDIF
TURN_OFF_HOMING_IRLED
VT=MAXV
IF DEBUG == 1 PRINT("MOTOR HOMED", #13) ENDIF
ELSE
IF DEBUG == 1 PRINT("MOTOR NOT READY") ENDIF
ENDIF
RETURN
C101 'HOME at the limits'
IF CADDR != OFFSET_FOCUS
END
ENDIF
SLD
MV
VT=MAXV/4
G
TWAIT
POS_MSWITCH_POS=PA
GOSUB(110)
MV
VT=-MAXV/4
G
TWAIT
NEG_MSWITCH_POS=PA
GOSUB(110)
PT=NEG_MSWITCH_POS+(POS_MSWITCH_POS-NEG_MSWITCH_POS)/2
G
TWAIT
O=0
PT=PA
VT=MAXV
SET_HOMED_BIT
OFF
RETURN
C100 ' Home all the motors in series
IF OFFSET_X_OK
IF DEBUG == 1 PRINT("HOMING OFFSET_X", #13) ENDIF
GOSUB(103):OFFSET_X
WHILE GET_HOMED_BIT:OFFSET_X == 0
WAIT=100
LOOP
IF DEBUG == 1 PRINT("HOMED OFFSET_X", #13) ENDIF
ENDIF
IF OFFSET_Y_OK
IF DEBUG == 1 PRINT("HOMING OFFSET_Y", #13) ENDIF
GOSUB(103):OFFSET_Y
WHILE GET_HOMED_BIT:OFFSET_Y == 0
WAIT=100
LOOP
OFF:OFFSET_Y
ENDIF
IF OFFSET_FOCUS_OK
IF DEBUG == 1 PRINT("HOMING OFFSET_FOCUS", #13) ENDIF
GOSUB(101):OFFSET_FOCUS
WHILE GET_HOMED_BIT:OFFSET_FOCUS == 0
WAIT=100
LOOP
OFF:OFFSET_FOCUS
ENDIF
IF OFFSET_MIRRORS_OK
IF DEBUG == 1 PRINT("HOMING OFFSET_MIRRORS", #13) ENDIF
GOSUB(103):OFFSET_MIRRORS
WHILE GET_HOMED_BIT:OFFSET_MIRRORS == 0
WAIT=100
LOOP
OFF:OFFSET_MIRRORS
ENDIF
IF FWHEEL_LOWER_OK
IF DEBUG == 1 PRINT("HOMING FWHEEL_LOWER", #13) ENDIF
GOSUB(102):FWHEEL_LOWER
WHILE GET_HOMED_BIT:FWHEEL_LOWER == 0
WAIT=100
LOOP
OFF:FWHEEL_LOWER
ENDIF
IF FWHEEL_UPPER_OK
IF DEBUG == 1 PRINT("HOMING FWHEEL_UPPER", #13) ENDIF
GOSUB(102):FWHEEL_UPPER
WHILE GET_HOMED_BIT:FWHEEL_UPPER == 0
WAIT=100
LOOP
ENDIF
IF OFFSET_FWHEEL_OK
IF DEBUG == 1 PRINT("HOMING OFFSET_FWHEEL", #13) ENDIF
GOSUB(102):OFFSET_FWHEEL
WHILE GET_HOMED_BIT:OFFSET_FWHEEL == 0
WAIT=100
LOOP
ENDIF
RETURN
C110 'Get out of limit
'Algorithm:
' 1. If in neg limit, move positive 5000 counts
' 2. If in pos limit, move negative 5000 counts
SWITCH CADDR ' Bail if I am the wrong motor
CASE OFFSET_FWHEEL
END
BREAK
CASE FWHEEL_UPPER
END
BREAK
CASE FWHEEL_LOWER
END
BREAK
ENDS
IF DEBUG PRINT("MOVING OUT OF LIMIT", #13) ENDIF
eee=0
IF SLIMITS_ENABLED
eee=1
IF DEBUG PRINT("CHECKING SOFT LIMIT", #13) ENDIF
SLD
IF NEG_SLIMIT_HIST == 1
IF DEBUG == 1 PRINT("NEG SLIMIT", #13) ENDIF
Zls 'reset limit
WAIT=100
PRT=5000
G TWAIT
ELSEIF POS_SLIMIT_HIST == 1
IF DEBUG == 1 PRINT("POS SLIMIT", #13) ENDIF
Zrs ' reset limit
WAIT=100
PRT=-5000
G TWAIT
ENDIF
ENDIF
MP
IF NEG_LIMIT_RT == 1
IF DEBUG == 1 PRINT("NEG LIMIT", #13) ENDIF
Zl 'reset limit
PRT=5000
G TWAIT
ELSEIF POS_LIMIT_RT == 1
IF DEBUG == 1 PRINT("POS LIMIT", #13) ENDIF
Zr ' reset limit
PRT=-5000
G TWAIT
ENDIF
IF eee
SLE
ENDIF
OFF
RETURN
C120 'Set the software limits
' Algorithm:
' 1. Home the motor
' 2. Using vel mode move the motor toward the positive limit
' 3. When the limit is reached, set the positive software limit to just inside of the limit
' 4. Move out of the limit
' 5. Using vel mode move toware the negative limit
' 6. When the limit is reached, set the neg software limit to just inside of the limit
' 7. Move out of the limit
' 8. Enable the software limits
SWITCH CADDR ' Bail if I am the wrong motor
CASE OFFSET_FWHEEL
END
BREAK
CASE FWHEEL_UPPER
END
BREAK
CASE FWHEEL_LOWER
END
BREAK
ENDS
IF READY == 1
IF GET_HOMED_BIT == 1
SLD
MV
VT=MAXV/4
G
WHILE POS_LIMIT_RT == 0 LOOP
SLP=PA-2000
POS_SLIMIT = SLP
EPTR = EEP_SOFTWARE_LIMIT+4
VST(POS_SLIMIT, 1)
GOSUB110
IF DEBUG PRINT("POS SLIMIT ") ENDIF
IF DEBUG PRINT(SLP, #13) ENDIF
MV
VT=-MAXV/4
G
WHILE NEG_LIMIT_RT == 0 LOOP
SLN = PA+2000
NEG_SLIMIT = SLN
EPTR = EEP_SOFTWARE_LIMIT
VST(NEG_SLIMIT, 1)
IF DEBUG PRINT("NEG SLIMIT ") ENDIF
IF DEBUG PRINT(SLN, #13) ENDIF
GOSUB110
SLE
ELSE
IF DEBUG PRINT("MOTOR NOT HOMED", #13) ENDIF
ENDIF
ELSE
IF DEBUG PRINT("MOTOR NOT READY", #13) ENDIF
ENDIF
RETURN
C105 ' Home sub assuming the home switch is active half of the travel.
' This is only used for a test linear stage.
IF DEBUG PRINT("Homing Motor", #13) ENDIF
MV
IF IN(10) == 0 ' positive side of home
IF DEBUG PRINT("MOVING toward home form positive side", #13) ENDIF
VT=-MAXV/2
G
WHILE IN(10) == 0 LOOP
X TWAIT
MP
PRT=-5000
G TWAIT
ENDIF
MV
VT=MAXV/2
G
IF DEBUG PRINT("HOMING From negative side", #13) ENDIF
WHILE IN(10) == 1
b=PA
LOOP
X
TWAIT
MP
PT=b
G
TWAIT
O=0
IF DEBUG PRINT("HOMED", #13) ENDIF
RETURN
C0 ' Startup stuff the head node motor
' This code only runs on the head node
' It writes the trajectory constants
' to all motors (CADDR=1..7) on the
' CAN bus.
' TODO: If an error is recieved Bs = 1
' it means that the motor in question
' is not available and we should fault
' all motors until the issue is resolved
' like checking for spares
'Head node is whoever recieves GOSUB0 command from the serial port
HEAD_NODE:0 = CADDR 'Let everyone know the head node address
'Clear the command error bit
Zs
' Push Trajectory constants
' and other info to all motors
MAXV:OFFSET_X = 300000
ACCEL:OFFSET_X = 500
DECEL:OFFSET_X = 500
HOME_INACTIVE_STATE:OFFSET_X = 0
HOME_ACTIVE_STATE:OFFSET_X = 1
HOME_DIRECTION:OFFSET_X = -1
WAIT=100 ' Give the error register time
IF Bs == 1
IF DEBUG == 1 PRINT("Motor 1 (X_stage) not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:OFFSET_X
US(OFFSET_X)
ENDIF
Zs
MAXV:OFFSET_Y = 300000
ACCEL:OFFSET_Y = 500
DECEL:OFFSET_Y = 500
HOME_INACTIVE_STATE:OFFSET_Y = 0
HOME_ACTIVE_STATE:OFFSET_Y = 1
HOME_DIRECTION:OFFSET_Y = -1
WAIT=100
IF Bs == 1
IF DEBUG == 1 PRINT("Motor 2 (Y stage) not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:OFFSET_Y
US(OFFSET_Y)
ENDIF
Zs
MAXV:OFFSET_FOCUS = 300000
ACCEL:OFFSET_FOCUS = 500
DECEL:OFFSET_FOCUS = 500
HOME_DIRECTION:OFFSET_FOCUS = 1
HOME_INACTIVE_STATE:OFFSET_FOCUS = 0
HOME_ACTIVE_STATE:OFFSET_FOCUS = 1
HOME_DIRECTION:OFFSET_FOCUS = -1
WAIT=100
IF Bs == 1
IF DEBUG == 1 PRINT("Motor 3 (Focus stage) not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:OFFSET_FOCUS
US(OFFSET_FOCUS)
ENDIF
Zs
MAXV:OFFSET_MIRRORS = 300000
ACCEL:OFFSET_MIRRORS = 500
DECEL:OFFSET_MIRRORS = 500
HOME_INACTIVE_STATE:OFFSET_MIRRORS = 0
HOME_ACTIVE_STATE:OFFSET_MIRRORS = 0
HOME_DIRECTION:OFFSET_MIRRORS = -1
UMIRROR_POS:OFFSET_MIRRORS = 5600
CENTER_FIELD_POS:OFFSET_MIRRORS = 335800
CALIB_POS:OFFSET_MIRRORS = 0
WAIT=100
IF Bs == 1
IF DEBUG == 1 PRINT("U-Mirror Stage not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:OFFSET_MIRRORS
US(OFFSET_MIRRORS)
ENDIF
Zs
MAXV:OFFSET_FWHEEL = 3000
ACCEL:OFFSET_FWHEEL = 2
DECEL:OFFSET_FWHEEL = 2
FILTER_DIST:OFFSET_FWHEEL = 800
HOME_OFFSET:OFFSET_FWHEEL = 0
HOME_INACTIVE_STATE:OFFSET_FWHEEL = 1
HOME_ACTIVE_STATE:OFFSET_FWHEEL = 0
HOME_DIRECTION:OFFSET_FWHEEL = 1
WAIT=100
IF Bs == 1
IF DEBUG == 1 PRINT("OFFSET Filter Wheel not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:OFFSET_FWHEEL
US(OFFSET_FWHEEL)
ENDIF
Zs
MAXV:FWHEEL_UPPER = 10000
ACCEL:FWHEEL_UPPER = 2
DECEL:FWHEEL_UPPER = 2
FILTER_DIST:FWHEEL_UPPER =1600
HOME_OFFSET:FWHEEL_UPPER = 0
HOME_INACTIVE_STATE:FWHEEL_UPPER = 1
HOME_ACTIVE_STATE:FWHEEL_UPPER = 0
HOME_DIRECTION:FWHEEL_UPPER = -1
WAIT=100
IF Bs == 1
IF DEBUG == 1 PRINT("Upper Filter Wheel not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:FWHEEL_UPPER
US(FWHEEL_UPPER)
ENDIF
Zs
WHILE Bs==1
IF DEBUG PRINT("COMMAD ERROR", #13) ENDIF
WAIT=3000
LOOP
MAXV:FWHEEL_LOWER = 10000
ACCEL:FWHEEL_LOWER = 2
DECEL:FWHEEL_LOWER = 2
FILTER_DIST:FWHEEL_LOWER = 1600
HOME_OFFSET:FWHEEL_LOWER = 0
HOME_INACTIVE_STATE:FWHEEL_UPPER = 1
HOME_ACTIVE_STATE:FWHEEL_UPPER = 0
HOME_DIRECTION:FWHEEL_UPPER = 1
WAIT=100
IF Bs == 1
IF DEBUG == 1 PRINT("Lower Filter wheel not accessible on CAN", #13 ) ENDIF
ELSE
SET_NODE_READY:FWHEEL_LOWER
US(FWHEEL_LOWER)
ENDIF
RETURN
C400 ' go to filter
SWITCH CADDR ' Bail if this isn't a filter wheel
CASE OFFSET_X
END
BREAK
CASE OFFSET_Y
END
BREAK
CASE OFFSET_FOCUS
END
BREAK
CASE OFFSET_MIRRORS
END
BREAK
ENDS
MP
AT=ACCEL
IF f >= 0
IF f < 5
PT = f * FILTER_DIST
IF IS_MAIN_FWHEEL ' Deal with the solenoid
GOSUB(499):FWHEEL_LOWER
GOSUB(499):FWHEEL_UPPER
RETRACT_SOLENOID
ENDIF
G
WHILE ISMOVING
IF IS_MAIN_FWHEEL
'GOSUB(499):FWHEEL_LOWER
'GOSUB(499):FWHEEL_UPPER
ENDIF
LOOP
IF FILTER_SENSOR == 0
IF DEBUG PRINT("WE are on the filter!", #13) ENDIF
ENDIF
'TURN_OFF_HOMING_IRLED
IF IS_MAIN_FWHEEL
GOSUB(498)
ELSE
OFF
ENDIF
ENDIF
ENDIF
RETURN
C500 'Linear stage go to
SWITCH CADDR ' Bail if this isn't a linear stage wheel
CASE FWHEEL_LOWER
END
BREAK
CASE FWHEEL_UPPER
END
BREAK
CASE OFFSET_FWHEEL
END
BREAK
ENDS
IF READY == 0
IF DEBUG PRINT("MOTOR NOT READY", #13) ENDIF
ENDIF
G TWAIT
OFF
RETURN
C501 ' UMIRROR goto
IF CADDR != OFFSET_MIRRORS
END
ENDIF
MP
PT=UMIRROR_POS
G TWAIT
OFF
RETURN
C502 ' Centerfield GOTO
IF CADDR != OFFSET_MIRRORS
END
ENDIF
MP
PT=CENTER_FIELD_POS
G TWAIT
OFF
RETURN
C503 ' Calib GOTO
IF CADDR != OFFSET_MIRRORS
END
ENDIF
MP
PT=CALIB_POS
G TWAIT
OFF
RETURN
C1 ' All nodes startup
' This subroutine is meant to be run by all nodes
xxx = 1
WHILE GET_NODE_READY == 0
WAIT=500 ' Dont flood the can bus
LOOP
xxx = 2
IF CADDR == HEAD_NODE
IF DEBUG PRINT( "Starting setup for all motors", #13 ) ENDIF
ENDIF
xxx = 3
VT=MAXV
AT=ACCEL
DT=DECEL
xxx = 4
EPTR=EEP_SOFTWARE_LIMIT
VLD(NEG_SLIMIT, 1)
xxx = 5
EPTR=EEP_SOFTWARE_LIMIT+4
VLD(POS_SLIMIT, 1)
xxx=6
IF NEG_SLIMIT == 0
SLD
IF DEBUG == 1 PRINT("SLIMIT not in EEPROM, Disabling 1", #13 ) ENDIF
ELSEIF POS_SLIMIT == 0
SLD
IF DEBUG == 1 PRINT("SLIMIT not in EEPROM, Disabling 2", #13 ) ENDIF
ELSEIF POS_SLIMIT == NEG_SLIMIT
SLD
IF DEBUG == 1 PRINT("SLIMIT not in EEPROM, Disabling 2", #13 ) ENDIF
ELSE
SLN=NEG_SLIMIT
SLP=POS_SLIMIT
SLE
ENDIF
xxx=7
RETURN
C55 ' Clear the command error
Zs
WHILE Bs == 1
IF DEBUG == 1 PRINT("Clearing command error.", #13) ENDIF
WAIT=1000
LOOP
RETURN
C50 ' Attempt to find spare
RETURN
C410 ' Retract the solenoid
IF CADDR != FWHEEL_UPPER
END
ENDIF
'Turn on solenoid sensor
OS(7) OS(8)
' Retract the solenoid
OS(9)
WAIT=100
WHILE IN(6) == 0
WAIT=100
LOOP
IF DEBUG PRINT("SOLENOID RETRACTED", #13) ENDIF
RETURN
C411 ' Release the solenoid
IF CADDR != FWHEEL_UPPER
END
ENDIF
WAIT=1000
OR(9)
OS(7) OS(8)
WAIT=100
WHILE IN(6) == 1
WAIT=100
LOOP
OFF:FWHEEL_UPPER OFF:FWHEEL_LOWER
' Set the acceleration to 0 in for safety.