dt_ctrl.ctrl 32.3 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
#	dt_ctrl.ctrl
#
#	Controller network for Dwingeloo Telescope.
#
#	This file contains the fixed part of the network and its parameters.
#	Other .ctrl files are used for those parts which are conditional,
#	such as the use of simulation networks or real servo drives.
#

# Frequency is limited by the stoeber drives.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
11
12
set frequency 250
set delay 0.0
Jeroen Vreeken's avatar
Jeroen Vreeken committed
13

14
15
16
17
trigger {
	{ "prestart", 0.000150 }
}

18
19
20
21
# dt_ctrl_el.ctrl	Elevation with real servo drives
# dt_ctrl_el_sim.ctrl	Elevation with simulated network
# dt_ctrl_az.ctrl	Azimuth with real servo drives
# dt_ctrl_az_sim.ctrl	Azimuth with simulated network
Jeroen Vreeken's avatar
Jeroen Vreeken committed
22
# dt_ctrl_ec_sim.ctrl   Ethercat simulation block
23
#
24
25
26
# Uncomment either the real network, or the sim network.
# But never both!

27
28
#import "dt_ctrl_el.ctrl"
import "dt_ctrl_el_sim.ctrl"
29

30
31
#import "dt_ctrl_az.ctrl"
import "dt_ctrl_az_sim.ctrl"
32

33
import "dt_ctrl_ec_sim.ctrl"
34

Jeroen Vreeken's avatar
Jeroen Vreeken committed
35
blocks ($(frequency), $(delay)) {
36
37
	{ "setpoint_generator_3d", "azimuth_spg", 	"Azimuth_Setpoint", "rad" }
	{ "servo_state",           "azimuth_servo_state"           }
38
	{ "and4",                  "azimuth_safe_and"              }
39
40
41
42
43
44
45
	{ "subtract",	           "azimuth_setpoint_error"	   }
	{ "subtract",              "azimuth_error"		   }
	{ "pid_aw",	           "azimuth_pid"		   }
	{ "filter_iir",	           "azimuth_pid_filter"		   }
	{ "limit_var",	           "azimuth_pid_limit"		   }
	{ "add",  	           "azimuth_speed_ff" 		   }
	{ "limit",	           "azimuth_speed_limit"	   }
46
	{ "limit_switch",          "azimuth_range_limit"           }
47
48
49
50
51
52
	{ "gain", 	           "azimuth_speed_servo"	   }
	{ "value",	           "azimuth_torque"		   }
	{ "value",	           "azimuth_position_offset"  	   }
	{ "add",  	           "azimuth_position_offset_sum"   }
	{ "gain", 	           "azimuth_position_gain"	   }
	{ "rangecheck",		   "azimuth_speed_warning"         }
53
54
55

	{ "log",                   "azimuth_safety_hw_pos" }
	{ "log",                   "azimuth_safety_hw_neg" }
56
57
58
	{ "rangecheck",            "azimuth_speed_range_negative" }
	{ "rangecheck",            "azimuth_speed_range_positive" }
	{ "or2",                   "azimuth_position_range"       }
59
60
	{ "command_bool",          "azimuth_recover"              }
	{ "and2",                  "azimuth_position_safe"        }
61
	{ "log",                   "azimuth_position_range_log"   }
62
	{ "log",                   "azimuth_position_safe_log"    }
63
		
64
65
66
	{ "matrix_2x2",	           "elevation_input_matrix"	      }
	{ "setpoint_generator_3d", "elevation_spg", 		"Elevation_Setpoint", "rad" }
	{ "servo_state",           "elevation_servo_state"            }
67
 	{ "and5",                  "elevation_safe_and"               }
68
69
70
71
72
73
74
75
76
77
78
79
80
	{ "setpoint_generator_1d", "elevation_torsion_spg",	"Elevation_Torsion_Setpoint", "rad" }
	{ "subtract",	           "elevation_setpoint_error" 	      }
	{ "subtract",  	           "elevation_error"  		      }
	{ "subtract",	           "elevation_torsion_error"  	      }
	{ "subtract",	           "elevation_torsion_setpoint_error" }
	{ "subtract",	           "elevation_torsion_torque" 	      }
	{ "filter_lp",	           "elevation_torsion_torque_lp"      }
	{ "pid_aw",	           "elevation_pid"	      	      }
	{ "pid_aw",  	           "elevation_torsion_pid"	      }
	{ "add",  	           "elevation_speed_ff"		      }
	{ "switch",                "elevation_position_switch"        }
	{ "limit",	           "elevation_torsion_speed_limit"    }
	{ "limit",	           "elevation_speed_limit"	      }
81
82
	{ "limit_switch",          "elevation_range_limit_r"          }
	{ "limit_switch",          "elevation_range_limit_l"          }
83
84
85
86
87
88
89
90
	{ "limit_2nd",	           "elevation_jerk_limit"	      }
	{ "matrix_2x2",	           "elevation_output_matrix"  	      }
	{ "value",	           "elevation_torque_r"		      }
	{ "value",	           "elevation_torque_l"		      }
	{ "value",	           "elevation_position_offset_r"      }
	{ "value",	           "elevation_position_offset_l"      }
	{ "add",  	           "elevation_position_offset_r_sum"  }
	{ "add",  	           "elevation_position_offset_l_sum"  }
91

92
93
	{ "log",                   "elevation_safety_hw_top" }
	{ "log",                   "elevation_safety_hw_bottom" }
94
95
96
97
98
99
100
101
102
	{ "rangecheck",            "elevation_speed_range_negative" }
	{ "rangecheck",            "elevation_speed_range_positive" }
	{ "rangecheck",            "elevation_torsion_range"        }
	{ "or2",                   "elevation_position_range"       }
	{ "command_bool",          "elevation_recover"              }
	{ "and2",                  "elevation_position_safe"        }
	{ "log",                   "elevation_position_range_log"   }
	{ "log",                   "elevation_position_safe_log"    }

Jeroen Vreeken's avatar
Jeroen Vreeken committed
103
104
105
	{ "command_bool",          "focusbox_plus" }
	{ "command_bool",          "focusbox_min"  }

106
	{ "value_bool", 	   "false"	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
107
108
}

109
alias {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
110
111
112
	{ "Azimuth_Position",          "azimuth_position_offset_sum", "out"  }
	{ "Azimuth_Drive_Safety_p270", "dt_az", "be4" }
	{ "Azimuth_Drive_Safety_m270", "dt_az", "be2" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
	{ "Elevation_Position",        "elevation_input_matrix", "out0" }
114
	{ "Elevation_Torsion",         "elevation_input_matrix", "out1" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
115
116
	{ "Elevation_Top_Safe",        "dt_el_l", "be4" }
	{ "Elevation_Bottom_Safe",     "dt_el_l", "be2" }
117
118
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
119
120
121
122
links {
	{ "dt_az",                          "position",   "azimuth_position_gain",            "in"           , true }
	{ "azimuth_position_gain",          "out",        "azimuth_position_offset_sum",      "in0"          , true }
	{ "azimuth_position_offset",        "value",      "azimuth_position_offset_sum",      "in1"          , true }
123
	{ $<Azimuth_Position>,                            "azimuth_error",                    "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
124
	{ "azimuth_servo_state",            "reset",      "azimuth_spg",                      "reset"        , false }
125
	{ $<Azimuth_Position>,                            "azimuth_spg",                      "reset_x"      , true }
126
127
128
	{ "azimuth_spg",                    "x",          "azimuth_servo_state",              "spg_x"        , true }
	{ "azimuth_spg",                    "v",          "azimuth_servo_state",              "spg_v"        , true }
	{ "azimuth_spg",                    "a",          "azimuth_servo_state",              "spg_a"        , true }
129
	{ "azimuth_position_range",         "q",          "azimuth_safe_and",                 "a"            , true }
130
	{ "ethercat",                       "operational","azimuth_safe_and",                 "b"            , true }
131
132
	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safe_and",                 "c"            , true }
	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safe_and",                 "d"            , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
	{ "azimuth_safe_and",               "q",          "azimuth_servo_state",              "safe"         , true }
134
135
	{ "azimuth_recover",                "value",      "azimuth_servo_state",              "override"     , true }
	{ "azimuth_recover",                "value",      "azimuth_range_limit",              "enable",        true }
136
	{ "azimuth_servo_state",            "out_x",      "azimuth_error",                    "positive"     , true }
137
	{ $<Azimuth_Position>,                            "azimuth_setpoint_error",           "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
138
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
139
	{ "azimuth_servo_state",            "out_v",      "azimuth_speed_ff",                 "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
140
141
142
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
143
	{ "azimuth_servo_state",            "out_v",      "azimuth_pid_limit",                "limit"        , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
144
145
146
	{ "azimuth_pid_limit",              "out",        "azimuth_pid_filter",               "in"           , true }
	{ "azimuth_pid_filter",             "out",        "azimuth_speed_ff",                 "in1"          , true }
	{ "azimuth_speed_ff",               "out",        "azimuth_speed_limit",              "in"           , true }
147
148
	{ "azimuth_speed_limit",            "out",        "azimuth_range_limit",              "in"           , true }
        { "azimuth_range_limit",            "out",        "azimuth_speed_servo",              "in"           , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
149
	{ "azimuth_speed_servo",            "out",        "dt_az",                            "speed"        , true }
150
	{ "azimuth_torque",                 "value",      "dt_az",                            "torque"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
151
	{ "azimuth_servo_state",            "enable",     "dt_az",                            "enable"       , true }
152
153
	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
154
	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
155

156
157
	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safety_hw_pos", "condition",                true }
	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safety_hw_neg", "condition",                true }
158
159
	{ $<Azimuth_Position>,                            "azimuth_speed_range_negative", "in",                true }
	{ $<Azimuth_Position>,                            "azimuth_speed_range_positive", "in",                true }
160
161
162
	{ "azimuth_speed_range_positive", "valid",        "azimuth_position_range", "a",                       true }
	{ "azimuth_speed_range_negative", "valid",        "azimuth_position_range", "b",                       true }
	{ "azimuth_position_range", "q",                  "azimuth_position_range_log", "condition",           true }
163
164
165
166
167
	{ "azimuth_speed_range_positive", "valid",        "azimuth_position_safe", "a",                        true }
	{ "azimuth_speed_range_negative", "valid",        "azimuth_position_safe", "b",                        true }
	{ "azimuth_position_safe", "q",                   "azimuth_position_safe_log", "condition",            true }
	{ "azimuth_speed_range_positive", "valid",        "azimuth_range_limit", "enable_pos",                 true }
	{ "azimuth_speed_range_negative", "valid",        "azimuth_range_limit", "enable_neg",                 true }
168

Jeroen Vreeken's avatar
Jeroen Vreeken committed
169
	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
170
	{ $<Elevation_Position>,                          "elevation_spg",                    "reset_x"      , true }
171
172
173
	{ "elevation_spg",                  "x",          "elevation_servo_state",            "spg_x"        , true }
	{ "elevation_spg",                  "v",          "elevation_servo_state",            "spg_v"        , true }
	{ "elevation_spg",                  "a",          "elevation_servo_state",            "spg_a"        , true }
174
	{ "elevation_position_range",       "q",          "elevation_safe_and",               "a"            , true }
175
	{ "ethercat",                       "operational","elevation_safe_and",               "b"            , true }
176
177
178
	{ $<Elevation_Top_Safe>,                          "elevation_safe_and",               "c",             true }
	{ $<Elevation_Bottom_Safe>,                       "elevation_safe_and",               "d",             true }
	{ "elevation_torsion_range",        "valid",      "elevation_safe_and",               "e",             true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
179
	{ "elevation_safe_and",             "q",          "elevation_servo_state",            "safe"         , true }
180
181
182
	{ "elevation_recover",              "value",      "elevation_servo_state",            "override"     , true }
	{ "elevation_recover",              "value",      "elevation_range_limit_r",          "enable",        true }
	{ "elevation_recover",              "value",      "elevation_range_limit_l",          "enable",        true }
183
184
	{ "elevation_servo_state",          "out_x",      "elevation_error",                  "positive"     , true }
	{ "elevation_servo_state",          "out_v",      "elevation_speed_ff",               "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
185
186
	{ "dt_el_r",                        "position",   "elevation_position_offset_r_sum",  "in0"          , true }
	{ "dt_el_l",                        "position",   "elevation_position_offset_l_sum",  "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
187
188
189
190
	{ "elevation_position_offset_r",    "value",      "elevation_position_offset_r_sum",  "in1"          , true }
	{ "elevation_position_offset_l",    "value",      "elevation_position_offset_l_sum",  "in1"          , true }
	{ "elevation_position_offset_r_sum","out",        "elevation_input_matrix",           "in0"          , true }
	{ "elevation_position_offset_l_sum","out",        "elevation_input_matrix",           "in1"          , true }
191
	{ $<Elevation_Position>,                          "elevation_error",                  "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
192
	{ "elevation_spg",                  "setpoint",   "elevation_setpoint_error",         "positive"     , true }
193
	{ $<Elevation_Position>,                          "elevation_setpoint_error",         "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
194
195
196
197
198
199
200
201
202
	{ "elevation_error",                "difference", "elevation_pid",                    "in"           , true }
	{ "elevation_servo_state",          "enable",     "elevation_pid",                    "enable"       , true }
	{ "elevation_pid",                  "out",        "elevation_speed_ff",               "in1"          , true }
	{ "elevation_speed_ff",             "out",        "elevation_position_switch",        "in"           , true }
	{ "elevation_servo_state",          "enable",     "elevation_position_switch",        "on"           , true }
	{ "elevation_position_switch",      "out",        "elevation_speed_limit",            "in"           , true }
	{ "elevation_servo_state",          "enable",     "elevation_jerk_limit",             "enable"       , true }
	{ "elevation_speed_limit",          "out",        "elevation_jerk_limit",             "in"           , true }
	{ "elevation_jerk_limit",           "out",        "elevation_output_matrix",          "in0"          , true }
203
204
205
	{ "dt_el_r",                        "torque",     "elevation_torsion_torque",         "positive"     , true }
	{ "dt_el_l",                        "torque",     "elevation_torsion_torque",         "negative"     , true }
	{ "elevation_torsion_torque",       "difference", "elevation_torsion_torque_lp",      "in"           , true }
206
207
208
209
210
211
	{ "elevation_output_matrix",        "out0",       "elevation_range_limit_r",          "in"           , true }
	{ "elevation_output_matrix",        "out1",       "elevation_range_limit_l",          "in"           , true }
	{ "elevation_range_limit_r",        "out",        "dt_el_r",                          "speed"        , true } 
	{ "elevation_range_limit_l",        "out",        "dt_el_l",                          "speed"        , true }
	{ "elevation_torque_r",             "value",      "dt_el_r",                          "torque"       , true }
	{ "elevation_torque_l",             "value",      "dt_el_l",                          "torque"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
212
213
	{ "elevation_servo_state",          "enable",     "dt_el_r",                          "enable"       , true }
	{ "elevation_servo_state",          "enable",     "dt_el_l",                          "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
214
215
	{ "focusbox_plus",                  "value",      "dt_el_r",                          "ba1"          , true }
	{ "focusbox_min",		    "value",      "dt_el_r",                          "ba2"          , true }
216
217
	{ "false",                          "value",      "dt_el_l",                          "ba1"          , true }
	{ "false",			    "value",      "dt_el_l",                          "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
218
219
	
	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
220
	{ $<Elevation_Torsion>,                           "elevation_torsion_spg",            "reset_x"      , true }
221
	{ "elevation_torsion_spg",          "x",          "elevation_torsion_error",          "positive"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
222
223
	{ "elevation_input_matrix",         "out1",       "elevation_torsion_error",          "negative"     , true }
	{ "elevation_torsion_spg",          "setpoint",   "elevation_torsion_setpoint_error", "positive"     , true }
224
	{ $<Elevation_Torsion>,                           "elevation_torsion_setpoint_error", "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
225
	{ "elevation_torsion_error",        "difference", "elevation_torsion_pid",            "in"           , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
226
	{ "elevation_servo_state",          "enable",     "elevation_torsion_pid",            "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
227
228
	{ "elevation_torsion_pid",          "out",        "elevation_torsion_speed_limit",    "in"           , true }
	{ "elevation_torsion_speed_limit",  "out",        "elevation_output_matrix",          "in1"          , true }
229
230
231

	{ $<Elevation_Top_Safe>,                          "elevation_safety_hw_top", "condition",              true }
	{ $<Elevation_Bottom_Safe>,                       "elevation_safety_hw_bottom", "condition",           true }
232
233
234
235
236
237
238
239
240
241
242
243
244
245
	{ $<Elevation_Torsion>,                           "elevation_torsion_range", "in",                     true }
	{ $<Elevation_Position>,                          "elevation_speed_range_negative", "in",              true }
	{ $<Elevation_Position>,                          "elevation_speed_range_positive", "in",              true }
	{ "elevation_speed_range_positive", "valid",      "elevation_position_range", "a",                     true }
	{ "elevation_speed_range_negative", "valid",      "elevation_position_range", "b",                     true }
	{ "elevation_position_range", "q",                "elevation_position_range_log", "condition",         true }
	{ "elevation_speed_range_positive", "valid",      "elevation_position_safe", "a",                      true }
	{ "elevation_speed_range_negative", "valid",      "elevation_position_safe", "b",                      true }
	{ "elevation_position_safe", "q",                 "elevation_position_safe_log", "condition",          true }
	{ "elevation_speed_range_positive", "valid",      "elevation_range_limit_r", "enable_pos",             true }
	{ "elevation_speed_range_negative", "valid",      "elevation_range_limit_r", "enable_neg",             true }
	{ "elevation_speed_range_positive", "valid",      "elevation_range_limit_l", "enable_pos",             true }
	{ "elevation_speed_range_negative", "valid",      "elevation_range_limit_l", "enable_neg",             true }

Jeroen Vreeken's avatar
Jeroen Vreeken committed
246
247
248
}

traces {
249
250
251
252
253
254
255
256
257
258
	{ "Azimuth_Spg0",                     "rad",       "azimuth_spg",                      "x"          }
	{ "Azimuth_Spg1",                     "rad/s",     "azimuth_spg",                      "v"          }
	{ "Azimuth_Spg2",                     "rad/s/s",   "azimuth_spg",                      "a"          }
	{ "Azimuth_Spg3",                     "rad/s/s/s", "azimuth_spg",                      "j"          }
	{ "Azimuth_Setpoint",                 "rad",       "azimuth_spg",                      "setpoint"   }
	{ "Azimuth_Error",                    "rad",       "azimuth_error",                    "difference" }
	{ "Azimuth_Setpoint_Error",           "rad",       "azimuth_setpoint_error",           "difference" }
	{ "Azimuth_PID",                      "rad/s",     "azimuth_pid",                      "out"        }
	{ "Azimuth_I",                        "rad/s",     "azimuth_pid",                      "outi"       }
	{ "Azimuth_PID_filtered",             "rad/s",     "azimuth_pid_filter",               "out"        }
259
	{ "Azimuth_Position",                 "rad",       $<Azimuth_Position>                              }
260
261
	{ "Azimuth_Speed",                    "rad/s",     "dt_az",                            "speed"      }
	{ "Azimuth_Torque",                   "Nm",        "dt_az",                            "torque"     }
262
	{ "Azimuth_Safe",                     "Boolean",   "azimuth_position_range",           "q"          }
263
264
	{ "Azimuth_Enabled",                  "Boolean",   "dt_az",                            "enabled"    }
	{ "Azimuth_Drive_Safety",             "Boolean",   "dt_az",                            "be1"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
265
266
	{ "Azimuth_Drive_Safety_p270",        "Boolean",   $<Azimuth_Drive_Safety_p270>                     }
	{ "Azimuth_Drive_Safety_m270",        "Boolean",   $<Azimuth_Drive_Safety_m270>                     }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
267
		
268
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
269

270
	{ "Elevation_Position",               "rad",       $<Elevation_Position>                            }
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
	{ "Elevation_Torsion",                "rad",       "elevation_input_matrix",           "out1"       }
	{ "Elevation_Setpoint",               "rad",       "elevation_spg",                    "setpoint"   }
	{ "Elevation_Spg0",                   "rad",       "elevation_spg",                    "x"          }
	{ "Elevation_Spg1",                   "rad/s",     "elevation_spg",                    "v"          }
	{ "Elevation_Spg2",                   "rad/s",     "elevation_spg",                    "a"          }
	{ "Elevation_Spg3",                   "rad/s",     "elevation_spg",                    "j"          }
	{ "Elevation_Error",                  "rad",       "elevation_error",                  "difference" }
	{ "Elevation_Setpoint_Error",         "rad",       "elevation_setpoint_error",         "difference" }
	{ "Elevation_PID",                    "rad/s",     "elevation_pid",                    "out"        }
	{ "Elevation_Torsion_Error",          "rad",       "elevation_torsion_error",          "difference" }
	{ "Elevation_Torsion_Setpoint_Error", "rad",       "elevation_torsion_setpoint_error", "difference" }
	{ "Elevation_Torsion_PID",            "rad/s",     "elevation_torsion_pid",            "out"        }
	{ "Elevation_Position_Right",         "rad",       "elevation_position_offset_r_sum",  "out"        }
	{ "Elevation_Position_Left",          "rad",       "elevation_position_offset_l_sum",  "out"        }
	{ "Elevation_Speed_Right",            "rad/s",     "dt_el_r",                          "speed"      }
	{ "Elevation_Speed_Left",             "rad/s",     "dt_el_l",                          "speed"      }
	{ "Elevation_Torque_Right",           "Nm",        "dt_el_r",                          "torque"     }
	{ "Elevation_Torque_Left",            "Nm",        "dt_el_l",                          "torque"     }
	{ "Elevation_Torsion_Torque",         "Nm",        "elevation_torsion_torque_lp",      "out"        }
290
	{ "Elevation_Safe",                   "Boolean",   "elevation_position_range",         "q"          }
291
292
293
	{ "Elevation_Enabled",                "Boolean",   "dt_el_l",                          "enabled"    }
	{ "Elevation_Drive_Safety_Right",     "Boolean",   "dt_el_r",                          "be1"        }
	{ "Elevation_Drive_Safety_Left",      "Boolean",   "dt_el_l",                          "be1"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
294
295
	{ "Elevation_Top_Safe",               "Boolean",   $<Elevation_Top_Safe>                            }
	{ "Elevation_Bottom_Safe",            "Boolean",   $<Elevation_Bottom_Safe>                         }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
296
297
}

298
299
300
set azimuth_gear	15006.75
set elevation_gear	44803.125

Jeroen Vreeken's avatar
Jeroen Vreeken committed
301
params {
302
	{ "azimuth_spg",           "setpoint",            (float) 0.0		 }
303
	# maximum speed and position clients input is checked against
304
305
	{ "azimuth_spg",           "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_spg",           "min_x",               (float) deg2rad(-270.0)  }
306
	{ "azimuth_spg",           "max_v",               (float) rpm2rads(3000.0)/$(azimuth_gear) }
307
	# acceleration and jerk we use to generate a profile
308
309
	{ "azimuth_spg",           "max_a",               (float) 0.0002    	 }
	{ "azimuth_spg",           "max_j",               (float) 0.00001        }
310
	# at which precision do we consider the values to be 'equal'
311
312
313
	{ "azimuth_spg",           "precision_x",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_a",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_v",         (float) 0.000001       }
314
	# maximum values within we allow normal operation
315
316
	{ "azimuth_servo_state",   "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_servo_state",   "min_x",               (float) deg2rad(-270.0)  }
317
	{ "azimuth_servo_state",   "max_v",               (float) rpm2rads(3000.0)/$(azimuth_gear) }
318
	{ "azimuth_servo_state",   "max_a",               (float) 0.0002         }
319
	# controller factors
320
321
322
	{ "azimuth_pid",           "kp",                  (float) 0.20           }
	{ "azimuth_pid",           "ki",                  (float) 0.00           }
	{ "azimuth_pid",           "kd",                  (float) 0.0            }
323
	# the amount of 'wind-up' we allow in the integrator
324
325
	{ "azimuth_pid",           "maxw",                (float) rpm2rads(0.005)  }
	{ "azimuth_pid",           "minw",                (float) rpm2rads(-0.005) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
326

327
	# second order filter to make sure we do not trigger the eigenfrequency of the DT
Jeroen Vreeken's avatar
Jeroen Vreeken committed
328
329
330
	{ "azimuth_pid_filter",    "transfer",            (int) 2,
							  (double) 6.343831259e+05,
						          (float) { -0.9964520027, 1.9964456974 },
331
							  (float) { 1.0, 2.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
332
333
										}

334
335
336
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

337
338
339
340
	# Due to high vibrations observed at high speed it was decided to limit
	# the pid controller. At low speed the value is untouched, but the 
	# output is limited when speed increases 
	# limit = offset + gain * speed
341
	{ "azimuth_pid_limit",     "offset",              (float) rpm2rads(100.0)/$(azimuth_gear) }
342
	{ "azimuth_pid_limit",     "gain",                (float)-0.3               }
343
	# speed limit on spg + pid just before we send it to the drive
344
345
	{ "azimuth_speed_limit",   "min",                 (float) rpm2rads(-3000.0)/$(azimuth_gear) }
	{ "azimuth_speed_limit",   "max",                 (float) rpm2rads(3000.0)/$(azimuth_gear)  }
346
	# gear ratio between DT and servodrive
347
	{ "azimuth_speed_servo",   "gain",                (float)-$(azimuth_gear)          }
348
	# torque we allow the servodrive to actuate on the DT
349
	{ "azimuth_torque",        "value",               (float) 10.0              }
350
	# inverse gear ratio between DT and servodrive
351
	{ "azimuth_position_gain", "gain",                (float)-1.0/$(azimuth_gear) }
352
	# callibration value for position
353
	{ "azimuth_position_offset","value",              (float) 0.0               }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
354

355
356
	{ "azimuth_safety_hw_pos", "msg_up",   2, "Azimuth HW positive safety switch is closed" }
	{ "azimuth_safety_hw_pos", "msg_down", 0, "Azimuth HW positive safety switch is open"   }
357
	{ "azimuth_safety_hw_pos", "init",     true }
358
359
	{ "azimuth_safety_hw_neg", "msg_up",   2, "Azimuth HW negative safety switch is closed" }
	{ "azimuth_safety_hw_neg", "msg_down", 0, "Azimuth HW negative safety switch is open"   }
360
361
	{ "azimuth_safety_hw_neg", "init",     true }

362
363
364
365
	# positions were we go into safe behavior
	# these must be outside the normal operating range
	# 'safe zone' is between the operating range and the absolute safety
	# value above
366
367
	{ "azimuth_speed_range_negative", "max", deg2rad(290.0)  }
	{ "azimuth_speed_range_negative", "min", deg2rad(-280.0) }
368
369
	{ "azimuth_speed_range_positive", "max", deg2rad(280.0) }
	{ "azimuth_speed_range_positive", "min", deg2rad(-290.0)  }
370

371
372
373
	{ "azimuth_position_range_log", "msg_up",   1, "Azimuth position inside allowed range"  }
	{ "azimuth_position_range_log", "msg_down", 0, "Azimuth position outside allowed range" }
	{ "azimuth_position_range_log", "init",     true }
374
375
376
377

	{ "azimuth_position_safe_log", "msg_up",   1, "Azimuth position left safe zone to normal operating range"  }
	{ "azimuth_position_safe_log", "msg_down", 1, "Azimuth position left normal operating range and is in safe zone" }
	{ "azimuth_position_safe_log", "init",     true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
378
	
379
        { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
380
                                        	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
381

382
383
	{ "elevation_output_matrix", "constants",         (float) { $(elevation_gear),  $(elevation_gear)/2.0 }, 
							  (float) { -$(elevation_gear), $(elevation_gear)/2.0 } }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
384
385

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
386
	{ "elevation_spg",         "max_x",               (float) deg2rad(90.0)   }
387
	{ "elevation_spg",         "min_x",               (float) deg2rad(-0.05)  }
388
	{ "elevation_spg",         "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
389
390
391
392
393
	{ "elevation_spg",         "max_a",               (float) 0.0003   	}
	{ "elevation_spg",         "max_j",               (float) 0.000016	}
	{ "elevation_spg",         "precision_x",         (float) 0.000001      }
	{ "elevation_spg",         "precision_a",         (float) 0.000001      }
	{ "elevation_spg",         "precision_v",         (float) 0.000001      }
394
	{ "elevation_servo_state", "max_x",               (float) deg2rad(90.0)   }
395
	{ "elevation_servo_state", "min_x",               (float) deg2rad(-0.05)  }
396
	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
397
	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
398
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
399
400
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
401
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
402
403
404
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
405
406
407
408
	{ "elevation_pid",         "maxw",                (float) rpm2rads(0.002)  }
	{ "elevation_pid",         "minw",                (float) rpm2rads(-0.002) }
	{ "elevation_speed_limit", "min",                 (float) rpm2rads(-0.08)  }
	{ "elevation_speed_limit", "max",                 (float) rpm2rads(0.08)   }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
409
410
411
412
413
	{ "elevation_jerk_limit",  "min",                 (float)-0.003         }
	{ "elevation_jerk_limit",  "max",                 (float) 0.003         }
	{ "elevation_torsion_pid", "kp",                  (float) 0.2  		}
	{ "elevation_torsion_pid", "ki",                  (float) 0.0  		}
	{ "elevation_torsion_pid", "kd",                  (float) 0.0 		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
414
415
	{ "elevation_torsion_pid", "maxw",                (float) 0.0001 }
	{ "elevation_torsion_pid", "minw",                (float)-0.0001 }
416
417
	{ "elevation_torsion_speed_limit", "min",         (float) rpm2rads(-0.001) }
	{ "elevation_torsion_speed_limit", "max",         (float) rpm2rads(0.001)  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
418
419


420
421
	{ "elevation_torsion_range", "max", (float) 0.0002 }
	{ "elevation_torsion_range", "min", (float)-0.0002 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
422
423
424
425
426
427
428

	{ "elevation_torque_r",    "value",               (float) 10.0          }
	{ "elevation_torque_l",    "value",               (float) 10.0          }
	{ "elevation_torsion_torque_lp", "tau",           (float) 10.0          }
	{ "elevation_position_offset_r","value",          (float) 0.0           }
	{ "elevation_position_offset_l","value",          (float) 0.0           }

429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
	# positions were we go into safe behavior
	# these must be outside the normal operating range
	# 'safe zone' is between the operating range and the absolute safety
	# value above
	{ "elevation_speed_range_negative", "max", deg2rad(90.5) * $(elevation_gear)  }
	{ "elevation_speed_range_negative", "min", deg2rad(-0.09) * $(elevation_gear) }
	{ "elevation_speed_range_positive", "max", deg2rad(90.4) * $(elevation_gear)  }
	{ "elevation_speed_range_positive", "min", deg2rad(-0.1) * $(elevation_gear)  }

	{ "elevation_position_range_log", "msg_up",   1, "Elevation position inside allowed range"  }
	{ "elevation_position_range_log", "msg_down", 0, "Elevation position outside allowed range" }
	{ "elevation_position_range_log", "init",     true }

	{ "elevation_position_safe_log", "msg_up",   1, "Elevation position left safe zone to normal operating range"  }
	{ "elevation_position_safe_log", "msg_down", 1, "Elevation position left normal operating range and is in safe zone" }
	{ "elevation_position_safe_log", "init",     true }
445
446
447

	{ "elevation_safety_hw_top",    "msg_up",   2, "Elevation HW top safety switch is closed"    }
	{ "elevation_safety_hw_top",    "msg_down", 0, "Elevation HW top safety switch is open"      }
448
	{ "elevation_safety_hw_top",    "init",     true }
449
450
	{ "elevation_safety_hw_bottom", "msg_up",   2, "Elevation HW bottom safety switch is closed" }
	{ "elevation_safety_hw_bottom", "msg_down", 0, "Elevation HW bottom safety switch is open"   }
451
	{ "elevation_safety_hw_bottom", "init",     true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
452
}
453
454
455
456

# Load file with calibration parameters.
# All parameters above should not be subject to calibration.
include "dt_ctrl.param"