gscript-ETLModules/GHCO Calibration.gscript at main · bu-gsLab/gscript-ETLModules · GitHub

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
# This script is to configure the gantry's tool_holder_offset (ghco) vector in the flex_config
VERSION v2.7
CLEARLOG
LOADCONFIG
LOADCONFIG "" Config\ETL\TP_Site_Config.txt
LOADCONFIG "" Scripts\ETLModules\ThroughPut\TP_Config.txt

SETLOG "LOG_Calibrate_GHCO.txt"

HOME if-needed
SETVAC * 0

COPY $iterations 3
COPY $step `360/$iterations`
COPY $count 0

@LOOP
    ALERT "Ensure that the test part is staged."

    HOME if-needed

    SETVAC etl_chuck_1_pos_6 1 #for top piece

    MOVENAME etl_chuck_1
    CALL @SURVEY_PART &default.ETROC.1.6.pos &default.ETROC.1.6.rot -> $init_pos $init_rot

    LOADTOOL picker_tool
    PICKPART $init_pos $init_rot etl_chuck_1_pos_6 etl_chuck_1

    SUB $target_rot `$step*$count` 180

    PLACEPART &default.ETROC.1.6.pos $target_rot etl_chuck_1_pos_6

    UNLOADTOOL
    MOVENAME etl_chuck_1

    CALL @SURVEY_PART  &default.ETROC.1.6.pos $target_rot -> $pos_corr $rot_corr
    SUB $delta $pos_corr &default.ETROC.1.6.pos
    COPY [$count] $delta
    PRINT "%d) delta=%v @ %d degrees rotation" $count $delta $target_rot
    SETVAC etl_chuck_1_pos_6 0
    INC $count $count
    GOTOIF @LOOP `$count<$iterations`

FITCIRCLE $center $_ $_ 0 $iterations


COPY $center.z 0
ADD $new_offset &geometry.tool_holder_offset $center
PRINT "New tool holder offset: %v" $new_offset

END

@SURVEY_PART(init_pos,init_rot)

    SETLIGHT 40

    #converts the local coordinates to global gantry coordinates usint start location and orientation
    TRANSFORML2G $tr_global &geometry.ETROC.fid_tr $init_pos $init_rot
    TRANSFORML2G $tl_global &geometry.ETROC.fid_tl $init_pos $init_rot
    TRANSFORML2G $br_global &geometry.ETROC.fid_br $init_pos $init_rot
    TRANSFORML2G $bl_global &geometry.ETROC.fid_bl $init_pos $init_rot

    #this is to get corrected centers and orientation, the code above gives general area
    #Routine to set up inital position of metal piece
    CALL @ACQUIREFIDUCIAL $tl_global -> $tl_corrected
    CALL @ACQUIREFIDUCIAL $tr_global -> $tr_corrected
    CALL @ACQUIREFIDUCIAL $bl_global -> $bl_corrected
    CALL @ACQUIREFIDUCIAL $br_global -> $br_corrected

    # Turns measured corners or fiducials into center and orientation
    FIT $pos $rot $res ETROC $tr_corrected $br_corrected $bl_corrected $tl_corrected
    QUAT2EULER $phi $_ $_ $rot
    RETURN $pos $phi

@ACQUIREFIDUCIAL(start_pos)
    # Arguments:
    #  $start_pos - Position to initially search for the fiducial

    MOVETO $start_pos 50
    WAIT 100

    # GOTO @manual_fiducial_acquisition
    COPY &vision.autofocus.step_size 0.1
    AUTOFOCUS $_ 1 "" autoclose

    SETERRORMODE setvar
    FINDFID $fid_position ETROC
    GOTOIF @manual_fiducial_acquisition $ERR
    SETERRORMODE default
    RETURN $fid_position

    @manual_fiducial_acquisition
    SETERRORMODE default
    VIDEO
    getpos $fid_position
    RETURN $fid_position