From 7072018ca2cc5ea4f3e3a1487be96f6692abcc40 Mon Sep 17 00:00:00 2001
From: crinq <crinq@crinq.de>
Date: Sun, 16 Sep 2018 20:15:06 +0200
Subject: [PATCH] velbuf

---
 Makefile              |  1 +
 shared/comps/velbuf.c | 92 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 93 insertions(+)
 create mode 100644 shared/comps/velbuf.c

diff --git a/Makefile b/Makefile
index caefea3f..903f5172 100644
--- a/Makefile
+++ b/Makefile
@@ -91,6 +91,7 @@ COMPS += shared/comps/not.c
 COMPS += shared/comps/and.c
 COMPS += shared/comps/or.c
 COMPS += shared/comps/jog.c
+COMPS += shared/comps/velbuf.c
 
 SOURCES += $(COMPS)
 
diff --git a/shared/comps/velbuf.c b/shared/comps/velbuf.c
new file mode 100644
index 00000000..aa132758
--- /dev/null
+++ b/shared/comps/velbuf.c
@@ -0,0 +1,92 @@
+#include "commands.h"
+#include "hal.h"
+#include "math.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(velbuf);
+
+HAL_PIN(pos);
+HAL_PIN(vel);
+HAL_PIN(vel_lp);
+HAL_PIN(min_pos_diff);
+
+HAL_PIN(lpf)
+
+HAL_PIN(diff_time);
+HAL_PIN(diff_pos);
+
+#define velbuf_size  20
+
+struct velbuf_ctx_t {
+  float buf[velbuf_size];
+  uint32_t ptr;
+  float frt_period;
+};
+
+static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  struct velbuf_ctx_t *ctx      = (struct velbuf_ctx_t *)ctx_ptr;
+  struct velbuf_pin_ctx_t *pins = (struct velbuf_pin_ctx_t *)pin_ptr;
+  
+  PIN(min_pos_diff) = 0.01;
+
+  for(int i = 0; i < velbuf_size; i++){
+      ctx->buf[i] = 0;
+  }
+
+  PIN(lpf) = 1000.0;
+}
+
+
+static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  struct velbuf_ctx_t *ctx      = (struct velbuf_ctx_t *)ctx_ptr;
+  struct velbuf_pin_ctx_t *pins = (struct velbuf_pin_ctx_t *)pin_ptr;
+
+  ctx->buf[ctx->ptr++] = PIN(pos);
+  ctx->ptr %= velbuf_size;
+  ctx->frt_period = period;
+}
+
+
+static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  struct velbuf_ctx_t *ctx      = (struct velbuf_ctx_t *)ctx_ptr;
+  struct velbuf_pin_ctx_t *pins = (struct velbuf_pin_ctx_t *)pin_ptr;
+
+  int p = ctx->ptr;
+  float pos = ctx->buf[(p - 1 + velbuf_size) % velbuf_size];
+   
+  float old_pos = ctx->buf[p];
+  float diff_time = ctx->frt_period * velbuf_size;
+  
+  float min_diff = PIN(min_pos_diff);
+   
+  for(int i = 0; i < velbuf_size; i++){
+    if(ABS(minus(pos, ctx->buf[(p + i) % velbuf_size])) > min_diff){
+        old_pos = ctx->buf[(p + i) % velbuf_size];
+        diff_time = (velbuf_size - i - 1) * ctx->frt_period;
+    }else{
+        break;
+    }
+  }
+   
+   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)));
+}
+
+hal_comp_t velbuf_comp_struct = {
+    .name      = "velbuf",
+    .nrt       = 0,
+    .rt        = rt_func,
+    .frt       = frt_func,
+    .nrt_init  = nrt_init,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = sizeof(struct velbuf_ctx_t),
+    .pin_count = sizeof(struct velbuf_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};
\ No newline at end of file