You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

220 lines
8.0 KiB

# embedded ctrl
#
#
set frequency 100
set delay 0.0
trigger {
{ "systick" }
}
blocks (2.0, 0.0) {
{ "atsamx70_pio_out", "status0_LED", 0, 2 } #PA2
{ "atsamx70_pio_out", "blinky_LED", 2, 8 } #PC8
{ "not", "blinky" }
}
blocks ($(frequency), $(delay)) {
# Profiling block, may be commented out.
#{ "controller_profile", "profile" }
{ "atsamx70_tc", "counter0", 0, 2500, 0 }
{ "atsamx70_tc", "counter1", 1, 10000, 0, 1 }
{ "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
{ "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" }
{ "quadratic_sign", "joystick_scale" }
{ "deadzone", "joystick_deadzone" }
{ "gain", "joystick2v" }
{ "gain", "joystick2p" }
{ "motor_model_dc_v", "motor_model" }
{ "i2t", "motor_i2t" }
{ "gain", "Ilim2Vlim" }
{ "limit", "Vlim_supply" }
{ "limit_dyn", "Vlim" }
{ "not", "button_v" }
{ "not", "button_x" }
{ "add", "position_offset" }
{ "and2", "track_x_cmd" }
{ "not", "spg_reset" }
{ "delay_bool", "spg_enable" }
{ "setpoint_generator_3d", "spg", "spg", "rad" }
{ "subtract", "error" }
{ "deadzone", "error_deadzone" }
{ "pid_aw", "pid" }
{ "add", "speed_ff" }
{ "value_bool", "false" }
{ "value", "position_calibrate" }
{ "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 }
{ "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 }
}
alias {
{ "position", "position_offset", "out" }
{ "motor_speed", "counter1", "speed" }
{ "error", "error", "difference" }
{ "setpoint", "spg", "x" }
}
set w_nom rpm2rads(1800.0)
set V_nom 24.0
set I_nom 28.0
set I_peak 95.0
set t_peak 0.1 #???
set R_peak $(V_nom)/$(I_peak)
set V_supply 24.0 #27.6?
set gear_ratio 100.0 * 19.33
set max_speed $(w_nom)/$(gear_ratio)
set max_accel $(max_speed)*2.0
set max_jerk $(max_accel)*2.0
links {
{ "button0", "value", "button_v", "input", true }
{ "button1", "value", "button_x", "input", true }
{ "button_x", "output", "track_x_cmd", "a", true }
{ "counter0", "homed", "track_x_cmd", "b", true }
{ "counter0", "position", "position_offset", "in0", true }
{ "position_calibrate", "value", "position_offset", "in1", true }
{ "false", "value", "counter0", "reset", true }
{ "false", "value", "counter1", "reset", true }
{ "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 }
{ $<position>, "spg", "reset_x", true }
{ "joystick2v", "out", "spg", "track_v", true }
{ "joystick2p", "out", "spg", "track_x", true }
{ "track_x_cmd", "q", "spg", "track_x_cmd", true }
{ "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 }
{ "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 }
{ "spg_enable", "out", "pid", "enable", true }
{ "pid", "out", "speed_ff", "in0", true }
{ "speed_ff", "out", "w2V", "in", true }
{ "Vlim", "out", "motor_model", "V", false }
{ $<motor_speed>, "motor_model", "w", true }
{ "motor_model", "I", "motor_i2t", "I", true }
{ "motor_i2t", "Ilim", "Ilim2Vlim", "in", true }
{ "Ilim2Vlim", "out", "Vlim_supply", "in", true }
{ "Vlim_supply", "out", "Vlim", "limit", true }
{ "w2V", "out", "Vlim", "in", true }
{ "Vlim", "out", "V2pwm", "in", true }
{ "V2pwm", "out", "pwm0", "in", true }
# 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 }
{ "button_v", "output", "status3_LED", "value", true }
{ "button_x", "output", "status4_LED", "value", true }
{ "button2", "value", "status5_LED", "value", true }
# Packet output
{ $<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 }
}
params {
{ "w2V", "gain", (float)$(gear_ratio)*$(V_nom)/$(w_nom) }
{ "V2pwm", "gain", (float)1.0/$(V_supply) }
{ "joystick2v", "gain", (float)$(max_speed) }
{ "joystick2p", "gain", (float)deg2rad(45.0) }
{ "position_calibrate", "value", 0.0 }
# GNM8055/4
# 24V 28A 490W
# zul. Spitzenstrom 95A
# S3 25% I.CI F
#
# 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) }
{ "Vlim_supply", "min", (float)-$(V_supply) }
{ "Vlim_supply", "max", (float) $(V_supply) }
{ "Ilim2Vlim", "gain", (float)$(R_peak) }
{ "joystick_deadzone", "deadzone", (float)0.1 }
{ "joystick_deadzone", "intercept", (float)1.0 }
{ "spg", "max_x", (float)deg2rad( 43.0) }
{ "spg", "min_x", (float)deg2rad(-28.0) }
{ "spg", "max_v", (float)$(max_speed) }
{ "spg", "max_a", (float)$(max_accel) }
{ "spg", "max_j", (float)$(max_jerk) }
{ "spg", "softreset", true }
{ "spg", "precision_x", (float)deg2rad(0.1) }
{ "spg", "precision_v", 0.0005 }
{ "spg", "precision_a", $(max_jerk) }
{ "error_deadzone", "deadzone", (float)deg2rad(0.1) }
# { "error_deadzone", "intercept", (float)deg2rad(45.0) }
{ "pid", "kp", (float)$(max_speed) * 20.0 }
{ "pid", "ki", (float)$(max_speed) * 0.0 }
{ "pid", "kd", (float)0.0 }
{ "pid", "maxw", (float)$(max_speed)/10.0 }
{ "pid", "minw", (float)$(max_speed)/-10.0 }
}