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
trigger {
15
16
#	{ "busyloop" }
	{ "prestart", 0.000250 }
17
18
}

19
20
21
22
# 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
23
# dt_ctrl_ec_sim.ctrl   Ethercat simulation block
24
#
25
26
27
# Uncomment either the real network, or the sim network.
# But never both!

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

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

34
#import "dt_ctrl_ec_sim.ctrl"
35

36
37


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

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

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

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

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

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

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

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

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

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

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

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

307
308
309
set azimuth_gear	15006.75
set elevation_gear	44803.125

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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