dt_ctrl_el_sim.ctrl 9.52 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
blocks ($(frequency), $(delay)) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
	{ "limit",	"dt_el_speed_limit_r" 	}
	{ "quantize",  	"dt_el_speed_quantize_r" }
	{ "subtract",  	"dt_el_speed_fb_r" 	}
	{ "pid",	"dt_el_driver_r" 	}
	{ "gain",  	"dt_el_torque_gain_r" 	}
	{ "limit_dyn", 	"dt_el_torque_limit_r" 	}
	{ "add",  	"dt_el_torque_add_cross_r" }
	{ "friction",  	"dt_el_friction_r" 	}
	{ "pid",  	"dt_el_accelerate_r" 	}
	{ "pid",  	"dt_el_speed2pos_r" 	}
	{ "value",	"dt_el_offset_r" 	}
	{ "add",  	"dt_el_offset_add_r" 	}
	{ "quantize",  	"dt_el_pos_quantize_r" 	}

	{ "limit",	"dt_el_speed_limit_l" 	}
	{ "quantize",  	"dt_el_speed_quantize_l" }
	{ "subtract",  	"dt_el_speed_fb_l" 	}
	{ "pid", 	"dt_el_driver_l" 	}
	{ "gain",  	"dt_el_torque_gain_l" 	}
	{ "limit_dyn", 	"dt_el_torque_limit_l" 	}
	{ "add",  	"dt_el_torque_add_cross_l" }
	{ "friction",  	"dt_el_friction_l" 	}
	{ "pid",  	"dt_el_accelerate_l" 	}
	{ "pid",  	"dt_el_speed2pos_l" 	}
	{ "value",	"dt_el_offset_l" 	}
	{ "add",  	"dt_el_offset_add_l" 	}
	{ "quantize",  	"dt_el_pos_quantize_l" 	}

	{ "add",  	"dt_el_cross_diff" 	}
	{ "gain",  	"dt_el_cross_tension_r"	}
	{ "gain",  	"dt_el_cross_tension_l"	}
	
	{ "register",	"dt_el_enabled" 	}
	{ "value_bool",	"dt_el_safety_t"	}
	{ "value_bool",	"dt_el_safety_b" 	}
	{ "not",	"dt_el_safety_t_not"	}
	{ "not",	"dt_el_safety_b_not" 	}
	{ "value_bool",	"dt_el_bex" 		}
	{ "value",	"dt_el_ae1"	 	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41
42
43
44
	{ "not",        "dt_el_ba1_r"           }
	{ "not",        "dt_el_ba2_r"           }
	{ "not",        "dt_el_ba1_l"           }
	{ "not",        "dt_el_ba2_l"           }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
45

Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
	module ("dt_el_r") {
		{
			{ "speed",	"dt_el_speed_limit_r",  "in" }
			{ "torque",	"dt_el_torque_gain_r", 	"in" }
			{ "enable",	"dt_el_enabled",      	"input" }
			{ "ba1",        "dt_el_ba1_r", "input" }
			{ "ba2",        "dt_el_ba2_r", "input" }
		}
		{
			{ "position", 	"dt_el_pos_quantize_r", "out"   }
			{ "speed",    	"dt_el_accelerate_r",   "out"   }
			{ "torque",   	"dt_el_torque_limit_r", "out"   }
			{ "enabled", 	"dt_el_enabled",      	"output" }
			{ "be1",	"dt_el_bex",		"value" }
			{ "be2",	"dt_el_bex",		"value" }
			{ "be3",	"dt_el_bex",		"value" }
			{ "be4",	"dt_el_bex",		"value" }
			{ "be5",	"dt_el_bex",		"value" }
			{ "ae1", 	"dt_el_ae1",		"value" }
			{ "external_enable", "dt_el_bex",       "value" }
		}
	}
	
	module ("dt_el_l") {
		{
			{ "speed",	"dt_el_speed_limit_l",  "in" }
			{ "torque",	"dt_el_torque_gain_l", 	"in" }
			{ "enable",	"dt_el_enabled",      	"input" }
			{ "ba1",        "dt_el_ba1_l", "input" }
			{ "ba2",        "dt_el_ba2_l", "input" }
		}
		{
			{ "position", 	"dt_el_pos_quantize_l", "out"    }
			{ "speed",    	"dt_el_accelerate_l",   "out"    }
			{ "torque",   	"dt_el_torque_limit_l", "out"    }
			{ "enabled", 	"dt_el_enabled",      	"output" }
			{ "be1",	"dt_el_safety_b_not",	"output" }
			{ "be2",	"dt_el_safety_b",	"value"  }
			{ "be3",	"dt_el_safety_t_not",	"output" }
			{ "be4",	"dt_el_safety_t",	"value"  }
			{ "be5",	"dt_el_bex",		"value"  }
			{ "ae1", 	"dt_el_ae1",		"value"  }
			{ "external_enable", "dt_el_bex",       "value"  }	
		}
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
92
93
94
}

params {
	{ "dt_el_torque_gain_r",    "gain",              (float)1.0              }
95
96
97
	{ "dt_el_speed_limit_r",    "max",               (float) rpm2rads(3000.0)}
	{ "dt_el_speed_limit_r",    "min",               (float)-rpm2rads(3000.0)}
	{ "dt_el_speed_quantize_r", "quantum",           (float) rpm2rads(3000.0)/16384.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
98
99
100
101
102
103
104
105
106
107
108
109
110
111
	{ "dt_el_driver_r",         "kp",                (float)5.0              }
	{ "dt_el_driver_r",         "ki",                (float)10.0             }
	{ "dt_el_driver_r",         "kd",                (float)0.01             }
	{ "dt_el_friction_r",       "friction_static",   (float)0.25             }
	{ "dt_el_friction_r",       "friction_dynamic",  (float)0.22             }
	{ "dt_el_friction_r",       "deadzone_max",      (float)0.1              }
	{ "dt_el_friction_r",       "deadzone_min",      (float)-0.1             }
	{ "dt_el_accelerate_r",     "kp",                (float)0.0              }
	{ "dt_el_accelerate_r",     "ki",                (float)10.0             }
	{ "dt_el_accelerate_r",     "kd",                (float)0.0              }
	{ "dt_el_speed2pos_r",      "kp",                (float)0.0              }
	{ "dt_el_speed2pos_r",      "ki",                (float)1.0              }
	{ "dt_el_speed2pos_r",      "kd",                (float)0.0              }
	{ "dt_el_offset_r",         "value",             (float)-40680.0833715   }
112
	{ "dt_el_pos_quantize_r",   "quantum",           (float)2.0 * pi / 16384.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
	
	{ "dt_el_torque_gain_l",    "gain",              (float)1.0              }
115
116
117
	{ "dt_el_speed_limit_l",    "max",               (float) rpm2rads(3000.0)}
	{ "dt_el_speed_limit_l",    "min",               (float)-rpm2rads(3000.0)}
	{ "dt_el_speed_quantize_l", "quantum",           (float) rpm2rads(3000.0)/16384.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
118
119
120
121
122
123
124
125
126
127
128
129
130
131
	{ "dt_el_driver_l",         "kp",                (float)5.0              }
	{ "dt_el_driver_l",         "ki",                (float)10.0             }
	{ "dt_el_driver_l",         "kd",                (float)0.01             }
	{ "dt_el_friction_l",       "friction_static",   (float)0.25             }
	{ "dt_el_friction_l",       "friction_dynamic",  (float)0.23             }
	{ "dt_el_friction_l",       "deadzone_max",      (float)0.1              }
	{ "dt_el_friction_l",       "deadzone_min",      (float)-0.1             }
	{ "dt_el_accelerate_l",     "kp",                (float)0.0              }
	{ "dt_el_accelerate_l",     "ki",                (float)10.0             }
	{ "dt_el_accelerate_l",     "kd",                (float)0.0              }
	{ "dt_el_speed2pos_l",      "kp",                (float)0.0              }
	{ "dt_el_speed2pos_l",      "ki",                (float)1.0              }
	{ "dt_el_speed2pos_l",      "kd",                (float)0.0              }
	{ "dt_el_offset_l",         "value",             (float)-10783.776612094  }
132
	{ "dt_el_pos_quantize_l",   "quantum",           (float)2.0 * pi / 16384.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
134
135
136

	{ "dt_el_cross_tension_r",  "gain",              (float)-50.0              }
	{ "dt_el_cross_tension_l",  "gain",              (float)-50.0             }

137
	{ "dt_el_safety_t",         "value",             true    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
	{ "dt_el_safety_b",         "value",             (bool)1 }
	{ "dt_el_bex",              "value",             (bool)0 }
}

links {
        { "dt_el_speed_limit_r",      "out",        "dt_el_speed_quantize_r",    "in",       true  }
	{ "dt_el_speed_quantize_r",   "out",        "dt_el_speed_fb_r",          "positive", true  }
	{ "dt_el_speed_fb_r",         "difference", "dt_el_driver_r",            "in",       true  }
	{ "dt_el_torque_gain_r",      "out",        "dt_el_torque_limit_r",      "limit",    false }
	{ "dt_el_driver_r",           "out",        "dt_el_torque_limit_r",      "in",       false }
	{ "dt_el_torque_limit_r",     "out",        "dt_el_torque_add_cross_r",  "in0",      true  }
	{ "dt_el_torque_add_cross_r", "out",        "dt_el_friction_r",          "in",       true  }
	{ "dt_el_friction_r",         "out",        "dt_el_accelerate_r",        "in",       true  }
	{ "dt_el_accelerate_r",       "out",        "dt_el_speed2pos_r",         "in",       true  }
	{ "dt_el_accelerate_r",       "out",        "dt_el_speed_fb_r",          "negative", false }
	{ "dt_el_accelerate_r",       "out",        "dt_el_friction_r",          "speed",    false }
	{ "dt_el_speed2pos_r",        "out",        "dt_el_offset_add_r",        "in0",      true  }
	{ "dt_el_offset_r",           "value",      "dt_el_offset_add_r",        "in1",      true  }
	{ "dt_el_offset_add_r",       "out",        "dt_el_pos_quantize_r",      "in",       true  }

        { "dt_el_speed_limit_l",      "out",        "dt_el_speed_quantize_l",    "in",       true  }
	{ "dt_el_speed_quantize_l",   "out",        "dt_el_speed_fb_l",          "positive", true  }
	{ "dt_el_speed_fb_l",         "difference", "dt_el_driver_l",            "in",       true  }
	{ "dt_el_torque_gain_l",      "out",        "dt_el_torque_limit_l",      "limit",    false }
	{ "dt_el_driver_l",           "out",        "dt_el_torque_limit_l",      "in",       false }
	{ "dt_el_torque_limit_l",     "out",        "dt_el_torque_add_cross_l",  "in0",      true  }
	{ "dt_el_torque_add_cross_l", "out",        "dt_el_friction_l",          "in",       true  }
	{ "dt_el_friction_l",         "out",        "dt_el_accelerate_l",        "in",       true  }
	{ "dt_el_accelerate_l",       "out",        "dt_el_speed2pos_l",         "in",       true  }
	{ "dt_el_accelerate_l",       "out",        "dt_el_speed_fb_l",          "negative", false }
	{ "dt_el_accelerate_l",       "out",        "dt_el_friction_l",          "speed",    false }
	{ "dt_el_speed2pos_l",        "out",        "dt_el_offset_add_l",        "in0",      true  }
	{ "dt_el_offset_l",           "value",      "dt_el_offset_add_l",        "in1",      true  }
	{ "dt_el_offset_add_l",       "out",        "dt_el_pos_quantize_l",      "in",       true  }
	
	{ "dt_el_speed2pos_r",        "out",        "dt_el_cross_diff",          "in0",      true  }
	{ "dt_el_speed2pos_l",        "out",        "dt_el_cross_diff",          "in1",      true  }
	{ "dt_el_cross_diff",         "out",        "dt_el_cross_tension_r",     "in",       true  }
	{ "dt_el_cross_diff",         "out", 	    "dt_el_cross_tension_l",     "in",       true  }
	{ "dt_el_cross_tension_r",    "out",        "dt_el_torque_add_cross_r",  "in1",      false }
	{ "dt_el_cross_tension_l",    "out",        "dt_el_torque_add_cross_l",  "in1",      false }

	{ "dt_el_safety_t",           "value",      "dt_el_safety_t_not",        "input",    true  }
	{ "dt_el_safety_b",           "value",      "dt_el_safety_b_not",        "input",    true  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
182
}