_chassis_data.delta=atan(2*L1/(L2+2*L2/(fabs(_driver_data.left_speed)/fabs(_driver_data.right_speed)-1)))*180.0f/PI;
时间: 2024-04-05 11:30:20 浏览: 18
这段代码是用来计算车辆前轮转角的角度值的。其中:
- `_driver_data.left_speed` 表示左轮的转速;
- `_driver_data.right_speed` 表示右轮的转速;
- `L1` 表示车辆前后轮的轴距;
- `L2` 表示车辆左右轮的轴距;
- `fabs()` 函数是求绝对值的函数;
- `atan()` 函数是求反正切值的函数;
- `PI` 是圆周率。
具体来说,这段代码通过计算左右轮的转速比值,来推算车辆前轮需要转动的角度。其中,`2*L1/(L2+2*L2/(fabs(_driver_data.left_speed)/fabs(_driver_data.right_speed)-1))` 这部分计算出了前轮的转角所对应的正切值,然后通过 `atan()` 计算出了前轮的转角度数。最后乘以 `180.0f/PI`,将弧度值转换为度数。
相关问题
else if(_chassis_data.mode == CONTROL_MODE_REMOTE) count_soc_down = 0;
这是一个条件语句,其含义为:如果底盘数据模式为远程控制模式(CONTROL_MODE_REMOTE),那么将计数器count_soc_down的值设为0。其中,else if表示如果前面的条件不成立,则执行这个条件语句;_chassis_data.mode表示底盘数据的模式;==是一个比较运算符,表示判断左右两侧的值是否相等;CONTROL_MODE_REMOTE是一个常量或枚举值,表示远程控制模式;count_soc_down是一个变量,表示一个电池的剩余电量计数器。因此,这行代码的作用是在远程控制模式下重置电池剩余电量计数器。
解释这段代码static void chassis_control_loop(chassis_move_t *chassis_move_control_loop) { fp32 max_vector = 0.0f, vector_rate = 0.0f; fp32 temp = 0.0f; fp32 wheel_speed[4] = {0.0f, 0.0f, 0.0f, 0.0f}; uint8_t i = 0; float position_error, speed_error; float position_output, speed_output; float current_position, current_speed; float target_position, target_speed; chassis_move_control_loop->vx_set=vx_set; chassis_move_control_loop->vy_set=vy_set; chassis_move_control_loop->wz_set=angle_set; chassis_vector_to_mecanum_wheel_speed(chassis_move_control_loop->vx_set, chassis_move_control_loop->vy_set, chassis_move_control_loop->wz_set, wheel_speed); if (chassis_move_control_loop->chassis_mode == CHASSIS_VECTOR_RAW) { for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].give_current = (int16_t)(wheel_speed[i]); } } for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].speed_set = wheel_speed[i]; temp = fabs(chassis_move_control_loop->motor_chassis[i].speed_set); if (max_vector < temp) { max_vector = temp; } } if (max_vector > MAX_WHEEL_SPEED) { vector_rate = MAX_WHEEL_SPEED / max_vector; for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].speed_set *= vector_rate; } } for (i = 0; i < 4; i++) { PID_Calc(&chassis_move_control_loop->motor_speed_pid[i], chassis_move_control_loop->motor_chassis[i].speed, chassis_move_control_loop->motor_chassis[i].speed_set); } for (i = 0; i < 4; i++) { chassis_move_control_loop->motor_chassis[i].give_current = (int16_t)(chassis_move_control_loop->motor_speed_pid[i].out); } }
chassis_move_control_loop->motor_chassis[i].position_pid, chassis_move_control_loop->motor_chassis[i].speed_pid, chassis_move_control_loop->motor_chassis[i].position_get, chassis_move_control_loop->motor_chassis[i].speed_get, chassis_move_control_loop->motor_chassis[i].speed_set, &position_error, &speed_error, &position_output, &speed_output); current_position = chassis_move_control_loop->motor_chassis[i].position_get; current_speed = chassis_move_control_loop->motor_chassis[i].speed_get; target_position = chassis_move_control_loop->motor_chassis[i].position_set; target_speed = speed_output; if(chassis_move_control_loop->motor_chassis[i].position_pid.enable == 1) //PID启动 { chassis_move_control_loop->motor_chassis[i].give_current = PID_Calc(&chassis_move_control_loop->motor_chassis[i].position_pid, current_position, target_position); } else { chassis_move_control_loop->motor_chassis[i].give_current = PID_Calc(&chassis_move_control_loop->motor_chassis[i].speed_pid, current_speed, target_speed); } } }
该函数为底盘控制代码,主要实现底盘的位置和速度控制。
具体实现方法为:
1.根据控制指令,将底盘的速度向量转换为各个驱动电机的速度。
2.根据底盘模式选择不同的控制方式:
如果模式为 CHASSIS_VECTOR_RAW,直接将每个驱动电机的输出电流设置为对应速度。
3.根据电机的最大速度设置,对转换后的速度进行限制。
4.对每个驱动电机进行PID控制,计算出目标位置和目标速度,并根据位置控制或速度控制模式下达电机电流控制指令。
5.将目标速度和目标位置在PID计算中使用,并将计算出的电流控制指令转换为电机的输出电流。
相关推荐
![pdf](https://img-home.csdnimg.cn/images/20210720083512.png)
![gz](https://img-home.csdnimg.cn/images/20210720083447.png)
![pdf](https://img-home.csdnimg.cn/images/20210720083512.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)