I've totally rewritten it anyway. I was able to move the state machine into a seperate procedure which has substantially streamlined the code. Now I just have to get it to compile image missing /media/kunena/emoticons/shocked.png
A lot of that does not need to be in the component because you are setting the pin externally. But we may need to scale or otherwise calculate data for them in which case they need to pass through the component. Get the basics first....
write all becomes something like
Code:
FUNCTION(writeall) {
// Restart sequence on rising edge of enable
if (enable && !data.prev_enable) data.control.raw = 0x0000;
data.prev_enable = enable;
if (enable) {
int drive_ready = runCiA402StateMachine(&__comp_inst);
if (drive_ready) {
if (homing) {
opmode_out = 6; // Homing Mode
// Bit 4 starts homing; check for errors before commanding
if (!data.status.bits.op_error) data.control.bits.op_specific = 1;
} else {
opmode_out = (s8)default_opmode;
data.control.bits.op_specific = 0; // Clear Bit 4
// Normal Motion
drive_target_position = (s32)(target_position * pos_scale);
drive_target_velocity = (s32)(target_velocity * vel_scale);
}
}
} else {
data.control.raw = 0x0006; // Shutdown / Disable Voltage
}
controlword_out = data.control.raw;
}
homing feedback is in the read command
