Exemplo n.º 1
0
void terminal_process_string(char *str) {
	enum { kMaxArgs = 64 };
	int argc = 0;
	char *argv[kMaxArgs];

	char *p2 = strtok(str, " ");
	while (p2 && argc < kMaxArgs) {
		argv[argc++] = p2;
		p2 = strtok(0, " ");
	}

	if (argc == 0) {
		commands_printf("No command received\n");
		return;
	}

	static mc_configuration mcconf; // static to save some stack
	static mc_configuration mcconf_old; // static to save some stack
	mcconf = *mc_interface_get_configuration();
	mcconf_old = mcconf;

	if (strcmp(argv[0], "ping") == 0) {
		commands_printf("pong\n");
	} else if (strcmp(argv[0], "stop") == 0) {
		mc_interface_set_duty(0);
		commands_printf("Motor stopped\n");
	} else if (strcmp(argv[0], "last_adc_duration") == 0) {
		commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
		commands_printf("Latest injected ADC duration: %.4f ms", (double)(mc_interface_get_last_inj_adc_isr_duration() * 1000.0));
		commands_printf("Latest sample ADC duration: %.4f ms\n", (double)(mc_interface_get_last_sample_adc_isr_duration() * 1000.0));
	} else if (strcmp(argv[0], "kv") == 0) {
		commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
	} else if (strcmp(argv[0], "mem") == 0) {
		size_t n, size;
		n = chHeapStatus(NULL, &size);
		commands_printf("core free memory : %u bytes", chCoreGetStatusX());
		commands_printf("heap fragments   : %u", n);
		commands_printf("heap free total  : %u bytes\n", size);
	} else if (strcmp(argv[0], "threads") == 0) {
		thread_t *tp;
		static const char *states[] = {CH_STATE_NAMES};
		commands_printf("    addr    stack prio refs     state           name time    ");
		commands_printf("-------------------------------------------------------------");
		tp = chRegFirstThread();
		do {
			commands_printf("%.8lx %.8lx %4lu %4lu %9s %14s %lu",
					(uint32_t)tp, (uint32_t)tp->p_ctx.r13,
					(uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
					states[tp->p_state], tp->p_name, (uint32_t)tp->p_time);
			tp = chRegNextThread(tp);
		} while (tp != NULL);
		commands_printf("");
	} else if (strcmp(argv[0], "fault") == 0) {
		commands_printf("%s\n", mc_interface_fault_to_string(mc_interface_get_fault()));
	} else if (strcmp(argv[0], "faults") == 0) {
		if (fault_vec_write == 0) {
			commands_printf("No faults registered since startup\n");
		} else {
			commands_printf("The following faults were registered since start:\n");
			for (int i = 0;i < fault_vec_write;i++) {
				commands_printf("Fault            : %s", mc_interface_fault_to_string(fault_vec[i].fault));
				commands_printf("Current          : %.1f", (double)fault_vec[i].current);
				commands_printf("Current filtered : %.1f", (double)fault_vec[i].current_filtered);
				commands_printf("Voltage          : %.2f", (double)fault_vec[i].voltage);
				commands_printf("Duty             : %.2f", (double)fault_vec[i].duty);
				commands_printf("RPM              : %.1f", (double)fault_vec[i].rpm);
				commands_printf("Tacho            : %d", fault_vec[i].tacho);
				commands_printf("Cycles running   : %d", fault_vec[i].cycles_running);
				commands_printf("TIM duty         : %d", (int)((float)fault_vec[i].tim_top * fault_vec[i].duty));
				commands_printf("TIM val samp     : %d", fault_vec[i].tim_val_samp);
				commands_printf("TIM current samp : %d", fault_vec[i].tim_current_samp);
				commands_printf("TIM top          : %d", fault_vec[i].tim_top);
				commands_printf("Comm step        : %d", fault_vec[i].comm_step);
				commands_printf("Temperature      : %.2f\n", (double)fault_vec[i].temperature);
			}
		}
	} else if (strcmp(argv[0], "rpm") == 0) {
		commands_printf("Electrical RPM: %.2f rpm\n", (double)mc_interface_get_rpm());
	} else if (strcmp(argv[0], "tacho") == 0) {
		commands_printf("Tachometer counts: %i\n", mc_interface_get_tachometer_value(0));
	} else if (strcmp(argv[0], "tim") == 0) {
		chSysLock();
		volatile int t1_cnt = TIM1->CNT;
		volatile int t8_cnt = TIM8->CNT;
		volatile int dir1 = !!(TIM1->CR1 & (1 << 4));
		volatile int dir8 = !!(TIM8->CR1 & (1 << 4));
		chSysUnlock();
		int duty1 = TIM1->CCR1;
		int duty2 = TIM1->CCR2;
		int duty3 = TIM1->CCR3;
		int top = TIM1->ARR;
		int voltage_samp = TIM8->CCR1;
		int current1_samp = TIM1->CCR4;
		int current2_samp = TIM8->CCR2;
		commands_printf("Tim1 CNT: %i", t1_cnt);
		commands_printf("Tim8 CNT: %u", t8_cnt);
		commands_printf("Duty cycle1: %u", duty1);
		commands_printf("Duty cycle2: %u", duty2);
		commands_printf("Duty cycle3: %u", duty3);
		commands_printf("Top: %u", top);
		commands_printf("Dir1: %u", dir1);
		commands_printf("Dir8: %u", dir8);
		commands_printf("Voltage sample: %u", voltage_samp);
		commands_printf("Current 1 sample: %u", current1_samp);
		commands_printf("Current 2 sample: %u\n", current2_samp);
	} else if (strcmp(argv[0], "volt") == 0) {
		commands_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
	} else if (strcmp(argv[0], "param_detect") == 0) {
		// Use COMM_MODE_DELAY and try to figure out the motor parameters.
		if (argc == 4) {
			float current = -1.0;
			float min_rpm = -1.0;
			float low_duty = -1.0;
			sscanf(argv[1], "%f", &current);
			sscanf(argv[2], "%f", &min_rpm);
			sscanf(argv[3], "%f", &low_duty);

			if (current > 0.0 && current < mcconf.l_current_max &&
					min_rpm > 10.0 && min_rpm < 3000.0 &&
					low_duty > 0.02 && low_duty < 0.8) {

				float cycle_integrator;
				float coupling_k;
				int8_t hall_table[8];
				int hall_res;
				if (conf_general_detect_motor_param(current, min_rpm, low_duty, &cycle_integrator, &coupling_k, hall_table, &hall_res)) {
					commands_printf("Cycle integrator limit: %.2f", (double)cycle_integrator);
					commands_printf("Coupling factor: %.2f", (double)coupling_k);

					if (hall_res == 0) {
						commands_printf("Detected hall sensor table:");
						commands_printf("%i, %i, %i, %i, %i, %i, %i, %i\n",
								hall_table[0], hall_table[1], hall_table[2], hall_table[3],
								hall_table[4], hall_table[5], hall_table[6], hall_table[7]);
					} else if (hall_res == -1) {
						commands_printf("Hall sensor detection failed:");
						commands_printf("%i, %i, %i, %i, %i, %i, %i, %i\n",
								hall_table[0], hall_table[1], hall_table[2], hall_table[3],
								hall_table[4], hall_table[5], hall_table[6], hall_table[7]);
					} else if (hall_res == -2) {
						commands_printf("WS2811 enabled. Hall sensors cannot be used.\n");
					} else if (hall_res == -3) {
						commands_printf("Encoder enabled. Hall sensors cannot be used.\n");
					}
				} else {
					commands_printf("Detection failed. Try again with different parameters.\n");
				}
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires three arguments.\n");
		}
	} else if (strcmp(argv[0], "rpm_dep") == 0) {
		mc_rpm_dep_struct rpm_dep = mcpwm_get_rpm_dep();
		commands_printf("Cycle int limit: %.2f", (double)rpm_dep.cycle_int_limit);
		commands_printf("Cycle int limit running: %.2f", (double)rpm_dep.cycle_int_limit_running);
		commands_printf("Cycle int limit max: %.2f\n", (double)rpm_dep.cycle_int_limit_max);
	} else if (strcmp(argv[0], "can_devs") == 0) {
		commands_printf("CAN devices seen on the bus the past second:\n");
		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
			can_status_msg *msg = comm_can_get_status_msg_index(i);

			if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1.0) {
				commands_printf("ID                 : %i", msg->id);
				commands_printf("RX Time            : %i", msg->rx_time);
				commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(msg->rx_time) * 1000.0));
				commands_printf("RPM                : %.2f", (double)msg->rpm);
				commands_printf("Current            : %.2f", (double)msg->current);
				commands_printf("Duty               : %.2f\n", (double)msg->duty);
			}
		}
	} else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
		if (argc == 2) {
			float current = -1.0;
			sscanf(argv[1], "%f", &current);

			if (current > 0.0 && current <= mcconf.l_current_max) {
				if (encoder_is_configured()) {
					mc_motor_type type_old = mcconf.motor_type;
					mcconf.motor_type = MOTOR_TYPE_FOC;
					mc_interface_set_configuration(&mcconf);

					float offset = 0.0;
					float ratio = 0.0;
					bool inverted = false;
					mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);

					mcconf.motor_type = type_old;
					mc_interface_set_configuration(&mcconf);

					commands_printf("Offset   : %.2f", (double)offset);
					commands_printf("Ratio    : %.2f", (double)ratio);
					commands_printf("Inverted : %s\n", inverted ? "true" : "false");
				} else {
					commands_printf("Encoder not enabled.\n");
				}
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_res") == 0) {
		if (argc == 2) {
			float current = -1.0;
			sscanf(argv[1], "%f", &current);

			if (current > 0.0 && current <= mcconf.l_current_max) {
				mcconf.motor_type = MOTOR_TYPE_FOC;
				mc_interface_set_configuration(&mcconf);

				commands_printf("Resistance: %.6f ohm\n", (double)mcpwm_foc_measure_resistance(current, 2000));

				mc_interface_set_configuration(&mcconf_old);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_ind") == 0) {
		if (argc == 2) {
			float duty = -1.0;
			sscanf(argv[1], "%f", &duty);

			if (duty > 0.0) {
				mcconf.motor_type = MOTOR_TYPE_FOC;
				mcconf.foc_f_sw = 3000.0;
				mc_interface_set_configuration(&mcconf);

				commands_printf("Inductance: %.2f microhenry\n", (double)(mcpwm_foc_measure_inductance(duty, 200, 0)));

				mc_interface_set_configuration(&mcconf_old);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_linkage") == 0) {
		if (argc == 5) {
			float current = -1.0;
			float duty = -1.0;
			float min_erpm = -1.0;
			float res = -1.0;
			sscanf(argv[1], "%f", &current);
			sscanf(argv[2], "%f", &duty);
			sscanf(argv[3], "%f", &min_erpm);
			sscanf(argv[4], "%f", &res);

			if (current > 0.0 && current <= mcconf.l_current_max && min_erpm > 0.0 && duty > 0.02 && res >= 0.0) {
				float linkage;
				conf_general_measure_flux_linkage(current, duty, min_erpm, res, &linkage);
				commands_printf("Flux linkage: %.7f\n", (double)linkage);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "measure_res_ind") == 0) {
		mcconf.motor_type = MOTOR_TYPE_FOC;
		mc_interface_set_configuration(&mcconf);

		float res = 0.0;
		float ind = 0.0;
		mcpwm_foc_measure_res_ind(&res, &ind);
		commands_printf("Resistance: %.6f ohm", (double)res);
		commands_printf("Inductance: %.2f microhenry\n", (double)ind);

		mc_interface_set_configuration(&mcconf_old);
	} else if (strcmp(argv[0], "measure_linkage_foc") == 0) {
		if (argc == 2) {
			float duty = -1.0;
			sscanf(argv[1], "%f", &duty);

			if (duty > 0.0) {
				mcconf.motor_type = MOTOR_TYPE_FOC;
				mc_interface_set_configuration(&mcconf);
				const float res = (3.0 / 2.0) * mcconf.foc_motor_r;

				// Disable timeout
				systime_t tout = timeout_get_timeout_msec();
				float tout_c = timeout_get_brake_current();
				timeout_configure(60000, 0.0);

				for (int i = 0;i < 100;i++) {
					mc_interface_set_duty(((float)i / 100.0) * duty);
					chThdSleepMilliseconds(20);
				}

				float vq_avg = 0.0;
				float rpm_avg = 0.0;
				float samples = 0.0;
				float iq_avg = 0.0;
				for (int i = 0;i < 1000;i++) {
					vq_avg += mcpwm_foc_get_vq();
					rpm_avg += mc_interface_get_rpm();
					iq_avg += mc_interface_get_tot_current_directional();
					samples += 1.0;
					chThdSleepMilliseconds(1);
				}

				mc_interface_release_motor();
				mc_interface_set_configuration(&mcconf_old);

				// Enable timeout
				timeout_configure(tout, tout_c);

				vq_avg /= samples;
				rpm_avg /= samples;
				iq_avg /= samples;

				float linkage = (vq_avg - res * iq_avg) / (rpm_avg * ((2.0 * M_PI) / 60.0));

				commands_printf("Flux linkage: %.7f\n", (double)linkage);
			} else {
				commands_printf("Invalid argument(s).\n");
			}
		} else {
			commands_printf("This command requires one argument.\n");
		}
	} else if (strcmp(argv[0], "foc_state") == 0) {
		mcpwm_foc_print_state();
		commands_printf(" ");
	}

	// The help command
	else if (strcmp(argv[0], "help") == 0) {
		commands_printf("Valid commands are:");
		commands_printf("help");
		commands_printf("  Show this help");

		commands_printf("ping");
		commands_printf("  Print pong here to see if the reply works");

		commands_printf("stop");
		commands_printf("  Stop the motor");

		commands_printf("last_adc_duration");
		commands_printf("  The time the latest ADC interrupt consumed");

		commands_printf("kv");
		commands_printf("  The calculated kv of the motor");

		commands_printf("mem");
		commands_printf("  Show memory usage");

		commands_printf("threads");
		commands_printf("  List all threads");

		commands_printf("fault");
		commands_printf("  Prints the current fault code");

		commands_printf("faults");
		commands_printf("  Prints all stored fault codes and conditions when they arrived");

		commands_printf("rpm");
		commands_printf("  Prints the current electrical RPM");

		commands_printf("tacho");
		commands_printf("  Prints tachometer value");

		commands_printf("tim");
		commands_printf("  Prints tim1 and tim8 settings");

		commands_printf("volt");
		commands_printf("  Prints different voltages");

		commands_printf("param_detect [current] [min_rpm] [low_duty]");
		commands_printf("  Spin up the motor in COMM_MODE_DELAY and compute its parameters.");
		commands_printf("  This test should be performed without load on the motor.");
		commands_printf("  Example: param_detect 5.0 600 0.06");

		commands_printf("rpm_dep");
		commands_printf("  Prints some rpm-dep values");

		commands_printf("can_devs");
		commands_printf("  Prints all CAN devices seen on the bus the past second");

		commands_printf("foc_encoder_detect [current]");
		commands_printf("  Run the motor at 1Hz on open loop and compute encoder settings");

		commands_printf("measure_res [current]");
		commands_printf("  Lock the motor with a current and calculate its resistance");

		commands_printf("measure_ind [duty]");
		commands_printf("  Send short voltage pulses, measure the current and calculate the motor inductance");

		commands_printf("measure_linkage [current] [duty] [min_rpm] [motor_res]");
		commands_printf("  Run the motor in BLDC delay mode and measure the flux linkage");
		commands_printf("  example measure_linkage 5 0.5 700 0.076");
		commands_printf("  tip: measure the resistance with measure_res first");

		commands_printf("measure_res_ind");
		commands_printf("  Measure the motor resistance and inductance with an incremental adaptive algorithm.");

		commands_printf("measure_linkage_foc [duty]");
		commands_printf("  Run the motor with FOC and measure the flux linkage.");

		commands_printf("foc_state");
		commands_printf("  Print some FOC state variables.\n");
	} else {
		commands_printf("Invalid command: %s\n"
				"type help to list all available commands\n", argv[0]);
	}
}
Exemplo n.º 2
0
static msg_t adc_thread(void *arg) {
    (void)arg;

    chRegSetThreadName("APP_ADC");

    // Set servo pin as an input with pullup
    palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT_PULLUP);

    for(;;) {
        // Sleep for a time according to the specified rate
        systime_t sleep_time = CH_FREQUENCY / config.update_rate_hz;

        // At least one tick should be slept to not block the other threads
        if (sleep_time == 0) {
            sleep_time = 1;
        }
        chThdSleep(sleep_time);

        // Read the external ADC pin and convert the value to a voltage.
        float pwr = (float)ADC_Value[ADC_IND_EXT];
        pwr /= 4095;
        pwr *= V_REG;

        read_voltage = pwr;

        // Optionally apply a mean value filter
        if (config.use_filter) {
            static float filter_buffer[FILTER_SAMPLES];
            static int filter_ptr = 0;

            filter_buffer[filter_ptr++] = pwr;
            if (filter_ptr >= FILTER_SAMPLES) {
                filter_ptr = 0;
            }

            pwr = 0.0;
            for (int i = 0; i < FILTER_SAMPLES; i++) {
                pwr += filter_buffer[i];
            }
            pwr /= FILTER_SAMPLES;
        }

        // Map and truncate the read voltage
        pwr = utils_map(pwr, config.voltage_start, config.voltage_end, 0.0, 1.0);
        utils_truncate_number(&pwr, 0.0, 1.0);

        // Optionally invert the read voltage
        if (config.voltage_inverted) {
            pwr = 1.0 - pwr;
        }

        decoded_level = pwr;

        // Read the servo pin and optionally invert it.
        bool button_val = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
        if (config.button_inverted) {
            button_val = !button_val;
        }

        switch (config.ctrl_type) {
        case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
        case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
        case ADC_CTRL_TYPE_DUTY_REV_CENTER:
            // Scale the voltage and set 0 at the center
            pwr *= 2.0;
            pwr -= 1.0;
            break;

        case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
        case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
        case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
            // Invert the voltage if the button is pressed
            if (button_val) {
                pwr = -pwr;
            }
            break;

        default:
            break;
        }

        // Apply a deadband
        utils_deadband(&pwr, config.hyst, 1.0);

        float current = 0;
        bool current_mode = false;
        bool current_mode_brake = false;
        const volatile mc_configuration *mcconf = mcpwm_get_configuration();
        bool send_duty = false;

        // Use the filtered and mapped voltage for control according to the configuration.
        switch (config.ctrl_type) {
        case ADC_CTRL_TYPE_CURRENT:
        case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
        case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
            current_mode = true;
            if (pwr >= 0.0) {
                current = pwr * mcconf->l_current_max;
            } else {
                current = pwr * fabsf(mcconf->l_current_min);
            }

            if (fabsf(pwr) < 0.001) {
                ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
            }
            break;

        case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
        case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
            current_mode = true;
            if (pwr >= 0.0) {
                current = pwr * mcconf->l_current_max;
            } else {
                current = fabsf(pwr * mcconf->l_current_min);
                current_mode_brake = true;
            }

            if (pwr < 0.001) {
                ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
            }
            break;

        case ADC_CTRL_TYPE_DUTY:
        case ADC_CTRL_TYPE_DUTY_REV_CENTER:
        case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
            if (fabsf(pwr) < 0.001) {
                ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
            }

            if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
                mcpwm_set_duty(pwr);
                send_duty = true;
            }
            break;

        default:
            continue;
        }

        // If safe start is enabled and the output has not been zero for long enough
        if (ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start) {
            static int pulses_without_power_before = 0;
            if (ms_without_power == pulses_without_power_before) {
                ms_without_power = 0;
            }
            pulses_without_power_before = ms_without_power;
            mcpwm_set_brake_current(timeout_get_brake_current());
            continue;
        }

        // Reset timeout
        timeout_reset();

        // Find lowest RPM (for traction control)
        float rpm_local = mcpwm_get_rpm();
        float rpm_lowest = rpm_local;
        if (config.multi_esc) {
            for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
                can_status_msg *msg = comm_can_get_status_msg_index(i);

                if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
                    float rpm_tmp = msg->rpm;

                    if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
                        rpm_lowest = rpm_tmp;
                    }
                }
            }
        }

        // Optionally send the duty cycles to the other ESCs seen on the CAN-bus
        if (send_duty && config.multi_esc) {
            float duty = mcpwm_get_duty_cycle_now();

            for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
                can_status_msg *msg = comm_can_get_status_msg_index(i);

                if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
                    comm_can_set_duty(msg->id, duty);
                }
            }
        }

        if (current_mode) {
            if (current_mode_brake) {
                mcpwm_set_brake_current(current);

                // Send brake command to all ESCs seen recently on the CAN bus
                for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
                    can_status_msg *msg = comm_can_get_status_msg_index(i);

                    if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
                        comm_can_set_current_brake(msg->id, current);
                    }
                }
            } else {
                // Apply soft RPM limit
                if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
                    current = mcconf->cc_min_current;
                } else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
                    current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
                } else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
                    current = mcconf->cc_min_current;
                } else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
                    rpm_lowest = -rpm_lowest;
                    current = -current;
                    current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
                    current = -current;
                    rpm_lowest = -rpm_lowest;
                }

                float current_out = current;
                bool is_reverse = false;
                if (current_out < 0.0) {
                    is_reverse = true;
                    current_out = -current_out;
                    current = -current;
                    rpm_local = -rpm_local;
                    rpm_lowest = -rpm_lowest;
                }

                // Traction control
                if (config.multi_esc) {
                    for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
                        can_status_msg *msg = comm_can_get_status_msg_index(i);

                        if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
                            if (config.tc) {
                                float rpm_tmp = msg->rpm;
                                if (is_reverse) {
                                    rpm_tmp = -rpm_tmp;
                                }

                                float diff = rpm_tmp - rpm_lowest;
                                current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
                                if (current_out < mcconf->cc_min_current) {
                                    current_out = 0.0;
                                }
                            }

                            if (is_reverse) {
                                comm_can_set_current(msg->id, -current_out);
                            } else {
                                comm_can_set_current(msg->id, current_out);
                            }
                        }
                    }

                    if (config.tc) {
                        float diff = rpm_local - rpm_lowest;
                        current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
                        if (current_out < mcconf->cc_min_current) {
                            current_out = 0.0;
                        }
                    }
                }

                if (is_reverse) {
                    mcpwm_set_current(-current_out);
                } else {
                    mcpwm_set_current(current_out);
                }
            }
        }
    }

    return 0;
}