Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include "home_comp.h"
#include "commands.h"
#include "hal.h"
#include "math.h"
#include "defines.h"
#include "angle.h"
HAL_COMP(home);
HAL_PIN(home_vel);
HAL_PIN(home_acc);
HAL_PIN(pos_in);
HAL_PIN(pos_out);
HAL_PIN(vel);
HAL_PIN(home_in);
HAL_PIN(home_polarity);
HAL_PIN(offset);
HAL_PIN(home_offset);
HAL_PIN(home_pos);
HAL_PIN(state);
HAL_PIN(en_in);
HAL_PIN(en_out);
HAL_PIN(re_home);
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) {
//struct home_ctx_t *ctx = (struct home_ctx_t *)ctx_ptr;
struct home_pin_ctx_t *pins = (struct home_pin_ctx_t *)pin_ptr;
float vel = 0.0;
if(PIN(en_in) <= 0.0){
if(PIN(re_home) > 0.0){
PIN(state) = 0;
}
PIN(en_out) = 0;
}
switch((int)PIN(state)){
case 0: // not homed
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;
case 1: // search home rising
if(PIN(home_polarity) > 0.0 && PIN(home_in) > 0.0){
PIN(state) = 2;
}
else if(PIN(home_polarity) <= 0.0 && PIN(home_in) <= 0.0){
PIN(state) = 2;
}
else{
vel = PIN(home_vel);
}
break;
case 2: // search home falling
if(PIN(home_polarity) <= 0.0 && PIN(home_in) > 0.0){
PIN(state) = 3;
PIN(home_offset) = PIN(offset) + PIN(home_pos);
}
else if(PIN(home_polarity) > 0.0 && PIN(home_in) <= 0.0){
PIN(state) = 3;
PIN(home_offset) = PIN(offset) + PIN(home_pos);
}
else{
vel = -PIN(home_vel);
}
break;
case 3: // go to home pos
vel = SIGN(PIN(home_offset) - PIN(offset)) * sqrtf(ABS(PIN(home_offset) - PIN(offset)) * 2.0 * PIN(home_acc));
if(ABS(PIN(offset) - PIN(home_offset)) < 0.01 && PIN(vel) < PIN(home_vel) * 0.01){
PIN(state) = 4;
}
break;
case 4: // homed
PIN(en_out) = 1;
PIN(offset) = PIN(home_offset);
PIN(vel) = 0.0;
break;
}
vel = LIMIT(vel, ABS(PIN(home_vel)));
PIN(vel) = CLAMP(vel, PIN(vel) - PIN(home_acc) * period, PIN(vel) + PIN(home_acc) * period);
PIN(offset) += PIN(vel) * period;
PIN(pos_out) = mod(PIN(pos_in) + mod(PIN(offset)));
}
hal_comp_t home_comp_struct = {
.name = "home",
.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 home_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};
9590cb339This file is pinned to the selected release. Download the original to open it in your design tool.