static THD_FUNCTION(periodic_thread, arg) {
(void)arg;
chRegSetThreadName("Main periodic");
int fault_print = 0;
for(;;) {
if (mc_interface_get_state() == MC_STATE_RUNNING) {
ledpwm_set_intensity(LED_GREEN, 1.0);
} else {
ledpwm_set_intensity(LED_GREEN, 0.2);
}
mc_fault_code fault = mc_interface_get_fault();
if (fault != FAULT_CODE_NONE) {
if (!fault_print && AUTO_PRINT_FAULTS) {
fault_print = 1;
commands_printf("%s\n", mc_interface_fault_to_string(
mc_interface_get_fault()));
}
for (int i = 0; i < (int)fault; i++) {
ledpwm_set_intensity(LED_RED, 1.0);
chThdSleepMilliseconds(250);
ledpwm_set_intensity(LED_RED, 0.0);
chThdSleepMilliseconds(250);
}
chThdSleepMilliseconds(500);
} else {
ledpwm_set_intensity(LED_RED, 0.0);
fault_print = 0;
}
if (mc_interface_get_state() == MC_STATE_DETECTING) {
commands_send_rotor_pos(mcpwm_get_detect_pos());
}
#if ENCODER_ENABLE
commands_send_rotor_pos(encoder_read_deg());
// comm_can_set_pos(0, encoder_read_deg());
#endif
chThdSleepMilliseconds(10);
}
}
float mc_interface_get_pid_pos_now(void) {
float ret = 0.0;
switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = encoder_read_deg();
break;
case MOTOR_TYPE_FOC:
ret = mcpwm_foc_get_pid_pos_now();
break;
default:
break;
}
return ret;
}
