dt_ctrl.ctrl 32.6 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

35
36


Jeroen Vreeken's avatar
Jeroen Vreeken committed
37
blocks ($(frequency), $(delay)) {
38
39
40
	# Profiling block, may be commented out.
	{ "controller_profile", "profile" }

41
42
	{ "setpoint_generator_3d", "azimuth_spg", 	"Azimuth_Setpoint", "rad" }
	{ "servo_state",           "azimuth_servo_state"           }
43
	{ "and4",                  "azimuth_safe_and"              }
44
45
46
47
48
49
50
	{ "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"	   }
51
	{ "limit_switch",          "azimuth_range_limit"           }
52
53
54
55
56
57
	{ "gain", 	           "azimuth_speed_servo"	   }
	{ "value",	           "azimuth_torque"		   }
	{ "value",	           "azimuth_position_offset"  	   }
	{ "add",  	           "azimuth_position_offset_sum"   }
	{ "gain", 	           "azimuth_position_gain"	   }
	{ "rangecheck",		   "azimuth_speed_warning"         }
58
59
60

	{ "log",                   "azimuth_safety_hw_pos" }
	{ "log",                   "azimuth_safety_hw_neg" }
61
62
63
	{ "rangecheck",            "azimuth_speed_range_negative" }
	{ "rangecheck",            "azimuth_speed_range_positive" }
	{ "or2",                   "azimuth_position_range"       }
64
65
	{ "command_bool",          "azimuth_recover"              }
	{ "and2",                  "azimuth_position_safe"        }
66
	{ "log",                   "azimuth_position_range_log"   }
67
	{ "log",                   "azimuth_position_safe_log"    }
68
		
69
70
71
	{ "matrix_2x2",	           "elevation_input_matrix"	      }
	{ "setpoint_generator_3d", "elevation_spg", 		"Elevation_Setpoint", "rad" }
	{ "servo_state",           "elevation_servo_state"            }
72
 	{ "and5",                  "elevation_safe_and"               }
73
74
75
76
77
78
79
80
81
82
83
84
85
	{ "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"	      }
86
87
	{ "limit_switch",          "elevation_range_limit_r"          }
	{ "limit_switch",          "elevation_range_limit_l"          }
88
89
90
91
92
93
94
95
	{ "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"  }
96

97
98
	{ "log",                   "elevation_safety_hw_top" }
	{ "log",                   "elevation_safety_hw_bottom" }
99
100
101
102
103
104
105
106
107
	{ "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
108
109
110
	{ "command_bool",          "focusbox_plus" }
	{ "command_bool",          "focusbox_min"  }

Jeroen Vreeken's avatar
Jeroen Vreeken committed
111
112
	{ "value_bool", 	   "false" }
	{ "value_float",           "zero"  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
}

115
alias {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
116
117
118
	{ "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
119
	{ "Elevation_Position",        "elevation_input_matrix", "out0" }
120
	{ "Elevation_Torsion",         "elevation_input_matrix", "out1" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
121
122
	{ "Elevation_Top_Safe",        "dt_el_l", "be4" }
	{ "Elevation_Bottom_Safe",     "dt_el_l", "be2" }
123
124
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
125
126
127
128
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 }
129
	{ $<Azimuth_Position>,                            "azimuth_error",                    "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
	{ "azimuth_servo_state",            "reset",      "azimuth_spg",                      "reset"        , false }
131
	{ $<Azimuth_Position>,                            "azimuth_spg",                      "reset_x"      , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132
	{ "zero",                           "value",      "azimuth_spg",                      "track_x"      , true }
133
134
135
	{ "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 }
136
	{ "azimuth_position_range",         "q",          "azimuth_safe_and",                 "a"            , true }
137
	{ "ethercat",                       "operational","azimuth_safe_and",                 "b"            , true }
138
139
	{ $<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
140
	{ "azimuth_safe_and",               "q",          "azimuth_servo_state",              "safe"         , true }
141
142
	{ "azimuth_recover",                "value",      "azimuth_servo_state",              "override"     , true }
	{ "azimuth_recover",                "value",      "azimuth_range_limit",              "enable",        true }
143
	{ "azimuth_servo_state",            "out_x",      "azimuth_error",                    "positive"     , true }
144
	{ $<Azimuth_Position>,                            "azimuth_setpoint_error",           "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
145
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
146
	{ "azimuth_servo_state",            "out_v",      "azimuth_speed_ff",                 "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
147
148
149
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
150
	{ "azimuth_servo_state",            "out_v",      "azimuth_pid_limit",                "limit"        , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
151
152
153
	{ "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 }
154
155
	{ "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
156
	{ "azimuth_speed_servo",            "out",        "dt_az",                            "speed"        , true }
157
	{ "azimuth_torque",                 "value",      "dt_az",                            "torque"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
	{ "azimuth_servo_state",            "enable",     "dt_az",                            "enable"       , true }
159
160
	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
161
	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
162

163
164
	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safety_hw_pos", "condition",                true }
	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safety_hw_neg", "condition",                true }
165
166
	{ $<Azimuth_Position>,                            "azimuth_speed_range_negative", "in",                true }
	{ $<Azimuth_Position>,                            "azimuth_speed_range_positive", "in",                true }
167
168
169
	{ "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 }
170
171
172
173
174
	{ "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 }
175

Jeroen Vreeken's avatar
Jeroen Vreeken committed
176
	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
177
	{ $<Elevation_Position>,                          "elevation_spg",                    "reset_x"      , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
178
	{ "zero",                           "value",      "elevation_spg",                    "track_x"      , true }
179
180
181
	{ "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 }
182
	{ "elevation_position_range",       "q",          "elevation_safe_and",               "a"            , true }
183
	{ "ethercat",                       "operational","elevation_safe_and",               "b"            , true }
184
185
186
	{ $<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
187
	{ "elevation_safe_and",             "q",          "elevation_servo_state",            "safe"         , true }
188
189
190
	{ "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 }
191
192
	{ "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
193
194
	{ "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
195
196
197
198
	{ "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 }
199
	{ $<Elevation_Position>,                          "elevation_error",                  "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
200
	{ "elevation_spg",                  "setpoint",   "elevation_setpoint_error",         "positive"     , true }
201
	{ $<Elevation_Position>,                          "elevation_setpoint_error",         "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
202
203
204
205
206
207
208
209
210
	{ "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 }
211
212
213
	{ "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 }
214
215
216
217
218
219
	{ "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
220
221
	{ "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
222
223
	{ "focusbox_plus",                  "value",      "dt_el_r",                          "ba1"          , true }
	{ "focusbox_min",		    "value",      "dt_el_r",                          "ba2"          , true }
224
225
	{ "false",                          "value",      "dt_el_l",                          "ba1"          , true }
	{ "false",			    "value",      "dt_el_l",                          "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
226
227
	
	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
228
	{ $<Elevation_Torsion>,                           "elevation_torsion_spg",            "reset_x"      , true }
229
	{ "elevation_torsion_spg",          "x",          "elevation_torsion_error",          "positive"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
230
231
	{ "elevation_input_matrix",         "out1",       "elevation_torsion_error",          "negative"     , true }
	{ "elevation_torsion_spg",          "setpoint",   "elevation_torsion_setpoint_error", "positive"     , true }
232
	{ $<Elevation_Torsion>,                           "elevation_torsion_setpoint_error", "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
233
	{ "elevation_torsion_error",        "difference", "elevation_torsion_pid",            "in"           , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
234
	{ "elevation_servo_state",          "enable",     "elevation_torsion_pid",            "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
235
236
	{ "elevation_torsion_pid",          "out",        "elevation_torsion_speed_limit",    "in"           , true }
	{ "elevation_torsion_speed_limit",  "out",        "elevation_output_matrix",          "in1"          , true }
237
238
239

	{ $<Elevation_Top_Safe>,                          "elevation_safety_hw_top", "condition",              true }
	{ $<Elevation_Bottom_Safe>,                       "elevation_safety_hw_bottom", "condition",           true }
240
241
242
243
244
245
246
247
248
249
250
251
252
253
	{ $<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
254
255
256
}

traces {
257
258
259
260
261
262
263
264
265
266
	{ "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"        }
267
	{ "Azimuth_Position",                 "rad",       $<Azimuth_Position>                              }
268
269
	{ "Azimuth_Speed",                    "rad/s",     "dt_az",                            "speed"      }
	{ "Azimuth_Torque",                   "Nm",        "dt_az",                            "torque"     }
270
	{ "Azimuth_Safe",                     "Boolean",   "azimuth_position_range",           "q"          }
271
272
	{ "Azimuth_Enabled",                  "Boolean",   "dt_az",                            "enabled"    }
	{ "Azimuth_Drive_Safety",             "Boolean",   "dt_az",                            "be1"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
273
274
	{ "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
275
		
276
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
277

278
	{ "Elevation_Position",               "rad",       $<Elevation_Position>                            }
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
	{ "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"        }
298
	{ "Elevation_Safe",                   "Boolean",   "elevation_position_range",         "q"          }
299
300
301
	{ "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
302
303
	{ "Elevation_Top_Safe",               "Boolean",   $<Elevation_Top_Safe>                            }
	{ "Elevation_Bottom_Safe",            "Boolean",   $<Elevation_Bottom_Safe>                         }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
304
305
}

306
307
308
set azimuth_gear	15006.75
set elevation_gear	44803.125

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

335
	# second order filter to make sure we do not trigger the eigenfrequency of the DT
Jeroen Vreeken's avatar
Jeroen Vreeken committed
336
337
338
	{ "azimuth_pid_filter",    "transfer",            (int) 2,
							  (double) 6.343831259e+05,
						          (float) { -0.9964520027, 1.9964456974 },
339
							  (float) { 1.0, 2.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
340
341
										}

342
343
344
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

345
346
347
348
	# 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
349
	{ "azimuth_pid_limit",     "offset",              (float) rpm2rads(100.0)/$(azimuth_gear) }
350
	{ "azimuth_pid_limit",     "gain",                (float)-0.3               }
351
	# speed limit on spg + pid just before we send it to the drive
352
353
	{ "azimuth_speed_limit",   "min",                 (float) rpm2rads(-3000.0)/$(azimuth_gear) }
	{ "azimuth_speed_limit",   "max",                 (float) rpm2rads(3000.0)/$(azimuth_gear)  }
354
	# gear ratio between DT and servodrive
355
	{ "azimuth_speed_servo",   "gain",                (float)-$(azimuth_gear)          }
356
	# torque we allow the servodrive to actuate on the DT
357
	{ "azimuth_torque",        "value",               (float) 10.0              }
358
	# inverse gear ratio between DT and servodrive
359
	{ "azimuth_position_gain", "gain",                (float)-1.0/$(azimuth_gear) }
360
	# callibration value for position
361
	{ "azimuth_position_offset","value",              (float) 0.0               }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
362

363
364
	{ "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"   }
365
	{ "azimuth_safety_hw_pos", "init",     true }
366
367
	{ "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"   }
368
369
	{ "azimuth_safety_hw_neg", "init",     true }

370
371
372
373
	# 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
374
375
	{ "azimuth_speed_range_negative", "max", deg2rad(290.0)  }
	{ "azimuth_speed_range_negative", "min", deg2rad(-280.0) }
376
377
	{ "azimuth_speed_range_positive", "max", deg2rad(280.0) }
	{ "azimuth_speed_range_positive", "min", deg2rad(-290.0)  }
378

379
380
381
	{ "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 }
382
383
384
385

	{ "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
386
	
387
        { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
388
                                        	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
389

390
391
	{ "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
392
393

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
394
	{ "elevation_spg",         "max_x",               (float) deg2rad(90.0)   }
395
	{ "elevation_spg",         "min_x",               (float) deg2rad(-0.055)  }
396
	{ "elevation_spg",         "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
397
398
399
400
401
	{ "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      }
402
	{ "elevation_servo_state", "max_x",               (float) deg2rad(90.0)   }
403
	{ "elevation_servo_state", "min_x",               (float) deg2rad(-0.05)  }
404
	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
405
	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
406
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
407
408
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
409
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
410
411
412
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
413
414
415
416
	{ "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
417
418
	{ "elevation_jerk_limit",  "min",                 (float)-0.003         }
	{ "elevation_jerk_limit",  "max",                 (float) 0.003         }
419
	{ "elevation_torsion_pid", "kp",                  (float) 20.0 		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
420
421
	{ "elevation_torsion_pid", "ki",                  (float) 0.0  		}
	{ "elevation_torsion_pid", "kd",                  (float) 0.0 		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
422
423
	{ "elevation_torsion_pid", "maxw",                (float) 0.0001 }
	{ "elevation_torsion_pid", "minw",                (float)-0.0001 }
424
425
	{ "elevation_torsion_speed_limit", "min",         (float) rpm2rads(-0.01) }
	{ "elevation_torsion_speed_limit", "max",         (float) rpm2rads(0.01)  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
426
427


428
429
	{ "elevation_torsion_range", "max", (float) 0.0002 }
	{ "elevation_torsion_range", "min", (float)-0.0002 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
430
431
432
433
434
435
436

	{ "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           }

437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
	# 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 }
453
454
455

	{ "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"      }
456
	{ "elevation_safety_hw_top",    "init",     true }
457
458
	{ "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"   }
459
	{ "elevation_safety_hw_bottom", "init",     true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
460
}
461
462
463
464

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