优化底盘 (0b4c640a) · Commits · Wenxi XU / Mambo

drivers/chassis/chassis.c

+49 −37

Original line number	Diff line number	Diff line
		@@ -66,13 +66,8 @@ void cchassis_set_speed(const struct device *dev, float x_speed, float y_speed)
		void cchassis_set_gyro(const struct device *dev, float gyro)
		{
		chassis_data_t *data = dev->data;
		if (!float_equal(gyro, 0)) {
		data->targetGyro = gyro;
		data->angleControl = false;
		} else if (!data->angleControl) {
		data->targetYaw = data->chassis_sensor_data.Yaw;
		data->angleControl = true;
		}
		}

		chassis_status_t cchassis_get_status(const struct device dev)
		@@ -84,6 +79,19 @@ chassis_status_t cchassis_get_status(const struct device dev)
		void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		{
		// data->targetRollSpeed = 0;
		float error = 0;
		if (!isnan(data->chassis_sensor_data.Yaw)) {
		float delta_Yaw = data->chassis_sensor_data.Yaw - data->targetYaw;
		delta_Yaw = fmodf(delta_Yaw, 360.0f);
		if (delta_Yaw > 180) {
		delta_Yaw -= 360.0f;
		} else if (delta_Yaw < -180) {
		delta_Yaw += 360.0f;
		}
		if (fabsf(delta_Yaw) > 0.8f) {
		error = delta_Yaw;
		}
		}

		int idx = 0;
		float currentSpeedX[CHASSIS_WHEEL_COUNT] = {0};
		@@ -106,22 +114,47 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		idx++;
		}

		float delta_speed_X = data->targetXSpeed - data->currentXSpeed;
		float delta_speed_Y = data->targetYSpeed - data->currentYSpeed;
		float delta_speed = sqrtf(delta_speed_X * delta_speed_X + delta_speed_Y * delta_speed_Y);
		float delta_speed_angle = atan2f(delta_speed_Y, delta_speed_X);
		delta_speed = delta_speed > cfg->max_lin_accel ? cfg->max_lin_accel
		: delta_speed < -cfg->max_lin_accel ? -cfg->max_lin_accel
		: delta_speed;
		data->currentXSpeed += delta_speed * cosf(delta_speed_angle);
		data->currentYSpeed += delta_speed * sinf(delta_speed_angle);
		float currentSpeed = sqrtf(data->currentXSpeed * data->currentXSpeed +
		data->currentYSpeed * data->currentYSpeed);

		if (!isnan(data->chassis_sensor_data.Yaw)) {
		if (!data->angleControl) {
		data->currentGyro = data->targetGyro;
		} else {
		data->currentGyro =
		pid_calc_in(cfg->angle_pid, error,
		k_cyc_to_us_near32(data->currTime - data->prevTime));
		}
		}
		data->currentGyro = data->currentGyro > cfg->max_gyro ? cfg->max_gyro
		: data->currentGyro < -cfg->max_gyro ? -cfg->max_gyro
		: data->currentGyro;

		idx = 0;
		while (cfg->wheels[idx] != NULL) {
		float rollSpeedX = cfg->pos_Y_offset[idx] * data->targetGyro;
		float rollSpeedY = -cfg->pos_X_offset[idx] * data->targetGyro;
		float rollSpeedX = cfg->pos_Y_offset[idx] * data->currentGyro;
		float rollSpeedY = -cfg->pos_X_offset[idx] * data->currentGyro;
		float speedX, speedY;
		if (!data->track_angle) {
		speedX = rollSpeedX + data->targetXSpeed;
		speedY = rollSpeedY + data->targetYSpeed;
		speedX = rollSpeedX + data->currentXSpeed;
		speedY = rollSpeedY + data->currentYSpeed;
		} else {
		// 将角度转换为弧度
		float radians = -data->chassis_sensor_data.Yaw * PI / 180.0f;
		// 计算旋转后的新速度
		speedX = data->targetXSpeed * cosf(radians) -
		data->targetYSpeed * sinf(radians) + rollSpeedX;
		speedY = data->targetXSpeed * sinf(radians) +
		data->targetYSpeed * cosf(radians) + rollSpeedY;
		speedX = data->currentXSpeed * cosf(radians) -
		data->currentYSpeed * sinf(radians) + rollSpeedX;
		speedY = data->currentXSpeed * sinf(radians) +
		data->currentYSpeed * cosf(radians) + rollSpeedY;
		}
		float steerwheel_speed;
		arm_sqrt_f32(speedX * speedX + speedY * speedY, &steerwheel_speed);
		@@ -130,7 +163,7 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)

		steerwheel_angle = -RAD2DEG(steerwheel_angle) + 90.0f;

		if (fabsf(steerwheel_speed) > 0.1f \|\| fabsf(data->targetGyro) > 0.15f) {
		if (fabsf(steerwheel_speed) > 0.1f \|\| fabsf(data->currentGyro) > 0.15f) {
		wheel_set_speed(cfg->wheels[idx], steerwheel_speed, steerwheel_angle);
		} else {
		int err = wheel_set_static(cfg->wheels[idx],
		@@ -165,27 +198,6 @@ void cchassis_main_thread(const struct device dev, void ptr2, void *ptr3)
		while (!data->enabled) {
		k_msleep(100);
		}
		float error = 0;
		if (!isnan(data->chassis_sensor_data.Yaw)) {
		if (data->angleControl) {
		float delta_Yaw = data->chassis_sensor_data.Yaw - data->targetYaw;
		delta_Yaw = fmodf(delta_Yaw, 360.0f);
		if (delta_Yaw > 180) {
		delta_Yaw -= 360.0f;
		} else if (delta_Yaw < -180) {
		delta_Yaw += 360.0f;
		}
		if (fabsf(delta_Yaw) > 0.8f) {
		error = delta_Yaw;
		}
		}
		}

		if (data->angleControl && !isnan(data->chassis_sensor_data.Yaw)) {
		data->targetGyro =
		pid_calc_in(cfg->angle_pid, error,
		k_cyc_to_us_near32(data->currTime - data->prevTime));
		}

		// printk("Yaw: %f, targetYaw: %f, delta_Yaw: %f\n", data->currentYaw,
		// data->targetYaw,

drivers/chassis/chassis.h

+7 −1

Original line number	Diff line number	Diff line
		@@ -26,7 +26,10 @@
		.track_angle = DT_PROP(DT_DRV_INST(inst), track_angle), \
		};

		#define WHEELS_FOREACH(inst, fn) {DT_FOREACH_PROP_ELEM_SEP(DT_DRV_INST(inst), wheels, fn, (,))}
		#define WHEELS_FOREACH(inst, fn) \
		{ \
		DT_FOREACH_PROP_ELEM_SEP(DT_DRV_INST(inst), wheels, fn, (,)) \
		}

		#define GET_WHEEL_DEVICE(node_id, prop, idx) DEVICE_DT_GET(DT_PHANDLE_BY_IDX(node_id, prop, idx))

		@@ -43,6 +46,9 @@
		.wheels = WHEELS_FOREACH(inst, GET_WHEEL_DEVICE), \
		.pos_X_offset = WHEELS_FOREACH(inst, GET_WHEEL_X_OFFSET), \
		.pos_Y_offset = WHEELS_FOREACH(inst, GET_WHEEL_Y_OFFSET), \
		.max_gyro = DT_STRING_UNQUOTED_OR(DT_DRV_INST(inst), max_gyro, 10), \
		.max_lin_accel = \
		DT_STRING_UNQUOTED_OR(DT_DRV_INST(inst), max_linear_accel, 10) / 1000.0f, \
		};

		#define CHASSIS_DEFINE_INST(inst) \

include/zephyr/drivers/chassis.h

+3 −0

Original line number	Diff line number	Diff line
		@@ -49,6 +49,7 @@ typedef struct {
		float targetYaw;

		float targetGyro;
		float currentGyro;

		float targetXSpeed;
		float targetYSpeed;
		@@ -76,6 +77,8 @@ typedef struct {
		const struct device *wheels[CHASSIS_WHEEL_COUNT];
		float pos_X_offset[CHASSIS_WHEEL_COUNT];
		float pos_Y_offset[CHASSIS_WHEEL_COUNT];
		float max_lin_accel;
		float max_gyro, max_lin_speed;
		} chassis_cfg_t;

		/**

Admin message