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
trigger {
	{ "prestart", 0.000150 }
}

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

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

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

32
import "dt_ctrl_ec_sim.ctrl"
33

Jeroen Vreeken's avatar
Jeroen Vreeken committed
34
blocks {
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
	{ "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
52
	
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
	{ "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"  }
79

Jeroen Vreeken's avatar
Jeroen Vreeken committed
80
81
82
	{ "command_bool",          "focusbox_plus" }
	{ "command_bool",          "focusbox_min"  }

83
	{ "value_bool", 	   "false"	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
84
85
}

86
alias {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
87
88
89
	{ "Azimuth_Position",          "azimuth_position_offset_sum", "out"  }
	{ "Azimuth_Drive_Safety_p270", "dt_az", "be4" }
	{ "Azimuth_Drive_Safety_m270", "dt_az", "be2" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
90
91
92
	{ "Elevation_Position",        "elevation_input_matrix", "out0" }
	{ "Elevation_Top_Safe",        "dt_el_l", "be4" }
	{ "Elevation_Bottom_Safe",     "dt_el_l", "be2" }
93
94
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
95
96
97
98
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 }
99
	{ $<Azimuth_Position>,                            "azimuth_error",                    "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
100
	{ "azimuth_servo_state",            "reset",      "azimuth_spg",                      "reset"        , false }
101
	{ $<Azimuth_Position>,                            "azimuth_spg",                      "reset_x"      , true }
102
103
104
	{ "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
105
	{ "azimuth_safety",                 "safe_out",   "azimuth_safe_and",                 "a"            , false }
106
	{ "ethercat",                       "operational","azimuth_safe_and",                 "b"            , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
107
	{ "azimuth_safe_and",               "q",          "azimuth_servo_state",              "safe"         , true }
108
	{ "azimuth_safety",                 "recover",    "azimuth_servo_state",              "override"     , false }
109
	{ "azimuth_servo_state",            "out_x",      "azimuth_error",                    "positive"     , true }
110
	{ $<Azimuth_Position>,                            "azimuth_setpoint_error",           "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
111
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
112
	{ "azimuth_servo_state",            "out_v",      "azimuth_speed_ff",                 "in0"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
115
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
116
	{ "azimuth_servo_state",            "out_v",      "azimuth_pid_limit",                "limit"        , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
117
118
119
120
	{ "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 }
121
	{ $<Azimuth_Position>,                            "azimuth_safety",                   "position_in"  , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
122
	{ "azimuth_torque",                 "value",      "azimuth_safety",                   "torque_in"    , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
123
124
	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safety",                   "safety_in_positive", true }
	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safety",                   "safety_in_negative", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
125
126
127
128
129
	{ "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 }
130
131
	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
132
	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
134

	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
135
	{ $<Elevation_Position>,                          "elevation_spg",                    "reset_x"      , true }
136
137
138
	{ "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
139
	{ "elevation_safety",               "safe_out",   "elevation_safe_and",               "a"            , false }
140
	{ "ethercat",                       "operational","elevation_safe_and",               "b"            , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
141
	{ "elevation_safe_and",             "q",          "elevation_servo_state",            "safe"         , true }
142
	{ "elevation_safety",               "recover",    "elevation_servo_state",            "override"     , false }
143
144
	{ "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
145
146
	{ "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
147
148
149
150
	{ "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 }
151
	{ $<Elevation_Position>,                          "elevation_error",                  "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
152
	{ "elevation_spg",                  "setpoint",   "elevation_setpoint_error",         "positive"     , true }
153
	{ $<Elevation_Position>,                          "elevation_setpoint_error",         "negative"     , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
154
155
156
157
158
159
160
161
162
	{ "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 }
163
164
165
	{ "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
166
167
	{ "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
168
	{ "elevation_input_matrix",         "out1",       "elevation_safety",                 "torsion_in"   , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
169
170
	{ "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
171
	{ "elevation_torsion_torque_lp",    "out",        "elevation_safety",                 "torque_in"    , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
172
173
	{ "elevation_torque_r",             "value",      "elevation_safety",                 "torque_in_r"   , true }
	{ "elevation_torque_l",             "value",      "elevation_safety",                 "torque_in_l"   , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
174
175
	{ $<Elevation_Top_Safe>,                          "elevation_safety",                 "safety_in_top",    true }
	{ $<Elevation_Bottom_Safe>,                       "elevation_safety",                 "safety_in_bottom", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
176
	{ "elevation_servo_state",          "enable",     "elevation_safety",                 "enable"       , true }
177
178
179
180
	{ "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
181
182
	{ "elevation_servo_state",          "enable",     "dt_el_r",                          "enable"       , true }
	{ "elevation_servo_state",          "enable",     "dt_el_l",                          "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
183
184
	{ "focusbox_plus",                  "value",      "dt_el_r",                          "ba1"          , true }
	{ "focusbox_min",		    "value",      "dt_el_r",                          "ba2"          , true }
185
186
	{ "false",                          "value",      "dt_el_l",                          "ba1"          , true }
	{ "false",			    "value",      "dt_el_l",                          "ba2"          , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
187
188
	
	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
189
190
	{ "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
191
192
193
194
	{ "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
195
	{ "elevation_servo_state",          "enable",     "elevation_torsion_pid",            "enable"       , true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
196
197
198
199
200
	{ "elevation_torsion_pid",          "out",        "elevation_torsion_speed_limit",    "in"           , true }
	{ "elevation_torsion_speed_limit",  "out",        "elevation_output_matrix",          "in1"          , true }
}

traces {
201
202
203
204
205
206
207
208
209
210
	{ "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"        }
211
	{ "Azimuth_Position",                 "rad",       $<Azimuth_Position>                              }
212
213
214
215
216
	{ "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"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
217
218
	{ "Azimuth_Drive_Safety_p270",        "Boolean",   $<Azimuth_Drive_Safety_p270>                     }
	{ "Azimuth_Drive_Safety_m270",        "Boolean",   $<Azimuth_Drive_Safety_m270>                     }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
219
		
220
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
221

222
	{ "Elevation_Position",               "rad",       $<Elevation_Position>                            }
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
	{ "Elevation_Torsion",                "rad",       "elevation_input_matrix",           "out1"       }
	{ "Elevation_Setpoint",               "rad",       "elevation_spg",                    "setpoint"   }
	{ "Elevation_Spg0",                   "rad",       "elevation_spg",                    "x"          }
	{ "Elevation_Spg1",                   "rad/s",     "elevation_spg",                    "v"          }
	{ "Elevation_Spg2",                   "rad/s",     "elevation_spg",                    "a"          }
	{ "Elevation_Spg3",                   "rad/s",     "elevation_spg",                    "j"          }
	{ "Elevation_Error",                  "rad",       "elevation_error",                  "difference" }
	{ "Elevation_Setpoint_Error",         "rad",       "elevation_setpoint_error",         "difference" }
	{ "Elevation_PID",                    "rad/s",     "elevation_pid",                    "out"        }
	{ "Elevation_Torsion_Error",          "rad",       "elevation_torsion_error",          "difference" }
	{ "Elevation_Torsion_Setpoint_Error", "rad",       "elevation_torsion_setpoint_error", "difference" }
	{ "Elevation_Torsion_PID",            "rad/s",     "elevation_torsion_pid",            "out"        }
	{ "Elevation_Position_Right",         "rad",       "elevation_position_offset_r_sum",  "out"        }
	{ "Elevation_Position_Left",          "rad",       "elevation_position_offset_l_sum",  "out"        }
	{ "Elevation_Speed_Right",            "rad/s",     "dt_el_r",                          "speed"      }
	{ "Elevation_Speed_Left",             "rad/s",     "dt_el_l",                          "speed"      }
	{ "Elevation_Torque_Right",           "Nm",        "dt_el_r",                          "torque"     }
	{ "Elevation_Torque_Left",            "Nm",        "dt_el_l",                          "torque"     }
	{ "Elevation_Torsion_Torque",         "Nm",        "elevation_torsion_torque_lp",      "out"        }
	{ "Elevation_Safe",                   "Boolean",   "elevation_safety",                 "safe_out"   }
	{ "Elevation_Enabled",                "Boolean",   "dt_el_l",                          "enabled"    }
	{ "Elevation_Drive_Safety_Right",     "Boolean",   "dt_el_r",                          "be1"        }
	{ "Elevation_Drive_Safety_Left",      "Boolean",   "dt_el_l",                          "be1"        }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
246
247
	{ "Elevation_Top_Safe",               "Boolean",   $<Elevation_Top_Safe>                            }
	{ "Elevation_Bottom_Safe",            "Boolean",   $<Elevation_Bottom_Safe>                         }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
248
249
}

250
251
252
set azimuth_gear	15006.75
set elevation_gear	44803.125

Jeroen Vreeken's avatar
Jeroen Vreeken committed
253
params {
254
	{ "azimuth_spg",           "setpoint",            (float) 0.0		 }
255
	# maximum speed and position clients input is checked against
256
257
	{ "azimuth_spg",           "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_spg",           "min_x",               (float) deg2rad(-270.0)  }
258
	{ "azimuth_spg",           "max_v",               (float) rpm2rads(3000.0)/$(azimuth_gear) }
259
	# acceleration and jerk we use to generate a profile
260
261
	{ "azimuth_spg",           "max_a",               (float) 0.0002    	 }
	{ "azimuth_spg",           "max_j",               (float) 0.00001        }
262
	# at which precision do we consider the values to be 'equal'
263
264
265
	{ "azimuth_spg",           "precision_x",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_a",         (float) 0.000001       }
	{ "azimuth_spg",           "precision_v",         (float) 0.000001       }
266
	# maximum values within we allow normal operation
267
268
	{ "azimuth_servo_state",   "max_x",               (float) deg2rad(270.0)   }
	{ "azimuth_servo_state",   "min_x",               (float) deg2rad(-270.0)  }
269
	{ "azimuth_servo_state",   "max_v",               (float) rpm2rads(3000.0)/$(azimuth_gear) }
270
	{ "azimuth_servo_state",   "max_a",               (float) 0.0002         }
271
	# controller factors
272
273
274
	{ "azimuth_pid",           "kp",                  (float) 0.20           }
	{ "azimuth_pid",           "ki",                  (float) 0.00           }
	{ "azimuth_pid",           "kd",                  (float) 0.0            }
275
	# the amount of 'wind-up' we allow in the integrator
276
277
	{ "azimuth_pid",           "maxw",                (float) rpm2rads(0.005)  }
	{ "azimuth_pid",           "minw",                (float) rpm2rads(-0.005) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
278

279
	# second order filter to make sure we do not trigger the eigenfrequency of the DT
Jeroen Vreeken's avatar
Jeroen Vreeken committed
280
281
282
	{ "azimuth_pid_filter",    "transfer",            (int) 2,
							  (double) 6.343831259e+05,
						          (float) { -0.9964520027, 1.9964456974 },
283
							  (float) { 1.0, 2.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
284
285
										}

286
287
288
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

289
290
291
292
	# 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
293
	{ "azimuth_pid_limit",     "offset",              (float) rpm2rads(100.0)/$(azimuth_gear) }
294
	{ "azimuth_pid_limit",     "gain",                (float)-0.3               }
295
	# speed limit on spg + pid just before we send it to the drive
296
297
	{ "azimuth_speed_limit",   "min",                 (float) rpm2rads(-3000.0)/$(azimuth_gear) }
	{ "azimuth_speed_limit",   "max",                 (float) rpm2rads(3000.0)/$(azimuth_gear)  }
298
	# gear ratio between DT and servodrive
299
	{ "azimuth_speed_servo",   "gain",                (float)-$(azimuth_gear)          }
300
	# torque we allow the servodrive to actuate on the DT
301
	{ "azimuth_torque",        "value",               (float) 10.0              }
302
	# inverse gear ratio between DT and servodrive
303
	{ "azimuth_position_gain", "gain",                (float)-1.0/$(azimuth_gear) }
304
	# callibration value for position
305
	{ "azimuth_position_offset","value",              (float) 0.0               }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
306

307
308
	# positions were we go into safe behavior
	# these must be outside the normal operating range
309
310
	{ "azimuth_safety",        "position_min",        (float) deg2rad(-290.0) }
	{ "azimuth_safety",        "position_max",        (float) deg2rad(290.0)  }
311
312
	# 'safe zone' is between the operating range and the absolute safety
	# value above
313
314
	{ "azimuth_safety",        "safe_zone_min",       (float) deg2rad(-280.0) }
	{ "azimuth_safety",        "safe_zone_max",       (float) deg2rad(280.0)  }
315
316
	{ "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
317
	{ "azimuth_safety",        "emergency_torque",    (float) 3.0           }
318
	# we do not start in recovery mode
Jeroen Vreeken's avatar
Jeroen Vreeken committed
319
	{ "azimuth_safety",        "recover",             (int)   0  		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
320
	
321
        { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
322
                                        	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
323

324
325
	{ "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
326
327

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
328
	{ "elevation_spg",         "max_x",               (float) deg2rad(90.0)   }
329
	{ "elevation_spg",         "min_x",               (float) deg2rad(-0.05)  }
330
	{ "elevation_spg",         "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
331
332
333
334
335
	{ "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      }
336
	{ "elevation_servo_state", "max_x",               (float) deg2rad(90.0)   }
337
	{ "elevation_servo_state", "min_x",               (float) deg2rad(-0.05)  }
338
	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
339
	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
340
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
341
342
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
343
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
344
345
346
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
347
348
349
350
	{ "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
351
352
353
354
355
	{ "elevation_jerk_limit",  "min",                 (float)-0.003         }
	{ "elevation_jerk_limit",  "max",                 (float) 0.003         }
	{ "elevation_torsion_pid", "kp",                  (float) 0.2  		}
	{ "elevation_torsion_pid", "ki",                  (float) 0.0  		}
	{ "elevation_torsion_pid", "kd",                  (float) 0.0 		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
356
357
	{ "elevation_torsion_pid", "maxw",                (float) 0.0001 }
	{ "elevation_torsion_pid", "minw",                (float)-0.0001 }
358
359
	{ "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
360
361
362
363
364
365
366


	{ "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  		}
367
	{ "elevation_safety",      "torque_max",          (float) 20.0		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
368
369
370
371
372
373
374
375
	{ "elevation_safety",      "emergency_torque",    (float) 3.0 		}

	{ "elevation_torque_r",    "value",               (float) 10.0          }
	{ "elevation_torque_l",    "value",               (float) 10.0          }
	{ "elevation_torsion_torque_lp", "tau",           (float) 10.0          }
	{ "elevation_position_offset_r","value",          (float) 0.0           }
	{ "elevation_position_offset_l","value",          (float) 0.0           }

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

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