dt_ctrl.ctrl 32.4 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"  }

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

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

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

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

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

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

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

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

303
304
305
set azimuth_gear	15006.75
set elevation_gear	44803.125

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

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

339
340
341
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

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

360
361
	{ "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"   }
362
	{ "azimuth_safety_hw_pos", "init",     true }
363
364
	{ "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"   }
365
366
	{ "azimuth_safety_hw_neg", "init",     true }

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

376
377
378
	{ "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 }
379
380
381
382

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

387
388
	{ "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
389
390

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


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

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

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

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

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