corso2013.ctrl 6.52 KB
Newer Older
1
2
3
4
frequency 1000

blocks {
	{ "ec",              "ethercat",    "eth0", 6, 0 }
5
6
	{ "beckhoff_el2xxx", "output_top",  "EL2008", 1 }
	{ "beckhoff_el2xxx", "output_low",  "EL2008", 2 }
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
	{ "beckhoff_el1xxx", "inputblock1", "EL1008", 4 }
	{ "beckhoff_el3xxx", "analog_in",   "EL3102", 5 }

	{ "value_bool",      "false"       }
	{ "value_bool",      "true"        }
	{ "value",           "zero"        }

	{ "filter_lp", "distance_l_lp" }
	{ "filter_lp", "distance_r_lp" }

	{ "inverse_proportional", "distance_l" }
	{ "inverse_proportional", "distance_r" }
	
	{ "matrix_2x2", "input_matrix" }

22
23
24
25
26
27
28
29
	{ "rangecheck", "diff_check" }
	{ "rangecheck", "diff_deadband" }

	{ "trigger", "spg_enable" }
	{ "trigger", "spg_disable" }
	{ "setreset", "spg_enabled" }
	{ "or3", "panic_or" }
	{ "not", "panic" }
30
	{ "not", "disabled" }
31
	{ "and2", "diff_enable" }
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

	{ "setpoint_generator", "lift_spg", "lift_spg", "m" }
	{ "subtract", "distance_error" }
	{ "pid_aw", "distance_pid" }
	{ "pid_aw", "diff_pid" }
	
	{ "add", "lift_add" }

	{ "matrix_2x2", "output_matrix" }
	
	{ "bridge_pwm",      "lift_ll_pwm" }
	{ "bridge_pwm",      "lift_lh_pwm" }
	{ "bridge_pwm",      "lift_rl_pwm" }
	{ "bridge_pwm",      "lift_rh_pwm" }
}


links {
50
51
	{ "false", "value",             "output_low", "output1", true }
	{ "false", "value",             "output_low", "output2", true }
52
53
54
55
56
	{ "false", "value",             "output_low", "output5", true }
	{ "false", "value",             "output_low", "output6", true }
	{ "false", "value",             "output_low", "output7", true }
	{ "false", "value",             "output_low", "output8", true }
	
57
58
59
60
61
62
63
64
	{ "spg_enable", "q",            "spg_enabled", "set",    true }
	{ "inputblock1", "input3",      "panic", "input",        true }
	{ "panic", "output",            "panic_or", "a",         true }
	{ "spg_disable", "q",           "panic_or", "b",         true }
	{ "panic_or", "q",              "spg_enabled", "reset",  true }
	{ "panic_or", "q",              "output_low", "output3", true }
	{ "spg_enabled", "output",      "disabled", "input",     true }
	{ "spg_enabled", "output",      "output_low", "output4", true }
65
66
67
68
69

	{ "analog_in", "input1",        "distance_l_lp", "in",   true }
	{ "analog_in", "input2",        "distance_r_lp", "in",   true }

	{ "distance_l_lp", "out",       "distance_l", "in",      true }
70

71
72
73
74
75
76
77
78
79
80
81
	{ "distance_r_lp", "out",       "distance_r", "in",      true }
	
	{ "distance_l", "out",          "input_matrix", "in0",   true }
	{ "distance_r", "out",          "input_matrix", "in1",   true }

	{ "disabled", "output",         "lift_spg", "reset",     true }
	{ "input_matrix", "out0",       "lift_spg", "reset_x",   true }
	
	{ "lift_spg", "x",              "distance_error", "positive", true }
	{ "input_matrix", "out0",       "distance_error", "negative", true }
	
82
83
84
85
86
87
	{ "input_matrix", "out1",       "diff_check", "in",      true }
	{ "diff_check", "invalid",      "panic_or", "c",         true }
	{ "input_matrix", "out1",       "diff_deadband", "in",   true }
	{ "diff_check", "valid",        "diff_enable", "a",      true }
	{ "spg_enabled", "output",      "diff_enable", "b",      true }
	
88
	{ "distance_error", "difference",  "distance_pid", "in",     true }
89
	{ "spg_enabled", "output",      "distance_pid", "enable", true }
90
91
92
93
94
95
96
97
	
	{ "lift_spg", "v",              "lift_add", "in0",       true }
	{ "distance_pid", "out",        "lift_add", "in1",       true }

	{ "lift_add", "out",            "output_matrix", "in0",  true }

	{ "input_matrix", "out1",       "diff_pid", "in",        true }
	{ "diff_pid", "out",            "output_matrix", "in1",  true }
98
	{ "diff_enable", "q",           "diff_pid", "enable",    true }
99
100
101
102
103
104
	
	{ "output_matrix", "out0",      "lift_ll_pwm", "in",     true }
	{ "output_matrix", "out0",      "lift_lh_pwm", "in",     true }
	{ "output_matrix", "out1",      "lift_rl_pwm", "in",     true }
	{ "output_matrix", "out1",      "lift_rh_pwm", "in",     true }

105
106
107
108
109
110
111
112
	{ "lift_ll_pwm", "b",           "output_top", "output1", true }
	{ "lift_ll_pwm", "a",           "output_top", "output2", true }
	{ "lift_lh_pwm", "b",           "output_top", "output3", true }
	{ "lift_lh_pwm", "a",           "output_top", "output4", true }
	{ "lift_rl_pwm", "b",           "output_top", "output5", true }
	{ "lift_rl_pwm", "a",           "output_top", "output6", true }
	{ "lift_rh_pwm", "b",           "output_top", "output7", true }
	{ "lift_rh_pwm", "a",           "output_top", "output8", true }
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
}

set motor_speed 0.018

params {
	{ "true", "value", true }
	
	{ "lift_spg", "setpoint",    (float)0.0     }
	{ "lift_spg", "t",           (float)0.001   }
	{ "lift_spg", "max_x",       (float)0.8     }
	{ "lift_spg", "min_x",       (float)0.0     }
	{ "lift_spg", "max_v",       (float)0.015   }
	{ "lift_spg", "max_a",       (float)0.01    }
	{ "lift_spg", "max_j",       (float)0.01    }
	{ "lift_spg", "precision_x", (float)0.005   }
	{ "lift_spg", "precision_v", (float)0.005   }
	{ "lift_spg", "precision_a", (float)0.005   }
	
	{ "distance_l_lp", "t",   (float)0.001 }
132
	{ "distance_l_lp", "tau", (float)2.0   }
133
	{ "distance_r_lp", "t",   (float)0.001 }
134
	{ "distance_r_lp", "tau", (float)2.0   }
135

136
137
138
	{ "distance_l", "c0", (float)-0.495196 }
	{ "distance_l", "c1", (float)1.42175   }
	{ "distance_l", "c2", (float)0.515971  }
139

140
141
142
	{ "distance_r", "c0", (float)-0.407968 }
	{ "distance_r", "c1", (float)1.18524}
	{ "distance_r", "c2", (float)0.374365  }
143
144
145
146

	{ "input_matrix", "constants", (float) { 0.5,  0.5 },
                                       (float) { 1.0, -1.0 } }

147
148
149
150
151
152
	{ "diff_check",    "max", (float)  0.10  }
	{ "diff_check",    "min", (float) -0.10  }
	{ "diff_deadband", "max", (float)  0.06 }
	{ "diff_deadband", "min", (float) -0.06 }

	{ "distance_pid", "kp",   (float)0.05  }
153
154
155
156
157
	{ "distance_pid", "ki",   (float)0.0    }
	{ "distance_pid", "kd",   (float)0.0    }
	{ "distance_pid", "maxw", (float)0.005  }
	{ "distance_pid", "minw", (float)-0.005 }

158
	{ "diff_pid", "kp",   (float)0.0  }
159
160
161
162
163
164
	{ "diff_pid", "ki",   (float)0.0    }
	{ "diff_pid", "kd",   (float)0.0    }
	{ "diff_pid", "maxw", (float)0.005  }
	{ "diff_pid", "minw", (float)-0.005 }

	{ "output_matrix", "constants", (float) { 1.0/$(motor_speed), -0.5/$(motor_speed) },
165
                                        (float) { 1.0/$(motor_speed), 0.5/$(motor_speed) } }
166
167

	{ "lift_ll_pwm", "divider",       10   }
168
	{ "lift_ll_pwm", "continuous_on", false }
169
	{ "lift_lh_pwm", "divider",       10   }
170
	{ "lift_lh_pwm", "continuous_on", false }
171
	{ "lift_rl_pwm", "divider",       10   }
172
	{ "lift_rl_pwm", "continuous_on", false }
173
	{ "lift_rh_pwm", "divider",       10   }
174
	{ "lift_rh_pwm", "continuous_on", false }
175
176
177
178
179
}

traces {
	{ "distance",   "m/s",     "input_matrix", "out0"   }
}