Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-08-18 01:18:58 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

fanux fix

Browse Source
This commit is contained in:
crinq
2020-01-17 23:31:33 +01:00
parent 21fcd9b140
commit ff3c7bec34
1 changed files with 28 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/comps/encf.c
+28 -32
View File
@@ -236,40 +236,36 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
oldcrc[4] = crc[4];
}
if(crc[0] == 0 && crc[1] == 0 && crc[2] == 0 && crc[3] == 0 && crc[4] == 0) {
PIN(crc_ok)
++;
PIN(crc_ok)++;
int32_t pos = data.fanuc.pos_lo + (data.fanuc.pos_hi << 6);
PIN(index) = data.fanuc.no_index;
PIN(abs_pos) = mod((float)pos * 2.0 * M_PI / (1 << 22));
if(PIN(index) > 0.0) {
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
PIN(state) = 1;
state_counter = 1;
} else if(state_counter == 1) {
state_counter = 2;
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
} else {
state_counter = 3;
PIN(pos) = mod((float)(pos + pos_offset) * 2.0 * M_PI / (1 << 22));
PIN(state) = 3;
}
PIN(turns) = data.fanuc.turns;
pos = data.fanuc.com_pos;
PIN(com_pos) = mod(pos * 2.0 * M_PI / 1024);
PIN(error) = 0;
} else {
PIN(crc_er)
++;
PIN(crc_er)++;
PIN(state) = 1;
PIN(error) = 1;
}
int32_t pos;
pos = data.fanuc.pos_lo + (data.fanuc.pos_hi << 6);
PIN(index) = data.fanuc.no_index;
PIN(abs_pos) = mod((float)pos * 2.0 * M_PI / (1 << 22));
if(PIN(index) > 0.0) {
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
PIN(state) = 1;
state_counter = 1;
} else if(state_counter == 1) {
state_counter = 2;
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
} else {
state_counter = 3;
PIN(pos) = mod((float)(pos + pos_offset) * 2.0 * M_PI / (1 << 22));
PIN(state) = 3;
}
PIN(turns) = data.fanuc.turns;
pos = data.fanuc.com_pos;
PIN(com_pos) = mod(pos * 2.0 * M_PI / 1024);
PIN(error) = 0;
} else {
PIN(error) = 1;
PIN(state) = 1;