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

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

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

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

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

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

33
import "dt_ctrl_ec_sim.ctrl"
34

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

	{ "log",                   "azimuth_safety_hw_pos" }
	{ "log",                   "azimuth_safety_hw_neg" }
56
57
58
	{ "rangecheck",            "azimuth_speed_range_negative" }
	{ "rangecheck",            "azimuth_speed_range_positive" }
	{ "or2",                   "azimuth_position_range"       }
59
60
	{ "command_bool",          "azimuth_recover"              }
	{ "and2",                  "azimuth_position_safe"        }
61
	{ "log",                   "azimuth_position_range_log"   }
62
	{ "log",                   "azimuth_position_safe_log"    }
63
		
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
	{ "matrix_2x2",	           "elevation_input_matrix"	      }
	{ "setpoint_generator_3d", "elevation_spg", 		"Elevation_Setpoint", "rad" }
	{ "servo_state",           "elevation_servo_state"            }
 	{ "and2",                  "elevation_safe_and"               }
	{ "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"	      }
	{ "limit_2nd",	           "elevation_jerk_limit"	      }
	{ "dt_el_safety",          "elevation_safety" 		      }
	{ "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"  }
90

91
92
93
	{ "log",                   "elevation_safety_hw_top" }
	{ "log",                   "elevation_safety_hw_bottom" }
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
94
95
96
	{ "command_bool",          "focusbox_plus" }
	{ "command_bool",          "focusbox_min"  }

97
	{ "value_bool", 	   "false"	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
98
99
}

100
alias {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
101
102
103
	{ "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
104
105
106
	{ "Elevation_Position",        "elevation_input_matrix", "out0" }
	{ "Elevation_Top_Safe",        "dt_el_l", "be4" }
	{ "Elevation_Bottom_Safe",     "dt_el_l", "be2" }
107
108
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
109
110
111
112
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 }
113
	{ $<Azimuth_Position>,                            "azimuth_error",                    "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
114
	{ "azimuth_servo_state",            "reset",      "azimuth_spg",                      "reset"        , false }
115
	{ $<Azimuth_Position>,                            "azimuth_spg",                      "reset_x"      , true }
116
117
118
	{ "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 }
119
	{ "azimuth_position_range",         "q",          "azimuth_safe_and",                 "a"            , true }
120
	{ "ethercat",                       "operational","azimuth_safe_and",                 "b"            , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
121
	{ "azimuth_safe_and",               "q",          "azimuth_servo_state",              "safe"         , true }
122
123
	{ "azimuth_recover",                "value",      "azimuth_servo_state",              "override"     , true }
	{ "azimuth_recover",                "value",      "azimuth_range_limit",              "enable",        true }
124
	{ "azimuth_servo_state",            "out_x",      "azimuth_error",                    "positive"     , true }
125
	{ $<Azimuth_Position>,                            "azimuth_setpoint_error",           "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
126
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
127
	{ "azimuth_servo_state",            "out_v",      "azimuth_speed_ff",                 "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
128
129
130
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
131
	{ "azimuth_servo_state",            "out_v",      "azimuth_pid_limit",                "limit"        , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132
133
134
	{ "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 }
135
136
	{ "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
137
	{ "azimuth_speed_servo",            "out",        "dt_az",                            "speed"        , true }
138
	{ "azimuth_torque",                 "value",      "dt_az",                            "torque"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
139
	{ "azimuth_servo_state",            "enable",     "dt_az",                            "enable"       , true }
140
141
	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
142
	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
143

144
145
	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safety_hw_pos", "condition",                true }
	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safety_hw_neg", "condition",                true }
146
147
	{ $<Azimuth_Position>,                            "azimuth_speed_range_negative", "in",                true }
	{ $<Azimuth_Position>,                            "azimuth_speed_range_positive", "in",                true }
148
149
150
	{ "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 }
151
152
153
154
155
	{ "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 }
156

Jeroen Vreeken's avatar
Jeroen Vreeken committed
157
	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
158
	{ $<Elevation_Position>,                          "elevation_spg",                    "reset_x"      , true }
159
160
161
	{ "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 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
162
	{ "elevation_safety",               "safe_out",   "elevation_safe_and",               "a"            , false }
163
	{ "ethercat",                       "operational","elevation_safe_and",               "b"            , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
164
	{ "elevation_safe_and",             "q",          "elevation_servo_state",            "safe"         , true }
165
	{ "elevation_safety",               "recover",    "elevation_servo_state",            "override"     , false }
166
167
	{ "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
168
169
	{ "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
170
171
172
173
	{ "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 }
174
	{ $<Elevation_Position>,                          "elevation_error",                  "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
175
	{ "elevation_spg",                  "setpoint",   "elevation_setpoint_error",         "positive"     , true }
176
	{ $<Elevation_Position>,                          "elevation_setpoint_error",         "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
177
178
179
180
181
182
183
184
185
	{ "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 }
186
187
188
	{ "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 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
189
190
	{ "elevation_position_offset_r_sum","out",        "elevation_safety",                 "position_in_r" , true }
	{ "elevation_position_offset_l_sum","out",        "elevation_safety",                 "position_in_l" , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
191
	{ "elevation_input_matrix",         "out1",       "elevation_safety",                 "torsion_in"   , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
192
193
	{ "elevation_output_matrix",        "out0",       "elevation_safety",                 "speed_in_r"    , true }
	{ "elevation_output_matrix",        "out1",       "elevation_safety",                 "speed_in_l"    , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
194
	{ "elevation_torsion_torque_lp",    "out",        "elevation_safety",                 "torque_in"    , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
195
196
	{ "elevation_torque_r",             "value",      "elevation_safety",                 "torque_in_r"   , true }
	{ "elevation_torque_l",             "value",      "elevation_safety",                 "torque_in_l"   , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
197
198
	{ $<Elevation_Top_Safe>,                          "elevation_safety",                 "safety_in_top",    true }
	{ $<Elevation_Bottom_Safe>,                       "elevation_safety",                 "safety_in_bottom", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
199
	{ "elevation_servo_state",          "enable",     "elevation_safety",                 "enable"       , true }
200
201
202
203
	{ "elevation_safety",               "speed_out_r", "dt_el_r",                         "speed"        , true } 
	{ "elevation_safety",               "speed_out_l", "dt_el_l",                         "speed"        , true }
	{ "elevation_safety",               "torque_out_r","dt_el_r",                         "torque"       , true }
	{ "elevation_safety",               "torque_out_l","dt_el_l",                         "torque"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
204
205
	{ "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
206
207
	{ "focusbox_plus",                  "value",      "dt_el_r",                          "ba1"          , true }
	{ "focusbox_min",		    "value",      "dt_el_r",                          "ba2"          , true }
208
209
	{ "false",                          "value",      "dt_el_l",                          "ba1"          , true }
	{ "false",			    "value",      "dt_el_l",                          "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
210
211
	
	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
212
213
	{ "elevation_input_matrix",         "out1",       "elevation_torsion_spg",            "reset_x"      , true }
	{ "elevation_torsion_spg",          "x",          "elevation_torsion_error",          "positive"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
214
215
216
217
	{ "elevation_input_matrix",         "out1",       "elevation_torsion_error",          "negative"     , true }
	{ "elevation_torsion_spg",          "setpoint",   "elevation_torsion_setpoint_error", "positive"     , true }
	{ "elevation_input_matrix",         "out1",       "elevation_torsion_setpoint_error", "negative"     , true }
	{ "elevation_torsion_error",        "difference", "elevation_torsion_pid",            "in"           , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
218
	{ "elevation_servo_state",          "enable",     "elevation_torsion_pid",            "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
219
220
	{ "elevation_torsion_pid",          "out",        "elevation_torsion_speed_limit",    "in"           , true }
	{ "elevation_torsion_speed_limit",  "out",        "elevation_output_matrix",          "in1"          , true }
221
222
223

	{ $<Elevation_Top_Safe>,                          "elevation_safety_hw_top", "condition",              true }
	{ $<Elevation_Bottom_Safe>,                       "elevation_safety_hw_bottom", "condition",           true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
224
225
226
}

traces {
227
228
229
230
231
232
233
234
235
236
	{ "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"        }
237
	{ "Azimuth_Position",                 "rad",       $<Azimuth_Position>                              }
238
239
	{ "Azimuth_Speed",                    "rad/s",     "dt_az",                            "speed"      }
	{ "Azimuth_Torque",                   "Nm",        "dt_az",                            "torque"     }
240
	{ "Azimuth_Safe",                     "Boolean",   "azimuth_position_range",           "q"          }
241
242
	{ "Azimuth_Enabled",                  "Boolean",   "dt_az",                            "enabled"    }
	{ "Azimuth_Drive_Safety",             "Boolean",   "dt_az",                            "be1"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
243
244
	{ "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
245
		
246
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
247

248
	{ "Elevation_Position",               "rad",       $<Elevation_Position>                            }
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
	{ "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"        }
	{ "Elevation_Safe",                   "Boolean",   "elevation_safety",                 "safe_out"   }
	{ "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
272
273
	{ "Elevation_Top_Safe",               "Boolean",   $<Elevation_Top_Safe>                            }
	{ "Elevation_Bottom_Safe",            "Boolean",   $<Elevation_Bottom_Safe>                         }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
274
275
}

276
277
278
set azimuth_gear	15006.75
set elevation_gear	44803.125

Jeroen Vreeken's avatar
Jeroen Vreeken committed
279
params {
280
	{ "azimuth_spg",           "setpoint",            (float) 0.0		 }
281
	# maximum speed and position clients input is checked against
282
283
	{ "azimuth_spg",           "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_spg",           "min_x",               (float) deg2rad(-270.0)  }
284
	{ "azimuth_spg",           "max_v",               (float) rpm2rads(3000.0)/$(azimuth_gear) }
285
	# acceleration and jerk we use to generate a profile
286
287
	{ "azimuth_spg",           "max_a",               (float) 0.0002    	 }
	{ "azimuth_spg",           "max_j",               (float) 0.00001        }
288
	# at which precision do we consider the values to be 'equal'
289
290
291
	{ "azimuth_spg",           "precision_x",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_a",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_v",         (float) 0.000001       }
292
	# maximum values within we allow normal operation
293
294
	{ "azimuth_servo_state",   "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_servo_state",   "min_x",               (float) deg2rad(-270.0)  }
295
	{ "azimuth_servo_state",   "max_v",               (float) rpm2rads(3000.0)/$(azimuth_gear) }
296
	{ "azimuth_servo_state",   "max_a",               (float) 0.0002         }
297
	# controller factors
298
299
300
	{ "azimuth_pid",           "kp",                  (float) 0.20           }
	{ "azimuth_pid",           "ki",                  (float) 0.00           }
	{ "azimuth_pid",           "kd",                  (float) 0.0            }
301
	# the amount of 'wind-up' we allow in the integrator
302
303
	{ "azimuth_pid",           "maxw",                (float) rpm2rads(0.005)  }
	{ "azimuth_pid",           "minw",                (float) rpm2rads(-0.005) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
304

305
	# second order filter to make sure we do not trigger the eigenfrequency of the DT
Jeroen Vreeken's avatar
Jeroen Vreeken committed
306
307
308
	{ "azimuth_pid_filter",    "transfer",            (int) 2,
							  (double) 6.343831259e+05,
						          (float) { -0.9964520027, 1.9964456974 },
309
							  (float) { 1.0, 2.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
310
311
										}

312
313
314
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

315
316
317
318
	# 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
319
	{ "azimuth_pid_limit",     "offset",              (float) rpm2rads(100.0)/$(azimuth_gear) }
320
	{ "azimuth_pid_limit",     "gain",                (float)-0.3               }
321
	# speed limit on spg + pid just before we send it to the drive
322
323
	{ "azimuth_speed_limit",   "min",                 (float) rpm2rads(-3000.0)/$(azimuth_gear) }
	{ "azimuth_speed_limit",   "max",                 (float) rpm2rads(3000.0)/$(azimuth_gear)  }
324
	# gear ratio between DT and servodrive
325
	{ "azimuth_speed_servo",   "gain",                (float)-$(azimuth_gear)          }
326
	# torque we allow the servodrive to actuate on the DT
327
	{ "azimuth_torque",        "value",               (float) 10.0              }
328
	# inverse gear ratio between DT and servodrive
329
	{ "azimuth_position_gain", "gain",                (float)-1.0/$(azimuth_gear) }
330
	# callibration value for position
331
	{ "azimuth_position_offset","value",              (float) 0.0               }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
332

333
334
	{ "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"   }
335
	{ "azimuth_safety_hw_pos", "init",     true }
336
337
	{ "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"   }
338
339
	{ "azimuth_safety_hw_neg", "init",     true }

340
341
342
343
	# 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
344
345
	{ "azimuth_speed_range_negative", "max", deg2rad(290.0)  }
	{ "azimuth_speed_range_negative", "min", deg2rad(-280.0) }
346
347
	{ "azimuth_speed_range_positive", "max", deg2rad(280.0) }
	{ "azimuth_speed_range_positive", "min", deg2rad(-290.0)  }
348

349
350
351
	{ "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 }
352
353
354
355

	{ "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
356
	
357
        { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
358
                                        	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
359

360
361
	{ "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
362
363

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
364
	{ "elevation_spg",         "max_x",               (float) deg2rad(90.0)   }
365
	{ "elevation_spg",         "min_x",               (float) deg2rad(-0.05)  }
366
	{ "elevation_spg",         "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
367
368
369
370
371
	{ "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      }
372
	{ "elevation_servo_state", "max_x",               (float) deg2rad(90.0)   }
373
	{ "elevation_servo_state", "min_x",               (float) deg2rad(-0.05)  }
374
	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
375
	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
376
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
377
378
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
379
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
380
381
382
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
383
384
385
386
	{ "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
387
388
389
390
391
	{ "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
392
393
	{ "elevation_torsion_pid", "maxw",                (float) 0.0001 }
	{ "elevation_torsion_pid", "minw",                (float)-0.0001 }
394
395
	{ "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
396
397
398
399
400
401
402


	{ "elevation_safety",      "safe_zone_min_speed", (float)-30  		}
	{ "elevation_safety",      "safe_zone_max_speed", (float) 30  		}
	{ "elevation_safety",      "torsion_max",         (float) 0.0002 	}

	{ "elevation_safety",      "recover",             (int)   0  		}
403
	{ "elevation_safety",      "torque_max",          (float) 20.0		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
404
405
406
407
408
409
410
411
	{ "elevation_safety",      "emergency_torque",    (float) 3.0 		}

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

412
413
414
415
416
	{ "elevation_safety",      "position_min",        (float) deg2rad(-0.1) * $(elevation_gear)  }
	{ "elevation_safety",      "position_max",        (float) deg2rad(90.5) * $(elevation_gear)  }
	{ "elevation_safety",      "safe_zone_min",       (float) deg2rad(-0.09) * $(elevation_gear) }
	{ "elevation_safety",      "safe_zone_max",       (float) deg2rad(90.4) * $(elevation_gear)  }
	{ "elevation_safety",      "torsion_recover_max", (float) deg2rad(1.0) }
417
418
419

	{ "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"      }
420
	{ "elevation_safety_hw_top",    "init",     true }
421
422
	{ "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"   }
423
	{ "elevation_safety_hw_bottom", "init",     true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
424
}
425
426
427
428

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