dt_ctrl.ctrl 23.1 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
frequency 250

blocks {
	{ "ec",                 "ethercat", "eth0", 3, 0           }

	{ "setpoint_generator", "azimuth_spg"			   }
	{ "servo_state",        "azimuth_servo_state"              }
	{ "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"		   }
	
	{ "matrix_2x2",	        "elevation_input_matrix"	   }
	{ "setpoint_generator", "elevation_spg"			   }
	{ "servo_state",        "elevation_servo_state"            }
	{ "setpoint_generator", "elevation_torsion_spg"		   }
	{ "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",  	        "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"  }
}

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 }
	{ "azimuth_position_offset_sum",    "out",        "azimuth_spg",                      "cur0"         , true }
	{ "azimuth_spg",                    "spg0",       "azimuth_servo_state",              "spg0"         , true }
	{ "azimuth_spg",                    "spg1",       "azimuth_servo_state",              "spg1"         , true }
	{ "azimuth_spg",                    "spg2",       "azimuth_servo_state",              "spg2"         , true }
	{ "azimuth_safety",                 "safe_out",   "azimuth_servo_state",              "safe"         , false }
	{ "azimuth_servo_state",            "out0",       "azimuth_error",                    "positive"     , true }
	{ "azimuth_position_offset_sum",    "out",        "azimuth_setpoint_error",           "negative"     , true }
	{ "azimuth_spg",                    "setpoint",   "azimuth_setpoint_error",           "positive"     , true }
	{ "azimuth_servo_state",            "out1",       "azimuth_speed_ff",                 "in0"          , true }
	{ "azimuth_error",                  "difference", "azimuth_pid",                      "in"           , true }
	{ "azimuth_servo_state",            "enable",     "azimuth_pid",                      "enable"       , true }
	{ "azimuth_pid",                    "out",        "azimuth_pid_limit",                "in"           , true }
	{ "azimuth_servo_state",            "out1",       "azimuth_pid_limit",                "limit"        , true }
	{ "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 }
	{ "dt_az",                          "safety",     "azimuth_safety",                   "safety_in"    , true }
	{ "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 }

	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
	{ "elevation_input_matrix",         "out0",       "elevation_spg",                    "cur0"         , true }
	{ "elevation_spg",                  "spg0",       "elevation_servo_state",            "spg0"         , true }
	{ "elevation_spg",                  "spg1",       "elevation_servo_state",            "spg1"         , true }
	{ "elevation_spg",                  "spg2",       "elevation_servo_state",            "spg2"         , true }
	{ "elevation_safety",               "safe_out",   "elevation_servo_state",            "safe"         , false }
	{ "elevation_servo_state",          "out0",       "elevation_error",                  "positive"     , true }
	{ "elevation_servo_state",          "out1",       "elevation_speed_ff",               "in0"          , true }
	{ "dt_el",                          "position_r", "elevation_position_offset_r_sum",  "in0"          , true }
	{ "dt_el",                          "position_l", "elevation_position_offset_l_sum",  "in0"          , true }
	{ "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 }
	{ "dt_el",                          "torque_r",   "elevation_torsion_torque",         "positive"     , true }
	{ "dt_el",                          "torque_l",   "elevation_torsion_torque",         "negative"     , true }
	{ "elevation_torsion_torque",       "difference", "elevation_torsion_torque_lp",      "in"           , true }
	{ "elevation_position_offset_r_sum","out",        "elevation_safety",                 "position_in0" , true }
	{ "elevation_position_offset_l_sum","out",        "elevation_safety",                 "position_in1" , true }
	{ "elevation_input_matrix",         "out1",       "elevation_safety",                 "torsion_in"   , true }
	{ "elevation_output_matrix",        "out0",       "elevation_safety",                 "speed_in0"    , true }
	{ "elevation_output_matrix",        "out1",       "elevation_safety",                 "speed_in1"    , true }
	{ "elevation_torsion_torque_lp",    "out",        "elevation_safety",                 "torque_in"    , true }
	{ "elevation_torque_r",             "value",      "elevation_safety",                 "torque_in0"   , true }
	{ "elevation_torque_l",             "value",      "elevation_safety",                 "torque_in1"   , true }
	{ "dt_el",                          "safety",     "elevation_safety",                 "safety_in"    , true }
	{ "elevation_servo_state",          "enable",     "elevation_safety",                 "enable"       , true }
	{ "elevation_safety",               "speed_out0", "dt_el",                            "speed_r"      , true } 
	{ "elevation_safety",               "speed_out1", "dt_el",                            "speed_l"      , true }
	{ "elevation_safety",               "torque_out0","dt_el",                            "torque_r"     , true }
	{ "elevation_safety",               "torque_out1","dt_el",                            "torque_l"     , true }
	{ "elevation_servo_state",          "enable",     "dt_el",                            "enable"       , true }

	
	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
	{ "elevation_input_matrix",         "out1",       "elevation_torsion_spg",            "cur0"         , true }
	{ "elevation_torsion_spg",          "spg0",       "elevation_torsion_error",          "positive"     , true }
	{ "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 }
	{ "elevation_torsion_pid",          "out",        "elevation_torsion_speed_limit",    "in"           , true }
	{ "elevation_torsion_speed_limit",  "out",        "elevation_output_matrix",          "in1"          , true }
}

traces {
	{ "Azimuth_Spg0",                     "rad",       "azimuth_spg",                      "spg0"        }
	{ "Azimuth_Spg1",                     "rad/s",     "azimuth_spg",                      "spg1"        }
	{ "Azimuth_Spg2",                     "rad/s/s",   "azimuth_spg",                      "spg2"        }
	{ "Azimuth_Spg3",                     "rad/s/s/s", "azimuth_spg",                      "spg3"        }
	{ "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",                            "safety"      }
		
	{ "Focusbox_Position",                "Volt",      "dt_az",                            "focusbox"    }

	{ "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",                    "spg0"        }
	{ "Elevation_Spg1",                   "rad/s",     "elevation_spg",                    "spg1"        }
	{ "Elevation_Spg2",                   "rad/s",     "elevation_spg",                    "spg2"        }
	{ "Elevation_Spg3",                   "rad/s",     "elevation_spg",                    "spg3"        }
	{ "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",                            "speed_r"     }
	{ "Elevation_Speed_Left",             "rad/s",     "dt_el",                            "speed_l"     }
	{ "Elevation_Torque_Right",           "Nm",        "dt_el",                            "torque_r"    }
	{ "Elevation_Torque_Left",            "Nm",        "dt_el",                            "torque_l"    }
	{ "Elevation_Torsion_Torque",         "Nm",        "elevation_torsion_torque_lp",      "out"         }
	{ "Elevation_Safe",                   "Boolean",   "elevation_safety",                 "safe_out"    }
	{ "Elevation_Enabled",                "Boolean",   "dt_el",                            "enabled"     }
	{ "Elevation_Drive_Safety",           "Boolean",   "dt_el",                            "safety"      }
	{ "Elevation_Top_Safe",               "Boolean",   "dt_el",                            "top_safe"    }
	{ "Elevation_Bottom_Safe",            "Boolean",   "dt_el",                            "bottom_safe" }
}

params {
	{ "azimuth_spg",           "setpoint",            (float) 0.0		}
	{ "azimuth_spg",           "t",                   (float) 0.004         }
	{ "azimuth_spg",           "max",                 (float) 4.712388980384689858 }
	{ "azimuth_spg",           "min",                 (float)-4.712388980384689858 }
	{ "azimuth_spg",           "max1d",               (float) 0.01466076571675236845 }
	{ "azimuth_spg",           "min1d",               (float)-0.01466076571675236845 }
	{ "azimuth_spg",           "max2d",               (float) 0.0002   	}
	{ "azimuth_spg",           "min2d",               (float)-0.0002  	}
	{ "azimuth_spg",           "min2d",               (float)-0.0002  	}
	{ "azimuth_spg",           "max3d",               (float) 0.00001 	}
	{ "azimuth_spg",           "min3d",               (float)-0.00001	}
	{ "azimuth_servo_state",   "max0",                (float) 4.712388980384689858 }
	{ "azimuth_servo_state",   "min0",                (float)-4.712388980384689858 }
	{ "azimuth_servo_state",   "max1",                (float) 0.01466076571675236845 }
	{ "azimuth_servo_state",   "max2",                (float) 0.0002   	}
	{ "azimuth_pid",           "kp",                  (float) 0.20     	}
	{ "azimuth_pid",           "ki",                  (float) 0.00    	}
	{ "azimuth_pid",           "kd",                  (float) 0.0    	}
	{ "azimuth_pid",           "t",                   (float) 0.004         }
	{ "azimuth_pid",           "maxw",                (float) 0.0005235987755982988731 }
	{ "azimuth_pid",           "minw",                (float)-0.0005235987755982988731 }

	{ "azimuth_pid_filter",    "transfer",            (int) 2,
							  (double) 6.343831259e+05,
						          (float) { -0.9964520027, 1.9964456974 },
							  (float) { 1, 2 }
										}

	{ "azimuth_pid_limit",     "offset",              (float) 0.0006981317007977318308 }
	{ "azimuth_pid_limit",     "gain",                (float)-0.3           }
	{ "azimuth_speed_limit",   "min",                 (float)-0.02094395102393195492 }
	{ "azimuth_speed_limit",   "max",                 (float) 0.02094395102393195492 }
	{ "azimuth_speed_servo",   "gain",                (float)-15006.75      }
	{ "azimuth_torque",        "value",               (float) 10.0          }
	{ "azimuth_position_gain", "gain",                (float)-6.663668016059439919e-05 }
	{ "azimuth_position_offset","value",              (float) 0.0           }

	{ "azimuth_safety",        "position_min",        (float)-5.061454830783555773 }
	{ "azimuth_safety",        "position_max",        (float) 5.061454830783555773 }
	{ "azimuth_safety",        "safe_zone_min",       (float)-4.886921905584122815 }
	{ "azimuth_safety",        "safe_zone_max",       (float) 4.886921905584122815 }
	{ "azimuth_safety",        "safe_zone_min_speed", (float)-0.0001047197551196597746 }
	{ "azimuth_safety",        "safe_zone_max_speed", (float) 0.0001047197551196597746 }
	{ "azimuth_safety",        "emergency_torque",    (float) 3.0           }
	
        { "elevation_input_matrix", "constants",          (float) { 1.115993583036897538e-05,  1.115993583036897538e-05 },
                                                          (float) { 2.231987166073795076e-05, -2.231987166073795076e-05 } }

	{ "elevation_output_matrix", "constants",         (float) { 44803.125,  22401.5625 }, 
							  (float) { 44803.125, -22401.5625 } }

	{ "elevation_spg",         "setpoint",            (float) 0.0           }
	{ "elevation_spg",         "t",                   (float) 0.004         }
	{ "elevation_spg",         "max",                 (float) 1.570796326794896619 }
	{ "elevation_spg",         "min",                 (float)-0.0008726646259971647885 }
	{ "elevation_spg",         "max1d",               (float) 0.004921828490624009407 }
	{ "elevation_spg",         "min1d",               (float)-0.004921828490624009407 }
	{ "elevation_spg",         "max2d",               (float) 0.0003   	}
	{ "elevation_spg",         "min2d",               (float)-0.0003   	}
	{ "elevation_spg",         "max3d",               (float) 0.000016	}
	{ "elevation_spg",         "min3d",               (float)-0.000016 	}
	{ "elevation_servo_state", "max0",                (float) 1.570796326794896619 }
	{ "elevation_servo_state", "min0",                (float)-0.0008726646259971647885 }
	{ "elevation_servo_state", "max1",                (float) 0.004921828490624009407 }
	{ "elevation_servo_state", "max2",                (float) 0.0003   	}
	{ "elevation_torsion_spg", "setpoint",            (float) 0.0           }
	{ "elevation_torsion_spg", "t",                   (float) 0.004         }
	{ "elevation_torsion_spg", "max",                 (float) 0.01          }
	{ "elevation_torsion_spg", "min",                 (float)-0.01          }
	{ "elevation_torsion_spg", "max1d",               (float) 0.0001047197551196597746 }
	{ "elevation_torsion_spg", "min1d",               (float)-0.0001047197551196597746 }
	{ "elevation_torsion_spg", "max2d",               (float) 0.00004 	}
	{ "elevation_torsion_spg", "min2d",               (float)-0.00004 	}
	{ "elevation_torsion_spg", "max3d",               (float) 0.00001 	}
	{ "elevation_torsion_spg", "min3d",               (float)-0.00001  	}
	{ "elevation_pid",         "kp",                  (float) 0.80     	}
	{ "elevation_pid",         "ki",                  (float) 0.001  	}
	{ "elevation_pid",         "kd",                  (float) 0.001   	}
	{ "elevation_pid",         "t",                   (float) 0.004         }
	{ "elevation_pid",         "maxw",                (float) 0.0002094395102393195492 }
	{ "elevation_pid",         "minw",                (float)-0.0002094395102393195492 }
	{ "elevation_speed_limit", "min",                 (float)-0.008377580409572781969 }
	{ "elevation_speed_limit", "max",                 (float) 0.008377580409572781969 }
	{ "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 		}
	{ "elevation_torsion_speed_limit", "min",         (float)-0.0001047197551196597746 }
	{ "elevation_torsion_speed_limit", "max",         (float) 0.0001047197551196597746 }


	{ "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  		}
	{ "elevation_safety",      "torque_max",          (float) 1 		}
	{ "elevation_safety",      "emergency_torque",    (float) 3.0 		}

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

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