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

# Frequency is limited by the stoeber drives.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
11
12
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
87
88
89
90
alias {
	{ "Azimuth_Position",   "azimuth_position_offset_sum", "out"  }
	{ "Elevation_Position", "elevation_input_matrix",      "out0" }
}

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

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

traces {
197
198
199
200
201
202
203
204
205
206
	{ "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"        }
207
	{ "Azimuth_Position",                 "rad",       $<Azimuth_Position>                              }
208
209
210
211
212
213
214
	{ "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
215
		
216
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "ae1"	    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
217

218
	{ "Elevation_Position",               "rad",       $<Elevation_Position>                            }
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
	{ "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
244
245
}

246
247
248
set azimuth_gear	15006.75
set elevation_gear	44803.125

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

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

282
283
284
	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }

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

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

320
321
	{ "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
322
323

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
324
	{ "elevation_spg",         "max_x",               (float) deg2rad(90.0)   }
325
	{ "elevation_spg",         "min_x",               (float) deg2rad(-0.05)  }
326
	{ "elevation_spg",         "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
327
328
329
330
331
	{ "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      }
332
	{ "elevation_servo_state", "max_x",               (float) deg2rad(90.0)   }
333
	{ "elevation_servo_state", "min_x",               (float) deg2rad(-0.05)  }
334
	{ "elevation_servo_state", "max_v",               (float) rpm2rads(2100.0)/$(elevation_gear) }
335
	{ "elevation_servo_state", "max_a",               (float) 0.0003   	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
336
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
337
338
	{ "elevation_torsion_spg", "max_x",               (float) 0.00          }
	{ "elevation_torsion_spg", "min_x",               (float)-0.00          }
339
	{ "elevation_torsion_spg", "max_v",               (float) rpm2rads(0.001) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
340
341
342
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
343
344
345
346
	{ "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
347
348
349
350
351
	{ "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
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
	{ "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           }

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

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