1
0
mirror of https://github.com/rene-dev/stmbl.git synced 2024-12-25 01:52:13 +00:00
This commit is contained in:
Rene Hopf 2017-07-02 18:44:26 +02:00
parent fa558c51f2
commit d53ccb0d91
2 changed files with 82 additions and 0 deletions
Makefile
shared/comps

View File

@ -52,6 +52,7 @@ COMPS += shared/comps/fb_switch.c
COMPS += shared/comps/reslimit.c
COMPS += shared/comps/iit.c
COMPS += shared/comps/vel_int.c
COMPS += shared/comps/linrev.c
SOURCES += $(COMPS)

81
shared/comps/linrev.c Normal file
View File

@ -0,0 +1,81 @@
//Calculate motor angle from position in machine units
/*
|
Q2 | Q1
|
-------------
|
Q3 | Q4
|
*/
#include "hal.h"
#include "angle.h"
#include <math.h>
HAL_COMP(linrev);
HAL_PIN(scale);
HAL_PIN(cmd_in);
HAL_PIN(cmd_out);
HAL_PIN(fb_in);
HAL_PIN(fb_out);
struct linrev_ctx_t{
uint8_t lastq;//last quadrant
int32_t rev;//current multiturn
};
static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
struct linrev_ctx_t * ctx = (struct linrev_ctx_t *)ctx_ptr;
struct linrev_pin_ctx_t * pins = (struct linrev_pin_ctx_t *)pin_ptr;
float s = PIN(scale);
if(s < 0.01){
s = 0.01;
}
PIN(cmd_out) = mod((PIN(cmd_in) / s) * 2.0 * M_PI);
uint8_t q = 0;
if(PIN(fb_in) > M_PI/2.0){
q = 2;
}
if(PIN(fb_in) < -M_PI/2.0){
q = 3;
}
if(q != 0 && q == 3 && ctx->lastq == 2){
ctx->rev++;
}
if(q != 0 && q == 2 && ctx->lastq == 3){
ctx->rev--;
}
ctx->lastq = q;
PIN(fb_out) = ((PIN(fb_in) + ctx->rev * M_PI * 2.0) * s) / (2.0 * M_PI);
}
const hal_comp_t linrev_comp_struct = {
.name = "linrev",
.nrt = 0,
.rt = rt_func,
.frt = 0,
.nrt_init = 0,
.hw_init = 0,
.rt_start = 0,
.frt_start = 0,
.rt_stop = 0,
.frt_stop = 0,
.ctx_size = sizeof(struct linrev_ctx_t),
.pin_count = sizeof(struct linrev_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};