From b91001ba48e2a62cc31ac5dda400b56cee4abd5f Mon Sep 17 00:00:00 2001 From: crinq Date: Wed, 1 Jul 2015 01:29:30 +0200 Subject: [PATCH] rexroth motsim --- graphviz/test.hal | 310 ++++++++++++++++++++++++------------------ src/comps/curpid.comp | 70 ++++++++++ src/link.c | 7 +- src/main.c | 69 +++++++--- 4 files changed, 301 insertions(+), 155 deletions(-) create mode 100644 src/comps/curpid.comp diff --git a/graphviz/test.hal b/graphviz/test.hal index adcc34d5..db6d0b2e 100644 --- a/graphviz/test.hal +++ b/graphviz/test.hal @@ -1,107 +1,121 @@ -encm0.calc_time <= encm0.calc_time = 0.000001 -encm0.enable <= encm0.enable = 1.000000 -encm0.pos <= encm0.pos = -1.319751 -encm0.error <= encm0.error = 7.000000 -sim0.calc_time <= sim0.calc_time = 0.000005 -sim0.amp <= sim0.amp = 1.000000 -sim0.freq <= sim0.freq = 6.283185 -sim0.sin <= sim0.sin = 0.295820 -sim0.msin <= sim0.msin = 0.307800 -sim0.square <= sim0.square = 0.999970 -sim0.vel <= sim0.vel = 0.338030 +adc0.calc_time <= adc0.calc_time = 0.000012 +adc0.sin <= adc0.sin = 1.034309 +adc0.cos <= adc0.cos = -0.032400 +adc0.sin3 <= adc0.sin3 = 1.033684 +adc0.cos3 <= adc0.cos3 = -0.031800 +adc0.res_en <= adc0.res_en = 0.000000 +adc0.sin_gain <= conf0.sin_gain = 0.000151 +adc0.cos_gain <= conf0.cos_gain = 0.000150 +adc0.sin_offset <= conf0.sin_offset = -17600.000007 +adc0.cos_offset <= conf0.cos_offset = -17661.000087 +enc10.calc_time <= enc10.calc_time = 0.000005 +enc10.res <= conf0.fb_res = 512.000000 +enc10.ires <= enc10.ires = 1024.000000 +enc10.count <= enc10.count = 6.000000 +enc10.pos <= enc10.pos = 0.073631 +enc10.ipos <= enc10.ipos = 0.067746 +enc10.sin <= adc0.sin = 1.033419 +enc10.cos <= adc0.cos = -0.031893 +sim0.calc_time <= sim0.calc_time = 0.000003 +sim0.amp <= sim0.amp = 3.000000 +sim0.freq <= sim0.freq = 0.000000 +sim0.sin <= sim0.sin = 0.000000 +sim0.msin <= sim0.msin = 0.000000 +sim0.square <= sim0.square = 2.999940 +sim0.vel <= sim0.vel = 0.000000 sim0.res <= sim0.res = 100000.000077 rev0.calc_time <= rev0.calc_time = 0.000000 -rev0.in <= rev0.in = 0.000000 +rev0.in <= sim0.vel = 0.000000 rev0.out <= rev0.out = 0.000000 rev0.rev <= conf0.cmd_rev = 0.000000 rev1.calc_time <= rev1.calc_time = 0.000000 -rev1.in <= encm0.pos = -1.319751 -rev1.out <= rev1.out = -1.319751 -rev1.rev <= conf0.fb_rev = 0.000000 -cauto0.calc_time <= cauto0.calc_time = 0.000005 +rev1.in <= enc10.ipos = 0.067740 +rev1.out <= rev1.out = -0.067746 +rev1.rev <= conf0.fb_rev = 1.000000 +cauto0.calc_time <= cauto0.calc_time = 0.000002 cauto0.i_q <= cauto0.i_q = 0.000000 cauto0.i_d <= cauto0.i_d = 0.000000 -cauto0.magpos <= cauto0.magpos = 0.000000 +cauto0.magpos <= cauto0.magpos = -0.000053 cauto0.start <= cauto0.start = 1.000000 cauto0.ready <= cauto0.ready = 1.000000 cauto0.start_offset <= cauto0.start_offset = 0.000000 -cauto0.offset <= cauto0.offset = -1.319751 -cauto0.fb_in <= rev1.out = -1.319751 -cauto0.fb_out <= cauto0.fb_out = 0.000000 -cauto0.pole_count <= conf0.pole_count = 2.000000 +cauto0.offset <= cauto0.offset = -0.067746 +cauto0.fb_in <= rev1.out = -0.067740 +cauto0.fb_out <= cauto0.fb_out = 0.000005 +cauto0.pole_count <= conf0.pole_count = 3.000000 cauto0.cur <= cauto0.cur = 1.000000 cauto0.time <= cauto0.time = 0.500000 -pderiv0.calc_time <= pderiv0.calc_time = 0.000004 +pderiv0.calc_time <= pderiv0.calc_time = 0.000003 pderiv0.in <= net0.cmd = 0.000000 pderiv0.in_lp <= pderiv0.in_lp = 1.000000 pderiv0.out <= pderiv0.out = 0.000000 pderiv0.out_lp <= pderiv0.out_lp = 1.000000 -pderiv0.vel_max <= conf0.max_vel = 837.758006 -pderiv0.acc_max <= conf0.max_acc = 418879.008777 +pderiv0.vel_max <= conf0.max_vel = 942.477860 +pderiv0.acc_max <= conf0.max_acc = 18849.556207 pderiv0.res <= pderiv0.res = 100000.000077 -pderiv1.calc_time <= pderiv1.calc_time = 0.000004 -pderiv1.in <= net0.fb = 0.000000 +pderiv1.calc_time <= pderiv1.calc_time = 0.000003 +pderiv1.in <= net0.fb = 0.000011 pderiv1.in_lp <= pderiv1.in_lp = 1.000000 -pderiv1.out <= pderiv1.out = 0.000000 +pderiv1.out <= pderiv1.out = -0.028610 pderiv1.out_lp <= pderiv1.out_lp = 1.000000 -pderiv1.vel_max <= conf0.max_vel = 837.758006 -pderiv1.acc_max <= conf0.max_acc = 418879.008777 -pderiv1.res <= conf0.fb_res = 16384.000007 -pid0.calc_time <= pid0.calc_time = 0.000021 +pderiv1.vel_max <= conf0.max_vel = 942.477860 +pderiv1.acc_max <= conf0.max_acc = 18849.556207 +pderiv1.res <= pderiv1.res = 524288.000777 +pid0.calc_time <= pid0.calc_time = 0.000015 pid0.pos_ext_cmd <= net0.cmd = 0.000000 -pid0.pos_fb <= net0.fb = 0.000000 -pid0.pos_error <= pid0.pos_error = 0.000000 +pid0.pos_fb <= net0.fb = 0.000011 +pid0.pos_error <= pid0.pos_error = -0.000005 pid0.vel_ext_cmd <= net0.cmd_d = 0.000000 -pid0.vel_fb <= net0.fb_d = 0.000000 -pid0.vel_cmd <= pid0.vel_cmd = 0.000000 -pid0.vel_error <= pid0.vel_error = 0.000000 +pid0.vel_fb <= net0.fb_d = -0.028610 +pid0.vel_cmd <= pid0.vel_cmd = -0.000095 +pid0.vel_error <= pid0.vel_error = 0.000013 pid0.acc_ext_cmd <= pid0.acc_ext_cmd = 0.000000 -pid0.acc_cmd <= pid0.acc_cmd = 0.000000 +pid0.acc_cmd <= pid0.acc_cmd = -124.725018 pid0.force_ext_cmd <= pid0.force_ext_cmd = 0.000000 -pid0.force_cmd <= pid0.force_cmd = -0.087823 +pid0.force_cmd <= pid0.force_cmd = 0.018561 pid0.cur_ext_cmd <= cauto0.i_q = 0.000000 -pid0.cur_cmd <= pid0.cur_cmd = -0.396979 +pid0.cur_cmd <= pid0.cur_cmd = 0.064262 pid0.enable <= cauto0.ready = 1.000000 pid0.pos_en <= pid0.pos_en = 1.000000 pid0.vel_en <= pid0.vel_en = 1.000000 pid0.acc_en <= pid0.acc_en = 1.000000 pid0.force_en <= cauto0.ready = 1.000000 pid0.cur_en <= pid0.cur_en = 1.000000 -pid0.mot_r <= conf0.r = 1.500000 -pid0.mot_j <= conf0.j = 0.000500 -pid0.mot_km <= conf0.km = 0.220000 +pid0.mot_r <= conf0.r = 7.500000 +pid0.mot_j <= conf0.j = 0.000013 +pid0.mot_km <= conf0.km = 0.289999 pid0.mot_fr <= pid0.mot_fr = 30.000000 pid0.mot_fd <= pid0.mot_fd = 30.000000 pid0.mot_fl <= pid0.mot_fl = 30.000000 -pid0.pos_p <= conf0.pos_p = 50.000000 +pid0.pos_p <= pid0.pos_p = 85.000000 pid0.pos_lp <= pid0.pos_lp = 4000.000007 pid0.vel_p <= pid0.vel_p = 1.000000 pid0.vel_lp <= pid0.vel_lp = 4000.000007 pid0.vel_ff <= pid0.vel_ff = 1.000000 -pid0.acc_p <= conf0.acc_p = 0.150000 +pid0.acc_p <= pid0.acc_p = 0.029999 pid0.acc_lp <= pid0.acc_lp = 4000.000007 -pid0.acc_pi <= conf0.acc_pi = 50.000000 +pid0.acc_pi <= pid0.acc_pi = 50.000000 pid0.acc_ff <= pid0.acc_ff = 1.000000 pid0.force_p <= pid0.force_p = 1.000000 pid0.force_lp <= pid0.force_lp = 4000.000007 pid0.force_ff <= pid0.force_ff = 1.000000 pid0.cur_ff <= pid0.cur_ff = 1.000000 -pid0.volt <= net0.vlt = 0.417564 -pid0.induction <= pid0.induction = 0.000000 -pid0.max_vel <= conf0.max_vel = 837.758006 -pid0.max_acc <= conf0.max_acc = 418879.008777 -pid0.max_force <= conf0.max_force = 2.000000 -pid0.max_cur <= conf0.max_cur = 6.000000 +pid0.volt <= net0.vlt = 48.541840 +pid0.induction <= pid0.induction = 0.008296 +pid0.max_vel <= conf0.max_vel = 942.477860 +pid0.max_acc <= conf0.max_acc = 18849.556207 +pid0.max_force <= conf0.max_force = 1.800000 +pid0.max_cur <= conf0.max_cur = 6.800000 pid0.max_volt <= pid0.max_volt = 400.000000 pid0.max_pwm <= pid0.max_pwm = 0.950000 -pid0.vel_min <= pid0.vel_min = -1.898018 -pid0.vel_max <= pid0.vel_max = 1.898018 -pid0.acc_min <= pid0.acc_min = -232.722426 -pid0.acc_max <= pid0.acc_max = 232.722426 -pid0.force_min <= pid0.force_min = -0.116361 -pid0.force_max <= pid0.force_max = 0.116361 -pid0.cur_min <= pid0.cur_min = -0.528914 -pid0.cur_max <= pid0.cur_max = 0.528914 +pid0.vel_min <= pid0.vel_min = -167.385642 +pid0.vel_max <= pid0.vel_max = 167.385642 +pid0.acc_min <= pid0.acc_min = -18849.556207 +pid0.acc_max <= pid0.acc_max = 18849.556207 +pid0.force_min <= pid0.force_min = -1.800000 +pid0.force_max <= pid0.force_max = 1.800000 +pid0.cur_min <= pid0.cur_min = -6.800000 +pid0.cur_max <= pid0.cur_max = 6.800000 pid0.volt_min <= pid0.volt_min = 0.000000 pid0.volt_max <= pid0.volt_max = 0.000000 pid0.pwm_min <= pid0.pwm_min = 0.000000 @@ -114,68 +128,95 @@ pid0.saturated <= pid0.saturated = 0.000000 rev2.calc_time <= rev2.calc_time = 0.000000 rev2.in <= cauto0.magpos = 0.000000 rev2.out <= rev2.out = 0.000000 -rev2.rev <= conf0.out_rev = 1.000000 -cur0.calc_time <= cur0.calc_time = 0.000015 -cur0.id <= cauto0.i_d = 0.000000 -cur0.iq <= pid0.cur_cmd = -0.396685 -cur0.ia <= cur0.ia = 0.000000 -cur0.ib <= cur0.ib = -0.396685 -cur0.iu <= cur0.iu = 0.000000 -cur0.iv <= cur0.iv = -0.343540 -cur0.iw <= cur0.iw = 0.343554 -cur0.i_u <= cur0.i_u = -0.000000 -cur0.i_v <= cur0.i_v = -0.264454 -cur0.i_w <= cur0.i_w = 0.264456 -cur0.u <= cur0.u = 0.155678 -cur0.v <= cur0.v = 0.011650 -cur0.w <= cur0.w = 0.405914 -cur0.magpos <= rev2.out = 0.000000 -cur0.r <= conf0.r = 1.500000 -cur0.l <= conf0.l = 0.002499 -cur0.freq <= cur0.freq = 5000.000007 -cur0.volt <= net0.vlt = 0.417564 -cur0.cor_volt <= cur0.cor_volt = 0.002421 -cur0.induction <= pid0.induction = 0.000000 -cur0.ind_p <= cur0.ind_p = -1.000000 -cur0.pwm_max <= cur0.pwm_max = 0.950000 -cur0.cur_max <= conf0.max_cur = 6.000000 -cur0.p <= cur0.p = 3.125000 -cur0.ff <= cur0.ff = 1.000000 -cur0.lp <= conf0.cur_lp = 0.500000 -cur0.cor_p <= cur0.cor_p = 1.000000 +rev2.rev <= conf0.out_rev = 0.000000 +curpid0.calc_time <= curpid0.calc_time = 0.000004 +curpid0.id_cmd <= curpid0.id_cmd = 0.000000 +curpid0.iq_cmd <= pid0.cur_cmd = 0.064019 +curpid0.id_fb <= pmsm0.id = -0.000000 +curpid0.iq_fb <= curpid0.iq_fb = 0.000000 +curpid0.ud <= curpid0.ud = -0.000001 +curpid0.uq <= curpid0.uq = 0.607253 +curpid0.volt <= net0.vlt = 48.541840 +curpid0.rd <= conf0.r = 7.500000 +curpid0.rq <= conf0.r = 7.500000 +curpid0.ld <= conf0.l = 0.008000 +curpid0.lq <= conf0.l = 0.008000 +curpid0.ff <= curpid0.ff = 1.000000 +curpid0.kp <= curpid0.kp = 0.050000 +curpid0.ki <= curpid0.ki = 0.000000 +curpid0.id_error <= curpid0.id_error = -0.000000 +curpid0.iq_error <= curpid0.iq_error = 0.064117 +pmsm0.calc_time <= pmsm0.calc_time = 0.000002 +pmsm0.km <= conf0.km = 0.289999 +pmsm0.r <= conf0.r = 7.500000 +pmsm0.ld <= conf0.l = 0.008000 +pmsm0.lq <= conf0.l = 0.008000 +pmsm0.polecount <= pmsm0.polecount = 1.000000 +pmsm0.vel <= net0.fb_d = 0.000000 +pmsm0.ud <= curpid0.ud = -0.000001 +pmsm0.uq <= curpid0.uq = 0.607538 +pmsm0.id <= pmsm0.id = 0.000000 +pmsm0.iq <= pmsm0.iq = 0.080987 +pmsm0.psi_d <= pmsm0.psi_d = 0.289999 +pmsm0.psi_q <= pmsm0.psi_q = 0.000650 +pmsm0.torque <= pmsm0.torque = 0.035169 +mot0.calc_time <= mot0.calc_time = 0.000003 +mot0.torque <= pmsm0.torque = 0.035420 +mot0.mr <= mot0.mr = 0.001000 +mot0.md <= mot0.md = 0.001000 +mot0.ml <= mot0.ml = 0.000000 +mot0.j <= conf0.j = 0.000013 +mot0.real_pos <= net0.fb = 0.000000 +mot0.kp <= mot0.kp = 1.000000 +mot0.ki <= mot0.ki = 1.000000 +mot0.acc <= mot0.acc = -14.667976 +mot0.vel <= mot0.vel = 33.804880 +mot0.pos <= mot0.pos = 40.082120 +mot0.pos_error <= mot0.pos_error = -33.798924 +mot0.jerk <= mot0.jerk = -0.797621 +idq0.calc_time <= idq0.calc_time = 0.000006 +idq0.ud <= curpid0.ud = 0.000000 +idq0.uq <= curpid0.uq = 0.607578 +idq0.pos <= cauto0.fb_out = 0.000005 +idq0.polecount <= conf0.pole_count = 3.000000 +idq0.u <= idq0.u = 24.270898 +idq0.v <= idq0.v = 24.796202 +idq0.w <= idq0.w = 23.743232 +idq0.svm <= idq0.svm = 0.000000 +idq0.volt <= net0.vlt = 48.541840 pwm2uart0.calc_time <= pwm2uart0.calc_time = 0.000004 -pwm2uart0.u <= cur0.u = 0.209993 -pwm2uart0.v <= cur0.v = 0.011650 -pwm2uart0.w <= cur0.w = 0.405914 -pwm2uart0.uout <= pwm2uart0.uout = 603.000000 -pwm2uart0.vout <= pwm2uart0.vout = 33.000000 -pwm2uart0.wout <= pwm2uart0.wout = 1080.000000 +pwm2uart0.u <= idq0.u = 24.270908 +pwm2uart0.v <= idq0.v = 24.796298 +pwm2uart0.w <= idq0.w = 23.745644 +pwm2uart0.uout <= pwm2uart0.uout = 600.000000 +pwm2uart0.vout <= pwm2uart0.vout = 612.000000 +pwm2uart0.wout <= pwm2uart0.wout = 586.000000 pwm2uart0.enable <= pwm2uart0.enable = 0.900000 -pwm2uart0.volt <= net0.vlt = 0.417564 +pwm2uart0.volt <= net0.vlt = 48.541840 pwm2uart0.pwm_max <= pwm2uart0.pwm_max = 0.900000 avg0.calc_time <= avg0.calc_time = 0.000000 -avg0.in <= pid0.pos_error = 0.000000 +avg0.in <= pid0.pos_error = 0.000005 avg0.ac <= avg0.ac = 0.000099 -avg0.out <= avg0.out = -0.000000 -avg0.absout <= avg0.absout = 0.000001 -term0.calc_time <= term0.calc_time = 0.000001 +avg0.out <= avg0.out = 0.000000 +avg0.absout <= avg0.absout = 0.000006 +term0.calc_time <= term0.calc_time = 0.000000 term0.wave0 <= net0.cmd = 0.000000 -term0.gain0 <= term0.gain0 = 10.000000 +term0.gain0 <= term0.gain0 = 20.000000 term0.offset0 <= term0.offset0 = 0.000000 term0.wave1 <= net0.fb = 0.000000 -term0.gain1 <= term0.gain1 = 10.000000 +term0.gain1 <= term0.gain1 = 20.000000 term0.offset1 <= term0.offset1 = 0.000000 -term0.wave2 <= net0.cmd_d = 0.000000 -term0.gain2 <= term0.gain2 = 1.000000 +term0.wave2 <= net0.fb_d = 0.000000 +term0.gain2 <= term0.gain2 = 0.100000 term0.offset2 <= term0.offset2 = 0.000000 -term0.wave3 <= net0.fb_d = 0.000000 -term0.gain3 <= term0.gain3 = 1.000000 +term0.wave3 <= enc10.pos = 0.073631 +term0.gain3 <= term0.gain3 = 500.000000 term0.offset3 <= term0.offset3 = 0.000000 -term0.wave4 <= term0.wave4 = 0.000000 -term0.gain4 <= term0.gain4 = 10.000000 +term0.wave4 <= pid0.pos_error = 0.000005 +term0.gain4 <= term0.gain4 = 5000.000007 term0.offset4 <= term0.offset4 = 0.000000 -term0.wave5 <= term0.wave5 = 0.000000 -term0.gain5 <= term0.gain5 = 10.000000 +term0.wave5 <= enc10.pos = 0.073631 +term0.gain5 <= term0.gain5 = 200.000000 term0.offset5 <= term0.offset5 = 0.000000 term0.wave6 <= term0.wave6 = 0.000000 term0.gain6 <= term0.gain6 = 10.000000 @@ -190,36 +231,37 @@ led0.r <= cauto0.start = 1.000000 led0.g <= cauto0.ready = 1.000000 net0.enable <= net0.enable = 1.000000 net0.cmd <= rev0.out = 0.000000 -net0.fb <= cauto0.fb_out = 0.000000 +net0.fb <= cauto0.fb_out = -0.000005 net0.cmd_d <= pderiv0.out = 0.000000 net0.fb_d <= pderiv1.out = 0.000000 -net0.amp <= net0.amp = 0.071397 -net0.vlt <= net0.vlt = 0.417564 -net0.tmp <= net0.tmp = 22.186250 -net0.rt_calc_time <= net0.rt_calc_time = 0.000078 -conf0.r <= conf0.r = 1.500000 -conf0.l <= conf0.l = 0.002499 -conf0.j <= conf0.j = 0.000500 -conf0.km <= conf0.km = 0.220000 -conf0.pole_count <= conf0.pole_count = 2.000000 +net0.amp <= net0.amp = 0.026084 +net0.vlt <= net0.vlt = 48.541840 +net0.tmp <= net0.tmp = 37.432736 +net0.rt_calc_time <= net0.rt_calc_time = 0.000084 +conf0.r <= conf0.r = 7.500000 +conf0.l <= conf0.l = 0.008000 +conf0.j <= conf0.j = 0.000013 +conf0.km <= conf0.km = 0.289999 +conf0.pole_count <= conf0.pole_count = 3.000000 conf0.fb_pole_count <= conf0.fb_pole_count = 0.000000 conf0.fb_offset <= conf0.fb_offset = 0.000000 -conf0.pos_p <= conf0.pos_p = 50.000000 -conf0.acc_p <= conf0.acc_p = 0.150000 +conf0.pos_p <= conf0.pos_p = 80.000000 +conf0.acc_p <= conf0.acc_p = 0.050000 conf0.acc_pi <= conf0.acc_pi = 50.000000 -conf0.cur_lp <= conf0.cur_lp = 0.500000 -conf0.max_vel <= conf0.max_vel = 837.758006 -conf0.max_acc <= conf0.max_acc = 418879.008777 -conf0.max_force <= conf0.max_force = 2.000000 -conf0.max_cur <= conf0.max_cur = 6.000000 -conf0.fb_type <= conf0.fb_type = 0.000000 -conf0.cmd_type <= conf0.cmd_type = 1.000000 -conf0.fb_rev <= conf0.fb_rev = 0.000000 +conf0.cur_p <= conf0.cur_p = 0.000000 +conf0.cur_ff <= conf0.cur_ff = 1.000000 +conf0.max_vel <= conf0.max_vel = 942.477860 +conf0.max_acc <= conf0.max_acc = 18849.556207 +conf0.max_force <= conf0.max_force = 1.800000 +conf0.max_cur <= conf0.max_cur = 6.800000 +conf0.fb_type <= conf0.fb_type = 6.000000 +conf0.cmd_type <= conf0.cmd_type = 0.000000 +conf0.fb_rev <= conf0.fb_rev = 1.000000 conf0.cmd_rev <= conf0.cmd_rev = 0.000000 -conf0.out_rev <= conf0.out_rev = 1.000000 -conf0.fb_res <= conf0.fb_res = 16384.000007 +conf0.out_rev <= conf0.out_rev = 0.000000 +conf0.fb_res <= conf0.fb_res = 512.000000 conf0.cmd_res <= conf0.cmd_res = 2000.000000 -conf0.sin_offset <= conf0.sin_offset = -17371.000027 +conf0.sin_offset <= conf0.sin_offset = -17600.000007 conf0.cos_offset <= conf0.cos_offset = -17661.000087 -conf0.sin_gain <= conf0.sin_gain = 0.000099 -conf0.cos_gain <= conf0.cos_gain = 0.000099 +conf0.sin_gain <= conf0.sin_gain = 0.000151 +conf0.cos_gain <= conf0.cos_gain = 0.000150 diff --git a/src/comps/curpid.comp b/src/comps/curpid.comp new file mode 100644 index 00000000..91b04f9f --- /dev/null +++ b/src/comps/curpid.comp @@ -0,0 +1,70 @@ +COMP(curpid); + +HAL_PIN(id_cmd) = 0.0; +HAL_PIN(iq_cmd) = 0.0; + +HAL_PIN(id_fb) = 0.0; +HAL_PIN(iq_fb) = 0.0; + +HAL_PIN(ud) = 0.0; +HAL_PIN(uq) = 0.0; + +HAL_PIN(volt) = 0.0; + +HAL_PIN(rd) = 0.0; +HAL_PIN(rq) = 0.0; +HAL_PIN(ld) = 0.0; +HAL_PIN(lq) = 0.0; + +HAL_PIN(ff) = 0.0; +HAL_PIN(kp) = 0.0; +HAL_PIN(ki) = 0.0; + +HAL_PIN(id_error) = 0.0; +HAL_PIN(iq_error) = 0.0; + +MEM(float id_error_sum) = 0.0; +MEM(float iq_error_sum) = 0.0; + +RT( + float rd = MAX(PIN(rd), 0.1); + float rq = MAX(PIN(rq), 0.1); + float ld = MAX(PIN(ld), 0.001); + float lq = MAX(PIN(ld), 0.001); + float volt = MAX(PIN(volt), 0.1); + + float ff = PIN(ff); + float kpd = ld * PIN(kp) / period; + float kpq = lq * PIN(kp) / period; + float kid = rd * PIN(ki) / period; + float kiq = rq * PIN(ki) / period; + + float idc = PIN(id_cmd); + float iqc = PIN(iq_cmd); + + float id = PIN(id_fb); + float iq = PIN(id_fb); + + float id_error = (idc - id); + float iq_error = (iqc - iq); + + if(kid > 0.0){ + id_error_sum += id_error; + iq_error_sum += iq_error; + } + else{ + id_error_sum = 0.0; + iq_error_sum = 0.0; + } + + float ud = CLAMP(ff * rd * idc + kpd * id_error + kid * id_error_sum, -volt / 2.0, volt / 2.0); + float uq = CLAMP(ff * rq * iqc + kpq * iq_error + kiq * iq_error_sum, -volt / 2.0, volt / 2.0); + + PIN(ud) = ud; + PIN(uq) = uq; + + PIN(id_error) = id_error; + PIN(iq_error) = iq_error; +); + +ENDCOMP; diff --git a/src/link.c b/src/link.c index 5195bd89..a5501f83 100644 --- a/src/link.c +++ b/src/link.c @@ -186,10 +186,11 @@ void set_rexroth(){ set_hal_pin("conf0.j", 0.000013);//data set_hal_pin("conf0.km", 0.29);//data - set_hal_pin("conf0.pos_p", 80.0); - set_hal_pin("conf0.acc_p", 0.15); + set_hal_pin("conf0.pos_p", 85.0); + set_hal_pin("conf0.acc_p", 0.03); set_hal_pin("conf0.acc_pi", 50.0); - set_hal_pin("conf0.cur_lp", 0.4); + set_hal_pin("conf0.cur_p", 0.05); + set_hal_pin("conf0.cur_ff", 1.0); set_hal_pin("enc10.res", 512.0); set_fb_sincos(); } diff --git a/src/main.c b/src/main.c index 222980d2..55c0087b 100644 --- a/src/main.c +++ b/src/main.c @@ -134,9 +134,9 @@ int main(void) setup(); #include "comps/adc.comp" - //#include "comps/enc1.comp" + #include "comps/enc1.comp" //#include "comps/res.comp" - #include "comps/encm.comp" + //#include "comps/encm.comp" #include "comps/sim.comp" //#include "comps/mux.comp" @@ -155,7 +155,10 @@ int main(void) #include "comps/rev.comp" - #include "comps/cur.comp" + #include "comps/curpid.comp" + #include "comps/pmsm.comp" + #include "comps/mot.comp" + #include "comps/idq.comp" #include "comps/pwm2uart.comp" @@ -197,7 +200,8 @@ int main(void) HAL_PIN(pos_p) = 0.0; HAL_PIN(acc_p) = 0.0; HAL_PIN(acc_pi) = 0.0; - HAL_PIN(cur_lp) = 0.0; + HAL_PIN(cur_p) = 0.0; + HAL_PIN(cur_ff) = 1.0; HAL_PIN(max_vel) = 0.0; HAL_PIN(max_acc) = 0.0; HAL_PIN(max_force) = 0.0; @@ -214,10 +218,10 @@ int main(void) HAL_PIN(sin_gain) = 0.0001515; HAL_PIN(cos_gain) = 0.00015; - g_amp = map_hal_pin("net0.amp"); - g_vlt = map_hal_pin("net0.vlt"); - g_tmp = map_hal_pin("net0.tmp"); - rt_time = map_hal_pin("net0.rt_calc_time"); + g_amp_hal_pin = map_hal_pin("net0.amp"); + g_vlt_hal_pin = map_hal_pin("net0.vlt"); + g_tmp_hal_pin = map_hal_pin("net0.tmp"); + rt_time_hal_pin = map_hal_pin("net0.rt_calc_time"); for(int i = 0; i < hal.init_func_count; i++){ hal.init[i](); @@ -227,25 +231,54 @@ int main(void) //set_e240(); //set_bergerlahr();//pid2: ok - set_mitsubishi();//pid2: ok + //set_mitsubishi();//pid2: ok //set_festo(); //set_manutec(); - //set_rexroth();//pid2: ok - //link_hal_pins("enc10.ipos", "rev1.in"); - - //set_hal_pin("res0.reverse", 0.0); + set_rexroth();//pid2: ok //set_bosch1();//pid2: ok //set_bosch4();//pid2: ok //set_sanyo();//pid2: ok //set_br(); - //set_hal_pin("enc0.iquad_en1", 1.0); - //set_hal_pin("enc0.quad_en1", 1.0); - //set_hal_pin("enc0.iquad_en1", 1.0); - - //link_hal_pins("pderiv1.out2", "net0.fb_d"); set_cmd_sin(); + link_hal_pins("cauto0.i_d", "curpid0.id_cmd"); + link_hal_pins("pid0.cur_cmd", "curpid0.iq_cmd"); + link_hal_pins("net0.vlt", "curpid0.volt"); + link_hal_pins("conf0.r", "curpid0.rd"); + link_hal_pins("conf0.r", "curpid0.rq"); + link_hal_pins("conf0.l", "curpid0.ld"); + link_hal_pins("conf0.l", "curpid0.lq"); + link_hal_pins("conf0.cur_ff", "curpid0.ff"); + link_hal_pins("conf0.cur_p", "curpid0.kp"); + + link_hal_pins("curpid0.ud", "pmsm0.ud"); + link_hal_pins("curpid0.uq", "pmsm0.uq"); + link_hal_pins("net0.fb_d", "pmsm0.vel"); + link_hal_pins("conf0.r", "pmsm0.r"); + link_hal_pins("conf0.l", "pmsm0.ld"); + link_hal_pins("conf0.l", "pmsm0.lq"); + link_hal_pins("conf0.km", "pmsm0.km"); + link_hal_pins("conf0.polecount", "pmsm0.polecount"); + link_hal_pins("pmsm0.id", "curpid0.id_fb"); + link_hal_pins("pmsm0.iq", "curpid0.id_fq"); + link_hal_pins("pmsm0.torque", "mot0.torque"); + link_hal_pins("conf0.j", "mot0.j"); + link_hal_pins("net0.fb", "mot0.real_pos"); + set_hal_pin("mot0.kp", 0.01); + link_hal_pins("curpid0.ud", "idq0.ud"); + link_hal_pins("curpid0.uq", "idq0.uq"); + link_hal_pins("cauto0.magpos", "idq0.pos"); + link_hal_pins("conf0.pole_count", "idq0.polecount"); + link_hal_pins("net0.vlt", "idq0.volt"); + link_hal_pins("idq0.u", "pwm2uart0.u"); + link_hal_pins("idq0.v", "pwm2uart0.v"); + link_hal_pins("idq0.w", "pwm2uart0.w"); + link_hal_pins("conf0.pole_count", "pmsm0.polecount"); + set_hal_pin("cauto0.pole_count", 1.0); + + + link_hal_pins("pid0.pos_error", "avg0.in"); set_hal_pin("avg0.ac", 0.0001);