-
Notifications
You must be signed in to change notification settings - Fork 404
/
pos_filter.c
54 lines (43 loc) · 1.27 KB
/
pos_filter.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#include "hal.h"
#include "angle.h"
#include "defines.h"
HAL_COMP(pos_filter);
HAL_PIN(pos_in);
HAL_PIN(vel_in);
HAL_PIN(pos_out);
HAL_PIN(vel_out);
HAL_PIN(acc_out);
HAL_PIN(bandwidth);
HAL_PIN(en);
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
//struct pos_filter_ctx_t *ctx = (struct pos_filter_ctx_t *)ctx_ptr;
struct pos_filter_pin_ctx_t *pins = (struct pos_filter_pin_ctx_t *)pin_ptr;
float ki = 2.0 * MIN(PIN(bandwidth), 1.0 / period / 2.0);
float kp = 0.25 * ki * ki;
float pos_error = minus(PIN(pos_in), PIN(pos_out));
float vel_error = PIN(vel_in) - PIN(vel_out);
if(PIN(en) > 0.0){
PIN(acc_out) = kp * pos_error + ki * vel_error;
PIN(vel_out) += period * PIN(acc_out);
PIN(pos_out) = mod(PIN(pos_out) + period * PIN(vel_out));
}
else{
PIN(acc_out) = 0;
PIN(vel_out) = PIN(vel_in);
PIN(pos_out) = PIN(pos_in);
}
}
const hal_comp_t pos_filter_comp_struct = {
.name = "pos_filter",
.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 = 0,
.pin_count = sizeof(struct pos_filter_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};