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
# Uncomment either the real network, or the sim network.
# But never both!

22
23
#import "dt_ctrl_el.ctrl"
import "dt_ctrl_el_sim.ctrl"
24

25
26
#import "dt_ctrl_az.ctrl"
import "dt_ctrl_az_sim.ctrl"
27

28
import "dt_ctrl_ec_sim.ctrl"
29

Jeroen Vreeken's avatar
Jeroen Vreeken committed
30
blocks {
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
	{ "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"	   }
	{ "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"	   }
	{ "rangecheck",		   "azimuth_speed_warning"         }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
48
	
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
	{ "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"  }
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
	{ "azimuth_safety",                 "safe_out",   "azimuth_safe_and",                 "a"            , false }
90
	{ "ethercat",                       "operational","azimuth_safe_and",                 "b"            , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
	{ "azimuth_safe_and",               "q",          "azimuth_servo_state",              "safe"         , true }
92
	{ "azimuth_safety",                 "recover",    "azimuth_servo_state",              "override"     , false }
93
	{ "azimuth_servo_state",            "out_x",      "azimuth_error",                    "positive"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
94
95
	{ "azimuth_position_offset_sum",    "out",        "azimuth_setpoint_error",           "negative"     , true }
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
96
	{ "azimuth_servo_state",            "out_v",      "azimuth_speed_ff",                 "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
97
98
99
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
100
	{ "azimuth_servo_state",            "out_v",      "azimuth_pid_limit",                "limit"        , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
101
102
103
104
105
106
	{ "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 }
107
108
	{ "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
109
110
111
112
113
	{ "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 }
114
115
	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
116
	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
117
118

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

traces {
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
	{ "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
203
		
204
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
230
231
	{ "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
232
233
}

234
235
236
set azimuth_gear	15006.75
set elevation_gear	44803.125

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

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

272
273
274
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

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

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

310
311
	{ "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
312
313
314

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


	{ "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  		}
357
	{ "elevation_safety",      "torque_max",          (float) 20.0		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
358
359
360
361
362
363
364
365
366
	{ "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           }

367
368
369
370
371
	{ "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) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
372
}
373
374
375
376

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