dt_ctrl.ctrl 27.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
frequency 250

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

#include "dt_ctrl_el.ctrl"
include "dt_ctrl_el_sim.ctrl"

#include "dt_ctrl_az.ctrl"
include "dt_ctrl_az_sim.ctrl"

Jeroen Vreeken's avatar
Jeroen Vreeken committed
28
include "dt_ctrl_ec_sim.ctrl"
29

Jeroen Vreeken's avatar
Jeroen Vreeken committed
30
blocks {
31
	{ "setpoint_generator", "azimuth_spg", 			"Azimuth_Setpoint", "rad" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
32
	{ "servo_state",        "azimuth_servo_state"              }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
33
	{ "and2",               "azimuth_safe_and"                 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
34
35
36
37
38
39
40
41
42
43
44
45
46
	{ "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"		   }
	{ "dt_az_safety",       "azimuth_safety"		   }
	{ "gain", 	        "azimuth_speed_servo"		   }
	{ "value",	        "azimuth_torque"		   }
	{ "value",	        "azimuth_position_offset"  	   }
	{ "add",  	        "azimuth_position_offset_sum"	   }
	{ "gain", 	        "azimuth_position_gain"		   }
47
	{ "rangecheck",		"azimuth_speed_warning"            }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
48
49
	
	{ "matrix_2x2",	        "elevation_input_matrix"	   }
50
	{ "setpoint_generator", "elevation_spg", 		"Elevation_Setpoint", "rad" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
51
	{ "servo_state",        "elevation_servo_state"            }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
52
 	{ "and2",               "elevation_safe_and"               }
53
	{ "setpoint_generator", "elevation_torsion_spg",	"Elevation_Torsion_Setpoint", "rad" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
54
55
56
57
	{ "subtract",	        "elevation_setpoint_error" 	   }
	{ "subtract",  	        "elevation_error"  		   }
	{ "subtract",	        "elevation_torsion_error"  	   }
	{ "subtract",	        "elevation_torsion_setpoint_error" }
58
	{ "subtract",	        "elevation_torsion_torque" 	   }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
59
60
	{ "filter_lp",	        "elevation_torsion_torque_lp"	   }
	{ "pid_aw",	        "elevation_pid"			   }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
61
	{ "pid_aw",  	        "elevation_torsion_pid"		   }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
62
63
64
65
66
67
68
69
70
71
72
73
74
	{ "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"  }
75
76

	{ "value_bool", 	"false"	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
77
78
79
80
81
82
83
84
}

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 }
	{ "azimuth_position_offset_sum",    "out",        "azimuth_error",                    "negative"     , true }
	{ "azimuth_servo_state",            "reset",      "azimuth_spg",                      "reset"        , false }
85
	{ "azimuth_position_offset_sum",    "out",        "azimuth_spg",                      "reset_x"      , true }
86
87
88
	{ "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 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
89
90
91
	{ "azimuth_safety",                 "safe_out",   "azimuth_safe_and",                 "a"            , false }
	{ "ethercat",                       "pdo_data",   "azimuth_safe_and",                 "b"            , true }
	{ "azimuth_safe_and",               "q",          "azimuth_servo_state",              "safe"         , true }
92
	{ "azimuth_servo_state",            "out_x",      "azimuth_error",                    "positive"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
93
94
	{ "azimuth_position_offset_sum",    "out",        "azimuth_setpoint_error",           "negative"     , true }
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
95
	{ "azimuth_servo_state",            "out_v",      "azimuth_speed_ff",                 "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
96
97
98
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
99
	{ "azimuth_servo_state",            "out_v",      "azimuth_pid_limit",                "limit"        , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
100
101
102
103
104
105
	{ "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 }
	{ "azimuth_speed_limit",            "out",        "azimuth_safety",                   "speed_in"     , true }
	{ "azimuth_position_offset_sum",    "out",        "azimuth_safety",                   "position_in"  , true }
	{ "azimuth_torque",                 "value",      "azimuth_safety",                   "torque_in"    , true }
106
107
	{ "dt_az",                          "be4",        "azimuth_safety",                   "safety_in_positive", true }
	{ "dt_az",                          "be2",        "azimuth_safety",                   "safety_in_negative", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
108
109
110
111
112
	{ "azimuth_servo_state",            "enable",     "azimuth_safety",                   "enable"       , true }
        { "azimuth_safety",                 "speed_out",  "azimuth_speed_servo",              "in"           , true }
	{ "azimuth_speed_servo",            "out",        "dt_az",                            "speed"        , true }
	{ "azimuth_safety",                 "torque_out", "dt_az",                            "torque"       , true }
	{ "azimuth_servo_state",            "enable",     "dt_az",                            "enable"       , true }
113
114
	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
115
	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
116
117

	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
118
	{ "elevation_input_matrix",         "out0",       "elevation_spg",                    "reset_x"      , true }
119
120
121
	{ "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
122
123
124
	{ "elevation_safety",               "safe_out",   "elevation_safe_and",               "a"            , false }
	{ "ethercat",                       "pdo_data",   "elevation_safe_and",               "b"            , true }
	{ "elevation_safe_and",             "q",          "elevation_servo_state",            "safe"         , true }
125
126
	{ "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
127
128
	{ "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
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
	{ "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 }
	{ "elevation_input_matrix",         "out0",       "elevation_error",                  "negative"     , true }
	{ "elevation_spg",                  "setpoint",   "elevation_setpoint_error",         "positive"     , true }
	{ "elevation_input_matrix",         "out0",       "elevation_setpoint_error",         "negative"     , true }
	{ "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 }
145
146
147
	{ "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
148
149
	{ "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
150
	{ "elevation_input_matrix",         "out1",       "elevation_safety",                 "torsion_in"   , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
151
152
	{ "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
153
	{ "elevation_torsion_torque_lp",    "out",        "elevation_safety",                 "torque_in"    , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
154
155
	{ "elevation_torque_r",             "value",      "elevation_safety",                 "torque_in_r"   , true }
	{ "elevation_torque_l",             "value",      "elevation_safety",                 "torque_in_l"   , true }
156
157
	{ "dt_el_l",                        "be4",        "elevation_safety",                 "safety_in_top",    true }
	{ "dt_el_l",                        "be2",        "elevation_safety",                 "safety_in_bottom", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
	{ "elevation_servo_state",          "enable",     "elevation_safety",                 "enable"       , true }
159
160
161
162
	{ "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
163
164
	{ "elevation_servo_state",          "enable",     "dt_el_r",                          "enable"       , true }
	{ "elevation_servo_state",          "enable",     "dt_el_l",                          "enable"       , true }
165
166
167
168
	{ "false",                          "value",      "dt_el_r",                          "ba1"          , true }
	{ "false",			    "value",      "dt_el_r",                          "ba2"          , true }
	{ "false",                          "value",      "dt_el_l",                          "ba1"          , true }
	{ "false",			    "value",      "dt_el_l",                          "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
169
170
	
	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
171
172
	{ "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
173
174
175
176
	{ "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
177
	{ "elevation_servo_state",          "enable",     "elevation_torsion_pid",            "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
178
179
180
181
182
	{ "elevation_torsion_pid",          "out",        "elevation_torsion_speed_limit",    "in"           , true }
	{ "elevation_torsion_speed_limit",  "out",        "elevation_output_matrix",          "in1"          , true }
}

traces {
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
	{ "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"        }
	{ "Azimuth_Position",                 "rad",       "azimuth_position_offset_sum",      "out"        }
	{ "Azimuth_Speed",                    "rad/s",     "dt_az",                            "speed"      }
	{ "Azimuth_Torque",                   "Nm",        "dt_az",                            "torque"     }
	{ "Azimuth_Safe",                     "Boolean",   "azimuth_safety",                   "safe_out"   }
	{ "Azimuth_Enabled",                  "Boolean",   "dt_az",                            "enabled"    }
	{ "Azimuth_Drive_Safety",             "Boolean",   "dt_az",                            "be1"        }
	{ "Azimuth_Drive_Safety_p270",        "Boolean",   "dt_az",                            "be4"        }
	{ "Azimuth_Drive_Safety_m270",        "Boolean",   "dt_az",                            "be2"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
201
		
202
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
203

204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
	{ "Elevation_Position",               "rad",       "elevation_input_matrix",           "out0"       }
	{ "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"        }
	{ "Elevation_Top_Safe",               "Boolean",   "dt_el_l",                          "be4"        }
	{ "Elevation_Bottom_Safe",            "Boolean",   "dt_el_l",                          "be2"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
230
231
}

232
233
234
set azimuth_gear	15006.75
set elevation_gear	44803.125

Jeroen Vreeken's avatar
Jeroen Vreeken committed
235
params {
236
237
	{ "azimuth_spg",           "setpoint",            (float) 0.0		 }
	{ "azimuth_spg",           "t",                   (float) 0.004          }
238
	# maximum speed and position clients input is checked against
239
240
241
	{ "azimuth_spg",           "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_spg",           "min_x",               (float) deg2rad(-270.0)  }
	{ "azimuth_spg",           "max_v",               (float) rpm2rads(2100.0)/$(azimuth_gear) }
242
	# acceleration and jerk we use to generate a profile
243
244
	{ "azimuth_spg",           "max_a",               (float) 0.0002    	 }
	{ "azimuth_spg",           "max_j",               (float) 0.00001        }
245
	# at which precision do we consider the values to be 'equal'
246
247
248
	{ "azimuth_spg",           "precision_x",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_a",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_v",         (float) 0.000001       }
249
	# maximum values within we allow normal operation
250
251
252
	{ "azimuth_servo_state",   "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_servo_state",   "min_x",               (float) deg2rad(-270.0)  }
	{ "azimuth_servo_state",   "max_v",               (float) rpm2rads(2100.0)/$(azimuth_gear) }
253
	{ "azimuth_servo_state",   "max_a",               (float) 0.0002         }
254
	# controller factors
255
256
257
258
	{ "azimuth_pid",           "kp",                  (float) 0.20           }
	{ "azimuth_pid",           "ki",                  (float) 0.00           }
	{ "azimuth_pid",           "kd",                  (float) 0.0            }
	{ "azimuth_pid",           "t",                   (float) 0.004          }
259
	# the amount of 'wind-up' we allow in the integrator
260
261
	{ "azimuth_pid",           "maxw",                (float) rpm2rads(0.005)  }
	{ "azimuth_pid",           "minw",                (float) rpm2rads(-0.005) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
262

263
	# second order filter to make sure we do not trigger the eigenfrequency of the DT
Jeroen Vreeken's avatar
Jeroen Vreeken committed
264
265
266
	{ "azimuth_pid_filter",    "transfer",            (int) 2,
							  (double) 6.343831259e+05,
						          (float) { -0.9964520027, 1.9964456974 },
267
							  (float) { 1.0, 2.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
268
269
										}

270
271
272
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

273
274
275
276
	# 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
277
	{ "azimuth_pid_limit",     "offset",              (float) rpm2rads(100.0)/$(azimuth_gear) }
278
	{ "azimuth_pid_limit",     "gain",                (float)-0.3               }
279
	# speed limit on spg + pid just before we send it to the drive
280
281
	{ "azimuth_speed_limit",   "min",                 (float) rpm2rads(-3000.0)/$(azimuth_gear) }
	{ "azimuth_speed_limit",   "max",                 (float) rpm2rads(3000.0)/$(azimuth_gear)  }
282
	# gear ratio between DT and servodrive
283
	{ "azimuth_speed_servo",   "gain",                (float)-$(azimuth_gear)          }
284
	# torque we allow the servodrive to actuate on the DT
285
	{ "azimuth_torque",        "value",               (float) 10.0              }
286
	# inverse gear ratio between DT and servodrive
287
	{ "azimuth_position_gain", "gain",                (float)-1.0/$(azimuth_gear) }
288
	# callibration value for position
289
	{ "azimuth_position_offset","value",              (float) 0.0               }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
290

291
292
	# positions were we go into safe behavior
	# these must be outside the normal operating range
293
294
	{ "azimuth_safety",        "position_min",        (float) deg2rad(-290.0) }
	{ "azimuth_safety",        "position_max",        (float) deg2rad(290.0)  }
295
296
	# 'safe zone' is between the operating range and the absolute safety
	# value above
297
298
	{ "azimuth_safety",        "safe_zone_min",       (float) deg2rad(-280.0) }
	{ "azimuth_safety",        "safe_zone_max",       (float) deg2rad(280.0)  }
299
300
	{ "azimuth_safety",        "safe_zone_min_speed", (float) rpm2rads(-0.001) }
	{ "azimuth_safety",        "safe_zone_max_speed", (float) rpm2rads(0.001)  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
301
	{ "azimuth_safety",        "emergency_torque",    (float) 3.0           }
302
	# we do not start in recovery mode
Jeroen Vreeken's avatar
Jeroen Vreeken committed
303
	{ "azimuth_safety",        "recover",             (int)   0  		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
304
	
305
        { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
306
                                        	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
307

308
309
	{ "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
310
311
312

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
	{ "elevation_spg",         "t",                   (float) 0.004         }
313
	{ "elevation_spg",         "max_x",               (float) deg2rad(90.0)   }
314
	{ "elevation_spg",         "min_x",               (float) deg2rad(-0.05)  }
315
	{ "elevation_spg",         "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
316
317
318
319
320
	{ "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      }
321
	{ "elevation_servo_state", "max_x",               (float) deg2rad(90.0)   }
322
	{ "elevation_servo_state", "min_x",               (float) deg2rad(-0.05)  }
323
	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
324
	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
325
326
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
	{ "elevation_torsion_spg", "t",                   (float) 0.004         }
327
328
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
329
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
330
331
332
333
334
	{ "elevation_torsion_spg", "max_a",               (float) 0.00004 	}
	{ "elevation_torsion_spg", "max_j",               (float) 0.00001 	}
	{ "elevation_torsion_spg", "precision_x",         (float) 0.000001      }
	{ "elevation_torsion_spg", "precision_a",         (float) 0.000001      }
	{ "elevation_torsion_spg", "precision_v",         (float) 0.000001      }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
335
336
337
338
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
	{ "elevation_pid",         "t",                   (float) 0.004         }
339
340
341
342
	{ "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
343
344
345
346
347
348
349
	{ "elevation_jerk_limit",  "min",                 (float)-0.003         }
	{ "elevation_jerk_limit",  "max",                 (float) 0.003         }
	{ "elevation_jerk_limit",  "t",                   (float) 0.004         }
	{ "elevation_torsion_pid", "t",                   (float) 0.004         }
	{ "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
350
351
	{ "elevation_torsion_pid", "maxw",                (float) 0.0001 }
	{ "elevation_torsion_pid", "minw",                (float)-0.0001 }
352
353
	{ "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
354
355
356
357
358
359
360


	{ "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  		}
361
	{ "elevation_safety",      "torque_max",          (float) 20.0		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
	{ "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", "t",             (float) 0.004         }
	{ "elevation_torsion_torque_lp", "tau",           (float) 10.0          }
	{ "elevation_position_offset_r","value",          (float) 0.0           }
	{ "elevation_position_offset_l","value",          (float) 0.0           }

	{ "elevation_safety",      "position_min",        (float) -78.19620464325844733  }
	{ "elevation_safety",      "position_max",        (float)  70767.56520214889483  }
	{ "elevation_safety",      "safe_zone_min",       (float) -70.37658417893260259  }
	{ "elevation_safety",      "safe_zone_max",       (float)  70689.36899750563638  }
	{ "elevation_safety",      "torsion_recover_max", (float) 0.01745329251994329577 }
}
377
378
379
380

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