Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-09-24 08:46:28 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

drop and move stuff

Browse Source
This commit is contained in:
crinq
2019-12-11 10:22:44 +01:00
parent 5e86c707cc
commit 3a1696c77e
18 changed files with 675 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+5
View File
@@ -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)
conf/move.txt
+63
View File
@@ -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
conf/template/enc_fb0.txt
+1
View File
@@ -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
conf/template/enc_ws_fb0.txt
+30
View File
@@ -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
conf/template/mpid.txt
+118
View File
@@ -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
conf/template/mpmsm.txt
+11
View File
@@ -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
conf/template/uvw_fb1.txt
+2
View File
@@ -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
shared/angle.c
+4
View File
@@ -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) {
shared/angle.h
+1
View File
@@ -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);
shared/comps/en.c
+65
View File
@@ -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),
};
shared/comps/fmove.c
+150
View File
@@ -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),
};
shared/comps/home.c
+27 -46
View File
@@ -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 = {
shared/comps/mpid.c
+92
View File
@@ -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),
};
shared/comps/uvw.c
+35 -5
View File
@@ -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 = {
shared/comps/velbuf.c
+6 -6
View File
@@ -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 = {
shared/comps/veltime.c
+62
View File
@@ -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),
};
src/comps/enc_fb.c
+2 -1
View File
@@ -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);
stm32f303/src/comps/hv.c
+1 -1
View File
@@ -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) {