diff --git a/Makefile b/Makefile index 83786078..98cbf925 100644 --- a/Makefile +++ b/Makefile @@ -109,6 +109,11 @@ COMPS += shared/comps/sensorless.c COMPS += shared/comps/field.c COMPS += shared/comps/gain.c COMPS += shared/comps/rlpsij.c +COMPS += shared/comps/veltime.c +COMPS += shared/comps/mpid.c +COMPS += shared/comps/fmove.c +COMPS += shared/comps/home.c +COMPS += shared/comps/en.c SOURCES += $(COMPS) diff --git a/conf/move.txt b/conf/move.txt new file mode 100644 index 00000000..f5fbc4e9 --- /dev/null +++ b/conf/move.txt @@ -0,0 +1,63 @@ +link pid +link pmsm +link enc_fb0 +enc_fb0.en_index = 0 +link jog_cmd +link misc +conf0.r = 2 +conf0.l = 0.007 +conf0.j = 0.000067 +conf0.j_load = 0.00005 +conf0.polecount = 5 +conf0.max_ac_cur = 5 +conf0.mot_fb_res = 20000 + +load hx +hx0.rt_prio = 9 +hx0.gain = 1 + +load en +en0.rt_prio = 2 +en0.en_in = 1 +fault0.en = en0.en_out0 +en0.fault = fault0.fault +en0.time = 3 +conf0.phase_cur = 3 +conf0.phase_time = 1 + +load home +home0.rt_prio = 9 +home0.en_in = en0.en_out1 +rev0.in = home0.pos_out +home0.home_in = io0.ind0n +home0.home_vel = -12 + +load fmove +fmove0.rt_prio = 10 +fmove0.en = home0.en_out +fmove0.force_in = hx0.out +home0.pos_in = fmove0.mpos +fmove0.max_usr_vel = 0.1 +fmove0.max_usr_acc = 0.1 +term0.wave0 = fmove0.mpos +hx0.gain = 60 + +fmove0.damping = 1 +fmove0.real_mass = 0 +fmove0.virtual_mass = 1 +fmove0.friction = 0.1 +fmove0.min_pos = 0 +fmove0.max_pos = 0.5 +fmove0.scale = 500 +fmove0.max_vel = 0.1 +fmove0.max_acc = 0.5 +fmove0.max_usr_vel = 0.25 +fmove0.max_usr_acc = 2 +fmove0.force_th = 0.2 +term0.send_step = 0 +fmove0.print_freq = 1 +fmove0.force_offset = -0.1129 +fmove0.force_offset_lpf = 0.0001 + +term0.send_step = 50 +fmove0.print_freq = 0 diff --git a/conf/template/enc_fb0.txt b/conf/template/enc_fb0.txt index 34b12efa..f65eab6f 100644 --- a/conf/template/enc_fb0.txt +++ b/conf/template/enc_fb0.txt @@ -13,3 +13,4 @@ io0.fb0y = enc_fb0.error io0.fb0g = 1 enc_fb0.vel = vel1.vel enc_fb0.amp = adc0.amp0 +enc_fb0.en_index = 1 \ No newline at end of file diff --git a/conf/template/enc_ws_fb0.txt b/conf/template/enc_ws_fb0.txt new file mode 100644 index 00000000..65145de9 --- /dev/null +++ b/conf/template/enc_ws_fb0.txt @@ -0,0 +1,30 @@ +load enc_fb +enc_fb0.rt_prio = 2 +enc_fb0.res = conf0.mot_fb_res +enc_fb0.sin = adc0.sin0l +enc_fb0.cos = adc0.cos0l +enc_fb0.quad = adc0.quad +fb_switch0.mot_pos = enc_fb0.pos +fb_switch0.mot_abs_pos = enc_fb0.abs_pos +fb_switch0.mot_polecount = 1 +fb_switch0.mot_state = enc_fb0.state +fault0.mot_fb_error = enc_fb0.error +io0.fb0y = enc_fb0.error +io0.fb0g = 1 +enc_fb0.vel = vel1.vel +enc_fb0.amp = adc0.amp0 + +load uvw +uvw0.rt_prio = 2 +uvw0.u = io0.fb0a +uvw0.v = io0.fb0b +uvw0.w = io0.fb0z +fb_switch0.com_pos = uvw0.pos +fb_switch0.com_abs_pos = uvw0.pos +fb_switch0.com_state = uvw0.state +fb_switch0.com_polecount = conf0.com_fb_polecount +conf0.com_fb_polecount = conf0.polecount +uvw0.amp = adc0.amp0 +fault0.com_fb_error = uvw0.error +enc_fb0.en_index = uvw0.state +uvw0.mode = 1 \ No newline at end of file diff --git a/conf/template/mpid.txt b/conf/template/mpid.txt new file mode 100644 index 00000000..2080448e --- /dev/null +++ b/conf/template/mpid.txt @@ -0,0 +1,118 @@ +load adc +load reslimit +load rev +load fb_switch +load veltime +load veltime +load vel +load mpid +load fault +load hv +load sim +load io +load iit +link conf +hv0.rt_prio = 0.9 +hv0.frt_prio = 1 +adc0.rt_prio = 1 +reslimit0.rt_prio = 3 +rev0.rt_prio = 4 +fb_switch0.rt_prio = 5 +veltime0.frt_prio = 6 +veltime1.frt_prio = 6 +vel0.rt_prio = 6 +io0.rt_prio = 7 +mpid0.rt_prio = 8 +fault0.rt_prio = 10 +iit0.rt_prio = 13 +sim0.rt_prio = 13 +term0.rt_prio = 16 +rev0.rev = conf0.cmd_rev +fb_switch0.mot_rev = conf0.mot_fb_rev +fb_switch0.com_rev = conf0.com_fb_rev +fb_switch0.joint_rev = conf0.joint_fb_rev +reslimit0.res = conf0.mot_fb_res +mpid0.j = conf0.j +mpid0.pos_p = conf0.pos_p +mpid0.vel_p = conf0.vel_p +mpid0.vel_i = conf0.vel_i +mpid0.max_vel = conf0.max_vel +mpid0.max_acc = conf0.max_acc +mpid0.max_torque = conf0.max_force +fault0.max_sat = conf0.max_sat +fault0.max_pos_error = conf0.max_pos_error +fault0.max_vel_error = conf0.max_vel_error +fault0.max_dc_cur = conf0.max_dc_cur +fault0.max_ac_cur = conf0.max_ac_cur +fault0.max_dc_volt = conf0.max_dc_volt +fault0.high_dc_volt = conf0.max_dc_volt +fault0.min_dc_volt = conf0.min_dc_volt +fault0.max_hv_temp = conf0.max_hv_temp +fault0.max_mot_temp = conf0.max_mot_temp +fault0.high_hv_temp = conf0.high_hv_temp +fault0.high_mot_temp = conf0.high_mot_temp +fault0.fan_hv_temp = conf0.fan_hv_temp +fault0.fan_mot_temp = conf0.fan_mot_temp +fault0.ac_cur = hv0.abs_cur +hv0.rev = conf0.out_rev +hv0.r = conf0.r +hv0.l = conf0.l +hv0.psi = conf0.psi +hv0.cur_p = conf0.cur_p +hv0.cur_i = conf0.cur_i +hv0.cur_ff = conf0.cur_ff +hv0.cur_ind = conf0.cur_ind +hv0.max_cur = conf0.max_ac_cur +reslimit0.pos_in = rev0.out +fb_switch0.cmd_pos = reslimit0.pos_out +fb_switch0.polecount = conf0.polecount +fb_switch0.en = fault0.en_fb +fb_switch0.mot_polecount = conf0.mot_fb_polecount +fb_switch0.joint_polecount = conf0.joint_fb_polecount +fb_switch0.com_polecount = conf0.com_fb_polecount +fb_switch0.mot_offset = conf0.mot_fb_offset +fb_switch0.joint_offset = conf0.joint_fb_offset +fb_switch0.com_offset = conf0.com_fb_offset +fb_switch0.phase_time = conf0.phase_time +fb_switch0.phase_cur = conf0.phase_cur +veltime0.pos = rev0.out +veltime1.pos = fb_switch0.vel_fb +vel0.pos_in = fb_switch0.com_fb +vel0.en = fault0.en_fb +vel0.torque = mpid0.torque_cor_cmd +mpid0.en = fault0.en_pid +mpid0.pos_ext_cmd = reslimit0.pos_out +mpid0.vel_ext_cmd = veltime0.vel_lp +mpid0.pos_fb = fb_switch0.pos_fb +mpid0.vel_fb = veltime1.vel_lp +rev0.in_d = veltime0.vel_lp +fault0.pos_error = mpid0.pos_error +fault0.vel_error = mpid0.vel_error +fault0.dc_volt = hv0.dc_volt +fault0.hv_temp = hv0.hv_temp +fault0.dc_cur = hv0.dc_cur +fault0.hv_error = hv0.fault +fault0.fb_ready = fb_switch0.state +hv0.en = fault0.en_out +hv0.pos = vel2.pos_out +hv0.vel = vel2.vel +hv0.scale = fault0.scale +io0.fan = fault0.hv_fan +io0.fault = fault0.fault +io0.state = fault0.state +pe0.udc = hv0.dc_volt +pe0.idc = 0 +pe0.ud = 0 +pe0.uq = 0 +pe0.id = hv0.id_fb +pe0.iq = hv0.iq_fb +pe0.torque = mpid0.torque_cmd +pe0.vel = vel1.vel +pe0.r = conf0.r +pe0.j = conf0.j +pe0.cap = 0.00054 +iit0.max_temp = conf0.max_mot_temp +iit0.high_temp = conf0.high_mot_temp +iit0.max_cur = conf0.max_ac_cur +fault0.mot_temp = iit0.temp +iit0.cur = hv0.abs_cur \ No newline at end of file diff --git a/conf/template/mpmsm.txt b/conf/template/mpmsm.txt new file mode 100644 index 00000000..8ba37ab4 --- /dev/null +++ b/conf/template/mpmsm.txt @@ -0,0 +1,11 @@ +load pmsm_ttc +pmsm_ttc0.rt_prio = 9 +pmsm_ttc0.psi = conf0.psi +pmsm_ttc0.polecount = conf0.polecount +pmsm_ttc0.pos_in = vel0.pos_out +pmsm_ttc0.torque = mpid0.torque_cmd +hv0.d_cmd = fb_switch0.id +hv0.q_cmd = pmsm_ttc0.cur +hv0.cmd_mode = 1 +hv0.phase_mode = 2 +hv0.pos = pmsm_ttc0.pos_out \ No newline at end of file diff --git a/conf/template/uvw_fb1.txt b/conf/template/uvw_fb1.txt index ec23dbe8..317272bf 100644 --- a/conf/template/uvw_fb1.txt +++ b/conf/template/uvw_fb1.txt @@ -10,3 +10,5 @@ fb_switch0.com_polecount = conf0.com_fb_polecount io0.fb1g = 1 io0.fb1y = uvw0.led conf0.com_fb_polecount = conf0.polecount +uvw0.amp = adc0.amp1 +fault0.com_fb_error = uvw0.error \ No newline at end of file diff --git a/shared/angle.c b/shared/angle.c index 466e1037..fa854846 100644 --- a/shared/angle.c +++ b/shared/angle.c @@ -114,6 +114,10 @@ inline float mod(float a) { } } +inline int modi(int v, int m){ + return(SIGN(v) * (ABS(v) % m)); +} + int quadrant(float a) { if(a >= 0) { if(a < M_PI / 2.0) { diff --git a/shared/angle.h b/shared/angle.h index bbbb17ee..f9e0b55d 100644 --- a/shared/angle.h +++ b/shared/angle.h @@ -2,6 +2,7 @@ float minus(float a, float b); float mod(float a); +int modi(int v, int m); int quadrant(float a); void sincos_fast(float x, float *sin, float *cos); float err_filter(float *ctx, float max, float dens, float err); \ No newline at end of file diff --git a/shared/comps/en.c b/shared/comps/en.c new file mode 100644 index 00000000..a04fa921 --- /dev/null +++ b/shared/comps/en.c @@ -0,0 +1,65 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(en); + +HAL_PIN(en_in); +HAL_PIN(en_out0); +HAL_PIN(en_out1); +HAL_PIN(fault); + +HAL_PIN(time); +HAL_PIN(timer); + +static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr){ + struct en_pin_ctx_t *pins = (struct en_pin_ctx_t *)pin_ptr; + PIN(time) = 5; +} + +static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + //struct en_ctx_t *ctx = (struct en_ctx_t *)ctx_ptr; + struct en_pin_ctx_t *pins = (struct en_pin_ctx_t *)pin_ptr; + + if(PIN(en_in) <= 0.0){ + PIN(timer) = 0.0; + } + else{ + PIN(timer) += period; + } + + if(PIN(fault) > 0.0){ + PIN(timer) = 0.0; + } + + if(PIN(timer) > PIN(time) / 2.0){ + PIN(en_out0) = 1.0; + } + else{ + PIN(en_out0) = 0.0; + } + + if(PIN(timer) > PIN(time)){ + PIN(en_out1) = 1.0; + PIN(timer) = PIN(time); + } + else{ + PIN(en_out1) = 0.0; + } +} + +hal_comp_t en_comp_struct = { + .name = "en", + .nrt = 0, + .rt = rt_func, + .frt = 0, + .nrt_init = nrt_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct en_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; diff --git a/shared/comps/fmove.c b/shared/comps/fmove.c new file mode 100644 index 00000000..530ca0c0 --- /dev/null +++ b/shared/comps/fmove.c @@ -0,0 +1,150 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(fmove); + +HAL_PIN(en); + +HAL_PIN(gravity); +HAL_PIN(real_mass); +HAL_PIN(virtual_mass); +HAL_PIN(damping); +HAL_PIN(friction); + +HAL_PIN(min_pos); +HAL_PIN(max_pos); +HAL_PIN(max_vel); +HAL_PIN(max_acc); +HAL_PIN(max_usr_vel); +HAL_PIN(max_usr_acc); +HAL_PIN(scale); + +HAL_PIN(force_in); +HAL_PIN(force); +HAL_PIN(force_offset); +HAL_PIN(force_offset_lpf); +HAL_PIN(pos); +HAL_PIN(mpos); +HAL_PIN(vel); +HAL_PIN(vel_old); +HAL_PIN(acc); +HAL_PIN(target); + +HAL_PIN(force_time); +HAL_PIN(force_timer); +HAL_PIN(force_th); + +HAL_PIN(print_freq); +HAL_PIN(print_timer); + +HAL_PIN(mode); + +static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + struct fmove_pin_ctx_t *pins = (struct fmove_pin_ctx_t *)pin_ptr; + PIN(force_time) = 5.0; + PIN(max_acc) = 1; + PIN(max_vel) = 0.1; + PIN(max_usr_acc) = 2; + PIN(max_usr_vel) = 0.1; + PIN(min_pos) = 0.0; + PIN(max_pos) = 0.5; + PIN(real_mass) = 0.0; + PIN(virtual_mass) = 1.0; + PIN(damping) = 1.0; + PIN(friction) = 0.01; + PIN(scale) = 1000.0 / 25.0 * 2.0 * M_PI * 2.0; + PIN(force_th) = 0.1; + PIN(gravity) = 0.0; + PIN(force_offset_lpf) = 0.001; +} + +static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + struct fmove_pin_ctx_t *pins = (struct fmove_pin_ctx_t *)pin_ptr; + + PIN(vel_old) = PIN(vel); + + PIN(force) = PIN(force_in) - PIN(gravity) * PIN(real_mass) - PIN(acc) * PIN(real_mass); + PIN(force) -= PIN(force_offset); + PIN(force_offset) += PIN(force) * PIN(force_offset_lpf) * period; + PIN(force) -= SIGN(PIN(vel)) * PIN(friction) + PIN(vel) * PIN(damping); + + PIN(force_timer) += period; + if(ABS(PIN(force)) > PIN(force_th)){ + PIN(force_timer) = 0.0; + PIN(mode) = 1; + } + else if(PIN(mode) > 0 && ABS(PIN(vel)) > PIN(max_usr_vel) * 0.05){ + PIN(force_timer) = 0.0; + } + else if(PIN(force_timer) > PIN(force_time)){ + PIN(force_timer) = PIN(force_time); + PIN(mode) = 0; + } + + switch((int) PIN(mode)){ + case 0: // auto move + PIN(vel) = SIGN(PIN(target) - PIN(pos)) * sqrtf(ABS(PIN(target) - PIN(pos)) * 2.0 * PIN(max_acc)); + PIN(vel) = CLAMP(PIN(vel), PIN(vel_old) - PIN(max_acc) * period, PIN(vel_old) + PIN(max_acc) * period); + PIN(vel) = LIMIT(PIN(vel), PIN(max_vel)); + break; + case 1: // force move + PIN(acc) = PIN(force) / PIN(virtual_mass); + PIN(acc) = LIMIT(PIN(acc), PIN(max_acc)); + PIN(vel) += PIN(acc) * period; + PIN(vel) = CLAMP(PIN(vel), PIN(vel_old) - PIN(max_usr_acc) * period, PIN(vel_old) + PIN(max_usr_acc) * period); + PIN(vel) = LIMIT(PIN(vel), PIN(max_usr_vel)); + break; + } + + PIN(vel) = CLAMP(PIN(vel), -sqrtf(ABS(PIN(pos) - PIN(min_pos)) * 2.0 * PIN(max_acc)), sqrtf(ABS(PIN(pos) - PIN(max_pos)) * 2.0 * PIN(max_acc))); + PIN(acc) = (PIN(vel) - PIN(vel_old)) / period; + + PIN(pos) += PIN(vel) * period; + PIN(pos) = CLAMP(PIN(pos), PIN(min_pos), PIN(max_pos)); + PIN(mpos) = mod(PIN(pos) * PIN(scale)); + PIN(print_timer) += period; + + if(PIN(en) <= 0.0){ + PIN(pos) = 0.0; + PIN(mpos) = 0.0; + PIN(vel) = 0.0; + } +} + +static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { + struct fmove_pin_ctx_t *pins = (struct fmove_pin_ctx_t *)pin_ptr; + if(PIN(print_freq) > 0.0){ + if(PIN(print_timer) > 1.0 / PIN(print_freq)){ + PIN(print_timer) = 0.0; + if(PIN(mode) > 0.0){ + printf("force_move %f\n", PIN(pos)); + } + else if(ABS(PIN(target) - PIN(pos)) < 0.01 & ABS(PIN(vel)) < PIN(max_vel) * 0.1){ + printf("on_target %f\n", PIN(pos)); + } + else{ + printf("moving %f\n", PIN(pos)); + } + } + } + else{ + PIN(print_timer) = 0.0; + } +} + +hal_comp_t fmove_comp_struct = { + .name = "fmove", + .nrt = nrt_func, + .rt = rt_func, + .frt = 0, + .nrt_init = nrt_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct fmove_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; \ No newline at end of file diff --git a/shared/comps/home.c b/shared/comps/home.c index 1112fcf2..0cd8bf0f 100644 --- a/shared/comps/home.c +++ b/shared/comps/home.c @@ -6,36 +6,33 @@ HAL_COMP(home); -HAL_PIN(en); - HAL_PIN(home_vel); HAL_PIN(home_acc); -HAL_PIN(home_pos); HAL_PIN(pos_in); HAL_PIN(pos_out); -HAL_PIN(vel_cmd); +HAL_PIN(vel); HAL_PIN(home_in); HAL_PIN(home_polarity); -HAL_PIN(limit_in0); -HAL_PIN(limit_in1); -HAL_PIN(limit_polarity); - -HAL_PIN(state); -HAL_PIN(en_out); -HAL_PIN(target); +HAL_PIN(offset); HAL_PIN(home_offset); +HAL_PIN(state); + +HAL_PIN(en_in); +HAL_PIN(en_out); + HAL_PIN(re_home); -HAL_PIN(limit_fault); static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr){ struct home_pin_ctx_t *pins = (struct home_pin_ctx_t *)pin_ptr; PIN(state) = 0; PIN(re_home) = 1; PIN(home_polarity) = 1.0; + PIN(home_vel) = 2.0 * M_PI; + PIN(home_acc) = 2.0 * M_PI / 0.1; } static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { @@ -44,19 +41,21 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { float vel = 0.0; - if(PIN(en) <= 0.0){ + if(PIN(en_in) <= 0.0){ if(PIN(re_home) > 0.0){ PIN(state) = 0; } PIN(en_out) = 0; - PIN(limit_fault) = 0; } switch((int)PIN(state)){ case 0: // not homed - if(PIN(en) > 0.0){ + if(PIN(en_in) > 0.0){ PIN(state) = 1; PIN(en_out) = 0; + PIN(offset) = 0.0; + PIN(home_offset) = 0.0; + PIN(vel) = 0.0; } break; @@ -75,9 +74,11 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { case 2: // search home falling if(PIN(home_polarity) <= 0.0 && PIN(home_in) > 0.0){ PIN(state) = 3; + PIN(home_offset) = PIN(offset); } else if(PIN(home_polarity) > 0.0 && PIN(home_in) <= 0.0){ PIN(state) = 3; + PIN(home_offset) = PIN(offset); } else{ vel = -PIN(home_vel); @@ -85,47 +86,27 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { break; case 3: // set home pos - PIN(target) = PIN(home_pos); - PIN(state) = 4; - break; + vel = SIGN(PIN(home_offset) - PIN(offset)) * sqrtf(ABS(PIN(home_offset) - PIN(offset)) * 2.0 * PIN(home_acc)); - - case 4: // go to home pos - vel = PIN(target) * 10; - if(ABS(PIN(target)) < 0.0001){ - PIN(state) = 5; + if(ABS(PIN(offset) - PIN(home_offset)) < 0.0001 && PIN(vel) < PIN(home_vel) * 0.001){ + PIN(state) = 4; } break; - case 5: // homed + case 4: // homed PIN(en_out) = 1; - - if(PIN(limit_polarity) > 0.0 && (PIN(limit_in0) > 0.0 || PIN(limit_in1) > 0.0)){ - PIN(state) = 6; - PIN(en_out) = 0; - PIN(limit_fault) = 1; - } - else if(PIN(limit_polarity) <= 0.0 && (PIN(limit_in0) <= 0.0 || PIN(limit_in1) <= 0.0)){ - PIN(state) = 6; - PIN(en_out) = 0; - PIN(limit_fault) = 1; - } - break; - - case 6: // fault - PIN(en_out) = 0; - PIN(limit_fault) = 1; + PIN(offset) = PIN(home_offset); + PIN(vel) = 0.0; break; } vel = LIMIT(vel, ABS(PIN(home_vel))); - PIN(vel_cmd) = CLAMP(vel, PIN(vel_cmd) - PIN(home_acc) * period, PIN(vel_cmd) + PIN(home_acc) * period); + PIN(vel) = CLAMP(vel, PIN(vel) - PIN(home_acc) * period, PIN(vel) + PIN(home_acc) * period); - PIN(target) -= PIN(vel_cmd) * period; - PIN(home_offset) += PIN(vel_cmd) * period; - PIN(pos_out) = PIN(pos_in) + PIN(home_offset); - PIN(pos_out) = mod(PIN(pos_out)); - + PIN(offset) += PIN(vel) * period; + PIN(offset) = mod(PIN(offset)); + + PIN(pos_out) = mod(PIN(pos_in) + PIN(offset)); } hal_comp_t home_comp_struct = { diff --git a/shared/comps/mpid.c b/shared/comps/mpid.c new file mode 100644 index 00000000..d59e4e9b --- /dev/null +++ b/shared/comps/mpid.c @@ -0,0 +1,92 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(mpid); + +HAL_PIN(pos_ext_cmd); +HAL_PIN(pos_fb); +HAL_PIN(old_pos_ext_cmd); +HAL_PIN(old_pos_fb); +HAL_PIN(pos_error); + +HAL_PIN(vel_ext_cmd); +HAL_PIN(vel_fb); +HAL_PIN(vel_error); +HAL_PIN(vel_cmd); + +HAL_PIN(acc_cmd); + +HAL_PIN(torque_ext_cmd); +HAL_PIN(torque_sum); +HAL_PIN(torque_cmd); + +HAL_PIN(min_pos_error); +HAL_PIN(max_vel); +HAL_PIN(max_acc); +HAL_PIN(max_torque); + +HAL_PIN(pos_p); +HAL_PIN(vel_i); +HAL_PIN(vel_p); +HAL_PIN(j); +HAL_PIN(scale); + +HAL_PIN(en); + +static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + // struct mpid_ctx_t *ctx = (struct mpid_ctx_t *)ctx_ptr; + struct mpid_pin_ctx_t *pins = (struct mpid_pin_ctx_t *)pin_ptr; + + if(PIN(en) > 0.0){ + PIN(pos_error) += minus(PIN(pos_ext_cmd), PIN(old_pos_ext_cmd)) * PIN(scale); + PIN(pos_error) -= minus(PIN(pos_fb), PIN(old_pos_fb)); + + PIN(vel_cmd) = LIMIT(PIN(pos_error) * PIN(pos_p), PIN(max_vel)); + + if(ABS(PIN(pos_error)) < PIN(min_pos_error)){ + PIN(vel_cmd) = 0.0; + } + + PIN(vel_cmd) += PIN(vel_ext_cmd); + + PIN(vel_error) = PIN(vel_cmd) - PIN(vel_fb); + + PIN(acc_cmd) = LIMIT(PIN(vel_error) * PIN(vel_p), PIN(max_acc)); + + PIN(torque_cmd) = LIMIT(PIN(acc_cmd) * PIN(j), PIN(max_torque)); + + PIN(torque_sum) += PIN(vel_error) * PIN(vel_i) * period; + PIN(torque_sum) = CLAMP(PIN(torque_sum), -PIN(max_torque) - PIN(torque_cmd), PIN(max_torque) - PIN(torque_cmd)); + + PIN(torque_cmd) += PIN(torque_ext_cmd); + PIN(torque_cmd) += PIN(torque_sum); + } + else{ + PIN(pos_error) = 0.0; + PIN(vel_cmd) = 0.0; + PIN(vel_error) = 0.0; + PIN(acc_cmd) = 0.0; + PIN(torque_sum) = 0.0; + PIN(torque_cmd) = 0.0; + } + + PIN(old_pos_fb) = PIN(pos_fb); + PIN(old_pos_ext_cmd) = PIN(pos_ext_cmd); +} + +hal_comp_t mpid_comp_struct = { + .name = "mpid", + .nrt = 0, + .rt = rt_func, + .frt = 0, + .nrt_init = 0, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct mpid_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; \ No newline at end of file diff --git a/shared/comps/uvw.c b/shared/comps/uvw.c index 206c2b4d..53675c13 100644 --- a/shared/comps/uvw.c +++ b/shared/comps/uvw.c @@ -11,6 +11,11 @@ HAL_PIN(u); HAL_PIN(v); HAL_PIN(w); +HAL_PIN(amp); +HAL_PIN(timer); +HAL_PIN(en_time); +HAL_PIN(mode); // 0 = uvw, 1 = uvw line saving + HAL_PIN(led); HAL_PIN(p0); @@ -26,20 +31,21 @@ HAL_PIN(p7); HAL_PIN(pos); HAL_PIN(rpos); HAL_PIN(state); +HAL_PIN(error); static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { // struct uvw_ctx_t * ctx = (struct io_ctx_t *)ctx_ptr; struct uvw_pin_ctx_t *pins = (struct uvw_pin_ctx_t *)pin_ptr; - PIN(p0) = 0; //fault + PIN(p0) = -1; //fault PIN(p1) = 0; //u = 0 PIN(p2) = 2; //v = 2.094395 PIN(p3) = 1; //u + v = 1.047198 PIN(p4) = 4; //w = -2.094395 PIN(p5) = 5; //u + w = -1.047198 PIN(p6) = 3; //v + w = -3.141593 - PIN(p7) = 0; //fault - PIN(state) = 3.0; + PIN(p7) = -1; //fault + PIN(en_time) = 0.02; } static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { @@ -49,7 +55,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { uint32_t rpos = (PIN(u) > 0.0) * 1.0 + (PIN(v) > 0.0) * 2.0 + (PIN(w) > 0.0) * 4.0; PIN(led) = (PIN(u) > 0.0) ^ (PIN(v) > 0.0) ^ (PIN(w) > 0.0); //TODO: make this const, fault output - uint32_t t[8]; + int32_t t[8]; t[0] = PIN(p0); t[1] = PIN(p1); t[2] = PIN(p2); @@ -59,7 +65,31 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { t[6] = PIN(p6); t[7] = PIN(p7); PIN(rpos) = rpos; - PIN(pos) = mod((float)t[rpos] / 6.0 * 2.0 * M_PI); + if(rpos < 0 | PIN(amp) < 0.5){ + PIN(error) = 1.0; + PIN(state) = 0.0; + PIN(timer) = 0.0; + } + else{ + switch((int) PIN(mode)){ + case 0: + PIN(state) = 3.0; + PIN(error) = 0.0; + PIN(pos) = mod((float)t[rpos] / 6.0 * 2.0 * M_PI); + break; + case 1: + if(PIN(timer) < PIN(en_time) / 2.0){ + PIN(pos) = mod((float)t[rpos] / 6.0 * 2.0 * M_PI); + } + if(PIN(timer) > PIN(en_time)){ + PIN(state) = 2.0; + PIN(error) = 0.0; + } + else{ + PIN(timer) += period; + } + } + } } hal_comp_t uvw_comp_struct = { diff --git a/shared/comps/velbuf.c b/shared/comps/velbuf.c index 5228b59c..a8896fe4 100644 --- a/shared/comps/velbuf.c +++ b/shared/comps/velbuf.c @@ -68,13 +68,13 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { break; } } - - float diff_pos = minus(pos, old_pos); - PIN(diff_pos) = diff_pos; - PIN(diff_time) = diff_time; - PIN(vel) = diff_pos / diff_time; + + float diff_pos = minus(pos, old_pos); + PIN(diff_pos) = diff_pos; + PIN(diff_time) = diff_time; + PIN(vel) = diff_pos / diff_time; - PIN(vel_lp) = PIN(vel) * LP_HZ(PIN(lpf)) + PIN(vel_lp) * (1.0 - LP_HZ(PIN(lpf))); + PIN(vel_lp) = PIN(vel) * LP_HZ(PIN(lpf)) + PIN(vel_lp) * (1.0 - LP_HZ(PIN(lpf))); } hal_comp_t velbuf_comp_struct = { diff --git a/shared/comps/veltime.c b/shared/comps/veltime.c new file mode 100644 index 00000000..e4701867 --- /dev/null +++ b/shared/comps/veltime.c @@ -0,0 +1,62 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(veltime); + +HAL_PIN(pos); +HAL_PIN(old_pos); +HAL_PIN(timer); +HAL_PIN(vel); +HAL_PIN(vel_lp); + +HAL_PIN(max_time); +HAL_PIN(lpf) + + +static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + // struct veltime_ctx_t *ctx = (struct veltime_ctx_t *)ctx_ptr; + struct veltime_pin_ctx_t *pins = (struct veltime_pin_ctx_t *)pin_ptr; + + PIN(max_time) = 0.1; + + PIN(lpf) = 100.0; +} + + +static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + // struct veltime_ctx_t *ctx = (struct veltime_ctx_t *)ctx_ptr; + struct veltime_pin_ctx_t *pins = (struct veltime_pin_ctx_t *)pin_ptr; + + PIN(timer) += period; + + if(PIN(pos) != PIN(old_pos)){ + PIN(vel) = minus(PIN(pos), PIN(old_pos)) / PIN(timer); + PIN(old_pos) = PIN(pos); + PIN(timer) = 0.0; + } + + if(PIN(timer) > PIN(max_time)){ + PIN(timer) = PIN(max_time); + PIN(vel) = 0.0; + } + + PIN(vel_lp) = PIN(vel) * LP_HZ(PIN(lpf)) + PIN(vel_lp) * (1.0 - LP_HZ(PIN(lpf))); + +} + +hal_comp_t veltime_comp_struct = { + .name = "veltime", + .nrt = 0, + .rt = 0, + .frt = frt_func, + .nrt_init = nrt_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct veltime_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; \ No newline at end of file diff --git a/src/comps/enc_fb.c b/src/comps/enc_fb.c index 8ae785b8..37fee408 100644 --- a/src/comps/enc_fb.c +++ b/src/comps/enc_fb.c @@ -27,6 +27,7 @@ HAL_PIN(error); HAL_PIN(amp); HAL_PIN(vel); HAL_PIN(ccr3); +HAL_PIN(en_index); HAL_PIN(indexprint); @@ -186,7 +187,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { p = mod(tim * 2.0f * M_PI / (float)ctx->e_res); PIN(pos) = p; - if(FB0_ENC_TIM->SR & TIM_SR_CC3IF) { + if(PIN(en_index) > 0.0 & FB0_ENC_TIM->SR & TIM_SR_CC3IF) { int cc = FB0_ENC_TIM->CCR3; PIN(state) = 3.0; ctx->absoffset = mod(cc * 2.0f * M_PI / (float)ctx->e_res); diff --git a/stm32f303/src/comps/hv.c b/stm32f303/src/comps/hv.c index 8fc4a526..ca1319c0 100644 --- a/stm32f303/src/comps/hv.c +++ b/stm32f303/src/comps/hv.c @@ -47,7 +47,7 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { PIN(min_on) = 0.000005; PIN(min_off) = 0.000005; PIN(arr) = PWM_RES; - PIN(drop) = 1.6; + PIN(drop) = 0; } static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {