Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
9590cb339This file is pinned to the selected release. Download the original to open it in your design tool.
#include "fanuc_comp.h"
#include "commands.h"
#include "hal.h"
#include "math.h"
#include "defines.h"
#include "angle.h"
HAL_COMP(fanuc);
//fanuc encoder
HAL_PIN(C1);
HAL_PIN(C2);
HAL_PIN(C4);
HAL_PIN(C8);
//rotor position output
HAL_PIN(pos);
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
// struct fanuc_ctx_t * ctx = (struct fanuc_ctx_t *)ctx_ptr;
struct fanuc_pin_ctx_t *pins = (struct fanuc_pin_ctx_t *)pin_ptr;
//TODO: const...
uint8_t tab[16];
tab[0] = 0;
tab[1] = 15;
tab[2] = 1;
tab[3] = 2;
tab[4] = 13;
tab[5] = 14;
tab[6] = 12;
tab[7] = 11;
tab[8] = 5;
tab[9] = 6;
tab[10] = 4;
tab[11] = 3;
tab[12] = 8;
tab[13] = 7;
tab[14] = 9;
tab[15] = 10;
uint32_t t = (PIN(C1) > 0.0) + (PIN(C2) > 0.0) * 2 + (PIN(C4) > 0.0) * 4 + (PIN(C8) > 0.0) * 8;
PIN(pos) = mod((tab[t] / 15.0 * 2.0 * M_PI));
}
hal_comp_t fanuc_comp_struct = {
.name = "fanuc",
.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 fanuc_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};