ctrl_embedded.ctrl 7.97 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
2
3
4
5
6
7
8
9
10
11
#	embedded ctrl
#
#

set frequency 100
set delay 0.0

trigger {
	{ "systick" }
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
12
13
14
15
16
17
blocks (2.0, 0.0) {
	{ "atsamx70_pio_out", "status0_LED", 0, 2 }  #PA2
	{ "atsamx70_pio_out", "blinky_LED", 2, 8   } #PC8
	{ "not",              "blinky"             }
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
18
19
blocks ($(frequency), $(delay)) {
	# Profiling block, may be commented out.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
20
	#{ "controller_profile", "profile" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
21

Jeroen Vreeken's avatar
Jeroen Vreeken committed
22
	{ "atsamx70_tc",           "counter0", 0, 2500, 0  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
23
	{ "atsamx70_tc",           "counter1", 1, 10000, 0, 1 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
24
25
26
27
28
29
	{ "atsamx70_afec",         "afec0", 0              }
	{ "atsamx70_pwm",          "pwm0", 0, 0, 10000.0   }

	{ "atsamx70_pio_in",       "button0", 2, 30        } #PC30
	{ "atsamx70_pio_in",       "button1", 0, 17        } #PA17
	{ "atsamx70_pio_in",       "button2", 2, 12        } #PC12
Jeroen Vreeken's avatar
Jeroen Vreeken committed
30

Jeroen Vreeken's avatar
Jeroen Vreeken committed
31
32
33
34
35
36
37
38
	{ "atsamx70_pio_out",      "status1_LED", 0, 24    } #PA24
	{ "atsamx70_pio_out",      "status2_LED", 0, 6     } #PA6
	{ "atsamx70_pio_out",      "status3_LED", 3, 11    } #PD11
	{ "atsamx70_pio_out",      "status4_LED", 2, 19    } #PC19
	{ "atsamx70_pio_out",      "status5_LED", 3, 26    } #PD26
	
	{ "gain",                  "w2V"         }
	{ "gain",                  "V2pwm"       }
39
	{ "quadratic_sign",        "joystick_scale" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
40
	{ "deadzone",              "joystick_deadzone" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41
42
	{ "gain",                  "joystick2v"  }
	{ "gain",                  "joystick2p"  }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
43
44
45
	{ "motor_model_dc_v",      "motor_model" }
	{ "i2t",                   "motor_i2t"   }
	{ "gain",                  "Ilim2Vlim"   }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
46
	{ "limit",                 "Vlim_supply" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
47
48
	{ "limit_dyn",             "Vlim"        }
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
49
50
	{ "not",                   "button_v"    }
	{ "not",                   "button_x"    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
51
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
52
	{ "add",                   "position_offset" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
53
	{ "and2",                  "track_x_cmd" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
54
55
	{ "not",                   "spg_reset" }
	{ "delay_bool",            "spg_enable" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
56
	{ "setpoint_generator_3d", "spg", "spg", "rad" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
57
58
59
60
	{ "subtract",              "error"       }
	{ "deadzone",              "error_deadzone" }
	{ "pid_aw",                "pid" }
	{ "add",                   "speed_ff" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
61
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
62
	{ "value_bool", 	   "false" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
63
	{ "value",                 "position_calibrate" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
64

Jeroen Vreeken's avatar
Jeroen Vreeken committed
65
66
67
68
	{ "atsamx70_gmac", "packet" }
	{ "packet_out_float_be",  "out_position", "packet", 0 }
	{ "packet_out_float_be",  "out_stick",    "packet", 4 }
	{ "packet_out_float_be",  "out_speed",    "packet", 8 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
69
70
71
72
73
	{ "packet_out_float_be",  "out_speedff",    "packet", 12 }
	{ "packet_out_float_be",  "out_positionff",    "packet", 16 }
	{ "packet_out_float_be",  "out_pid", "packet", 20 }
	{ "packet_out_float_be",  "out_motor_I",  "packet", 24 }
	{ "packet_out_bool_byte", "out_homed",    "packet", 28 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
74
75
76
}

alias {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
77
	{ "position",    "position_offset", "out" }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
78
79
80
	{ "motor_speed", "counter1", "speed"    }
	{ "error",       "error", "difference"  }
	{ "setpoint",    "spg", "x"             }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
81
82
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
83
set w_nom    rpm2rads(1800.0)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
84
85
86
set V_nom    24.0
set I_nom    28.0
set I_peak   95.0
Jeroen Vreeken's avatar
Jeroen Vreeken committed
87
set t_peak   0.1    #???
Jeroen Vreeken's avatar
Jeroen Vreeken committed
88
89
90
91
92
93
set R_peak   $(V_nom)/$(I_peak)

set V_supply 24.0 #27.6?

set gear_ratio 100.0 * 19.33

Jeroen Vreeken's avatar
Jeroen Vreeken committed
94
set max_speed $(w_nom)/$(gear_ratio)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
95
96
set max_accel $(max_speed)*2.0
set max_jerk  $(max_accel)*2.0
Jeroen Vreeken's avatar
Jeroen Vreeken committed
97

Jeroen Vreeken's avatar
Jeroen Vreeken committed
98
links {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
99
100
	{ "button0", "value",      "button_v", "input",      true }
	{ "button1", "value",      "button_x", "input",      true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
101

Jeroen Vreeken's avatar
Jeroen Vreeken committed
102
	{ "button_x", "output",    "track_x_cmd", "a",       true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
103
	{ "counter0", "homed",     "track_x_cmd", "b",       true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
104
105
	{ "counter0", "position",  "position_offset", "in0", true }
	{ "position_calibrate", "value", "position_offset", "in1", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
107
	{ "false", "value",        "counter0", "reset",      true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
108
109
	{ "false", "value",        "counter1", "reset",      true }

110
111
112
113
	{ "afec0", "value",           "joystick_scale", "in",    true }
	{ "joystick_scale", "out",    "joystick_deadzone", "in", true }
	{ "joystick_deadzone", "out", "joystick2v", "in",        true }
	{ "joystick_deadzone", "out", "joystick2p", "in",        true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
114

Jeroen Vreeken's avatar
Jeroen Vreeken committed
115
	{ $<position>,             "spg", "reset_x",         true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
116
117
	{ "joystick2v", "out",     "spg", "track_v",         true }
	{ "joystick2p", "out",     "spg", "track_x",         true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
118
	{ "track_x_cmd", "q",      "spg", "track_x_cmd",     true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
119
120
121
122
	{ "button_v", "output",    "spg", "track_v_cmd",     true }
	{ "spg_enable", "out",     "spg_reset", "input",     true }
	{ "spg_reset", "output",   "spg", "reset",           true }
	{ "counter0", "homed",     "spg_enable", "in",       true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
123
124
125
126
127
128
	{ "spg", "v",              "speed_ff", "in1",        true }

	{ $<setpoint>,             "error", "positive",      true }
	{ $<position>,             "error", "negative",      true }
	{ $<error>,                "error_deadzone", "in",   true }
	{ "error_deadzone", "out", "pid", "in",              true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
129
	{ "spg_enable", "out",     "pid", "enable",          true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
131
	{ "pid", "out",            "speed_ff", "in0",        true }
	{ "speed_ff", "out",       "w2V", "in",              true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
134
	{ "Vlim", "out",            "motor_model", "V",       false }
	{ $<motor_speed>,          "motor_model", "w",       true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
135
136
	{ "motor_model", "I",      "motor_i2t", "I",         true }
	{ "motor_i2t", "Ilim",     "Ilim2Vlim", "in",        true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
137
138
	{ "Ilim2Vlim", "out",      "Vlim_supply", "in",      true }
	{ "Vlim_supply", "out",    "Vlim", "limit",          true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
139
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
140
141
142
143
	{ "w2V", "out",            "Vlim", "in",             true }
	{ "Vlim", "out",           "V2pwm", "in",            true }
	{ "V2pwm", "out",          "pwm0", "in",             true }

Jeroen Vreeken's avatar
Jeroen Vreeken committed
144
145
146
147
148
149
150
151
152

	# Status leds
	{ "counter0", "homed",     "status1_LED", "value",   true }
	{ "motor_i2t", "limiting", "status2_LED", "value",   true }
	
	{ "blinky", "output",      "blinky", "input",      false }
	{ "blinky", "output",      "blinky_LED", "value",  true }
	{ "blinky", "output",      "status0_LED", "value", true }

Jeroen Vreeken's avatar
Jeroen Vreeken committed
153
154
	{ "button_v", "output",      "status3_LED", "value", true }
	{ "button_x", "output",      "status4_LED", "value", true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
155
156
157
158
	{ "button2", "value",        "status5_LED", "value", true }


	# Packet output
Jeroen Vreeken's avatar
Jeroen Vreeken committed
159
160
161
162
163
164
165
166
	{ $<position>,                "out_position", "value",   true }
	{ "joystick_deadzone", "out", "out_stick", "value",      true }
	{ $<motor_speed>,             "out_speed", "value",      true }
	{ "counter0", "homed",        "out_homed", "value",      true }
	{ "spg", "v",                 "out_speedff", "value",    true }
	{ "spg", "x",                 "out_positionff", "value", true }
	{ "pid", "out",               "out_pid", "value",        true }
	{ "motor_model", "I",         "out_motor_I", "value",    true }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
167
168
169
}

params {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
170
171
	{ "w2V",        "gain",  (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
	{ "V2pwm",      "gain",  (float)1.0/$(V_supply) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
172
173
174
175
	{ "joystick2v", "gain",  (float)$(max_speed) }
	{ "joystick2p", "gain",  (float)deg2rad(45.0) }
	
	{ "position_calibrate", "value", 0.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
176
177
178
179
180

# GNM8055/4
# 24V 28A 490W
# zul. Spitzenstrom 95A
# S3 25%  I.CI F
Jeroen Vreeken's avatar
Jeroen Vreeken committed
181
#
Jeroen Vreeken's avatar
Jeroen Vreeken committed
182
183
184
185
186
187
188
189
190
191
192
193
# 24/95 --> R=0.25263158 (max R)
# 672W - 490W = 182W --> R=0.23214286 (6.5V) (17.5V emf)
# 490W @1800rpm --> T = 2.6Nm
# kt = 2.6/28 = 0.092857143
	{ "motor_model", "kt", (float)0.092857143 }
	{ "motor_model", "ke", (float)0.092857143 }
	{ "motor_model", "R",  (float)0.23214286  }
	
	{ "motor_i2t", "continuous", (float)$(I_nom)  }
	{ "motor_i2t", "peak",       (float)$(I_peak) }
	{ "motor_i2t", "duration",   (float)$(t_peak) }

Jeroen Vreeken's avatar
Jeroen Vreeken committed
194
195
	{ "Vlim_supply", "min", (float)-$(V_supply) }
	{ "Vlim_supply", "max", (float) $(V_supply) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
196
197
198
199
	{ "Ilim2Vlim", "gain", (float)$(R_peak) }

	{ "joystick_deadzone", "deadzone", (float)0.1 }
	{ "joystick_deadzone", "intercept", (float)1.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
200

Jeroen Vreeken's avatar
Jeroen Vreeken committed
201
202
	{ "spg", "max_x", (float)deg2rad( 43.0) }
	{ "spg", "min_x", (float)deg2rad(-28.0) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
203
204
205
	{ "spg", "max_v", (float)$(max_speed)   }
	{ "spg", "max_a", (float)$(max_accel)   }
	{ "spg", "max_j", (float)$(max_jerk)    }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
206
207
208
209
	{ "spg", "softreset", true }
	{ "spg", "precision_x", (float)deg2rad(0.1) }
	{ "spg", "precision_v", 0.0005 }
	{ "spg", "precision_a", $(max_jerk) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
210
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
211
212
	{ "error_deadzone", "deadzone",   (float)deg2rad(0.1)  }
#	{ "error_deadzone", "intercept",  (float)deg2rad(45.0) }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
213
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
214
215
	{ "pid", "kp", (float)$(max_speed) * 20.0 }
	{ "pid", "ki", (float)$(max_speed) * 0.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
216
217
218
	{ "pid", "kd", (float)0.0 }
	{ "pid", "maxw", (float)$(max_speed)/10.0 }
	{ "pid", "minw", (float)$(max_speed)/-10.0 }
Jeroen Vreeken's avatar
Jeroen Vreeken committed
219
220
}