dt_ctrl.ctrl 27.7 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
	{ "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
	{ "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
328
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
	{ "elevation_torsion_spg", "t",                   (float) 0.004         }
329
330
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
331
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
332
333
334
335
336
	{ "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
337
338
339
340
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
	{ "elevation_pid",         "t",                   (float) 0.004         }
341
342
343
344
	{ "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
345
346
347
348
349
350
351
	{ "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
352
353
	{ "elevation_torsion_pid", "maxw",                (float) 0.0001 }
	{ "elevation_torsion_pid", "minw",                (float)-0.0001 }
354
355
	{ "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
356
357
358
359
360
361
362


	{ "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  		}
363
	{ "elevation_safety",      "torque_max",          (float) 20.0		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
364
365
366
367
368
369
370
371
372
	{ "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           }

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

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