Bugs fixed in chassis driver (77d5ab1d) · Commits · Wenxi XU / Mambo

drivers/chassis/swchassis.c

+24 −21

Original line number	Diff line number	Diff line
		@@ -38,10 +38,8 @@ int swchassis_init(const struct device *dev)
		while (cfg->steerwheels[idx] != NULL) {
		float arc;
		arm_atan2_f32(cfg->pos_Y_offset[idx], cfg->pos_X_offset[idx], &arc);
		if (arc < 0) {
		arc += 2 * PI;
		}
		data->angle_to_center[idx] = RAD2DEG(arc);

		data->angle_to_center[idx] = -RAD2DEG(arc) + 90.0f;
		arm_sqrt_f32(cfg->pos_X_offset[idx] * cfg->pos_X_offset[idx] +
		cfg->pos_Y_offset[idx] * cfg->pos_Y_offset[idx],
		&data->distance_to_center[idx]);
		@@ -134,14 +132,17 @@ void swchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)

		steerwheel_angle = -RAD2DEG(steerwheel_angle) + 90.0f;

		printk("deg: %f\n", (double)steerwheel_angle);

		if (fabsf(steerwheel_speed) > 0.06f \|\| fabsf(data->targetGyro) > 0.2f) {
		if (fabsf(steerwheel_speed) > 0.1f \|\| fabsf(data->targetGyro) > 0.15f) {
		steerwheel_set_angle(cfg->steerwheels[idx], steerwheel_angle);
		steerwheel_set_speed(cfg->steerwheels[idx], steerwheel_speed);
		} else {
		int err = steerwheel_set_static(cfg->steerwheels[idx]);
		if (err < 0) {
		steerwheel_set_speed(cfg->steerwheels[idx], 0);
		}
		steerwheel_set_angle(cfg->steerwheels[idx],
		fmodf(data->angle_to_center[idx] + 90.0f, 360.0f));
		}

		idx++;
		}
		@@ -156,6 +157,13 @@ void swchassis_main_thread(const struct device dev, void ptr2, void *ptr3)
		chassis_data_t *data = dev->data;
		const chassis_cfg_t *cfg = dev->config;

		for (int i = 0; i < CONFIG_CHASSIS_MAX_STEERWHHEL_COUNT; i++) {
		if (cfg->steerwheels[i] == NULL) {
		break;
		}
		steerwheel_set_static(cfg->steerwheels[i]);
		}

		struct pos_data pos = {0};

		while (true) {
		@@ -171,18 +179,13 @@ void swchassis_main_thread(const struct device dev, void ptr2, void *ptr3)

		float delta_Yaw = data->targetYaw - data->currentYaw;
		delta_Yaw = fmodf(delta_Yaw, 360.0f);
		if (delta_Yaw > 0) {
		if (delta_Yaw < 180) {
		data->pid_input = delta_Yaw;
		} else {
		data->pid_input = delta_Yaw - 360.0f;
		if (delta_Yaw > 180) {
		delta_Yaw -= 360.0f;
		} else if (delta_Yaw < -180) {
		delta_Yaw += 360.0f;
		}
		} else if (delta_Yaw < 0) {
		if (delta_Yaw > -180) {
		if (fabsf(delta_Yaw) > 0.8f) {
		data->pid_input = delta_Yaw;
		} else {
		data->pid_input = delta_Yaw + 360.0f;
		}
		}
		if ((pos.accel[2] - 9.8f) > 0.4f) {
		// We are in the air
		@@ -199,6 +202,6 @@ struct chassis_driver_api swchassis_driver_api = {
		};

		K_THREAD_DEFINE(chassis_thread, CHASSIS_STACK_SIZE, swchassis_main_thread,
		DEVICE_DT_GET(DT_INST(0, DT_DRV_COMPAT)), NULL, NULL, 1, 0, 1000);
		DEVICE_DT_GET(DT_INST(0, DT_DRV_COMPAT)), NULL, NULL, 1, 0, 0);

		DT_INST_FOREACH_STATUS_OKAY(CHASSIS_INIT)
		No newline at end of file

include/ares/Kconfig

0 → 100644

+17 −0

Original line number	Diff line number	Diff line
		# ARES configuration options

		# Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn>
		# SPDX-License-Identifier: Apache-2.0

		menuconfig ARES
		bool "ARES options"
		help
		Enable support for ares modules.

		module = ARES
		module-str = ARES

		config IMU_PWM_TEMP_CTRL
		bool "IMU_PWM_TEMP_CTRL"
		help
		Enable IMU PWM TEMP CTRL
		No newline at end of file

include/zephyr/drivers/chassis.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -16,7 +16,9 @@
		#include <zephyr/device.h>
		#include <zephyr/drivers/steerwheel.h>

		#ifndef RADS_TO_RPM
		#define RADPS_TO_RPM 6.28318531f
		#endif

		#ifdef __cplusplus
		extern "C" {

include/zephyr/drivers/steerwheel.h

+42 −1

Original line number	Diff line number	Diff line
		@@ -45,8 +45,10 @@ typedef struct {

		float set_angle;
		float set_rpm;
		float static_angle;

		bool negative;
		bool initialized;
		} steerwheel_data_t;

		static inline void steerwheel_set_speed(const struct device *dev, float speed)
		@@ -54,9 +56,17 @@ static inline void steerwheel_set_speed(const struct device *dev, float speed)
		steerwheel_data_t *data = dev->data;
		const steerwheel_cfg_t *cfg = dev->config;

		if (!data->initialized) {
		data->initialized = true;
		data->static_angle = NAN;
		motor_set_torque(cfg->steer_motor, 0);
		}

		data->current_angle = motor_get_angle(cfg->steer_motor);
		data->current_speed = motor_get_speed(cfg->wheel_motor);

		data->static_angle = NAN;

		if (data->current_speed < 0) {
		data->current_speed = -data->current_speed;
		data->current_angle =
		@@ -69,11 +79,40 @@ static inline void steerwheel_set_speed(const struct device *dev, float speed)
		motor_set_speed(cfg->wheel_motor, data->set_rpm);
		}

		static inline int steerwheel_set_static(const struct device *dev)
		{
		steerwheel_data_t *data = dev->data;
		const steerwheel_cfg_t *cfg = dev->config;

		data->set_rpm = 0;

		if (!data->initialized) {
		data->initialized = true;
		data->static_angle = NAN;
		motor_set_torque(cfg->steer_motor, 0);
		}

		data->current_angle = motor_get_angle(cfg->steer_motor);
		data->current_speed = motor_get_speed(cfg->wheel_motor);

		if (isnan(data->static_angle)) {
		data->static_angle = motor_get_angle(cfg->wheel_motor);
		}

		return motor_set_angle(cfg->wheel_motor, data->static_angle);
		}

		static inline void steerwheel_set_angle(const struct device *dev, float angle)
		{
		steerwheel_data_t *data = dev->data;
		const steerwheel_cfg_t *cfg = dev->config;

		if (!data->initialized) {
		data->initialized = true;
		data->static_angle = NAN;
		motor_set_torque(cfg->wheel_motor, 0);
		}

		data->current_angle = motor_get_angle(cfg->steer_motor);
		data->current_speed = motor_get_speed(cfg->wheel_motor);

		@@ -128,9 +167,11 @@ static inline void steerwheel_set_angle(const struct device *dev, float angle)

		if (data->negative != prev_nega) {
		data->set_rpm = -data->set_rpm;
		if (isnan(data->static_angle)) {
		motor_set_speed(cfg->wheel_motor, data->set_rpm);
		}
		}
		}

		static inline float steerwheel_get_speed(const struct device *dev)
		{

Admin message