void Motor_Control() { /* switch(uStateSwicth) { case StopSwitch: // 停车 { xStatus = 0; yStatus = 0; xCarParam.Speed_X = 0; xCarParam.Speed_Y = 0; xCarParam.Speed_Z = 0; xCarParam.EncoderSumY = 0; xCarParam.EncoderSumX = 0; xCarParam.CarDistanceX = 0; xCarParam.CarDistanceY = 0; break; } case CascadeSwitch: // 小车控制速度 { SpeedX_Control(); SpeedY_Control(); break; } } */ //=================EndSwitch================================= Position_PID(&IMU,xCarParam.yaw,tarYaw); xCarParam.Speed_Z = IMU.result; xCarParam.Speed_Y = 0; xCarParam.Speed_X = 0; // 三个速度限幅函数 xCarParam.Speed_X = LimitProtect(SpeedMaxX,-SpeedMaxX,xCarParam.Speed_X); xCarParam.Speed_Y = LimitProtect(SpeedMaxY,-SpeedMaxY,xCarParam.Speed_Y); xCarParam.Speed_Z = LimitProtect(SpeedMaxZ,-SpeedMaxZ,xCarParam.Speed_Z); // 计算占空比 motor[0].duty = xCarParam.Speed_Y + xCarParam.Speed_X + xCarParam.Speed_Z; motor[1].duty = xCarParam.Speed_Y - xCarParam.Speed_X + xCarParam.Speed_Z; motor[2].duty = xCarParam.Speed_Y - xCarParam.Speed_X - xCarParam.Speed_Z; motor[3].duty = xCarParam.Speed_Y + xCarParam.Speed_X - xCarParam.Speed_Z; // 增量式PID 自己有限幅函数 Increment_PID(&(motor[0].pid),encoder_data_quaddec[0],motor[0].duty); Increment_PID(&(motor[1].pid),encoder_data_quaddec[1],motor[1].duty); Increment_PID(&(motor[2].pid),encoder_data_quaddec[2],motor[2].duty); Increment_PID(&(motor[3].pid),encoder_data_quaddec[3],motor[3].duty); // 电机调速 setMotorDuty(&motor[0],motor[0].pid.result); setMotorDuty(&motor[1],motor[1].pid.result); setMotorDuty(&motor[2],motor[2].pid.result); setMotorDuty(&motor[3],motor[3].pid.result); }

这段代码是用于控制电机的函数。

首先，代码中有一段被注释掉的switch语句，根据uStateSwicth的不同值来执行不同的操作。当uStateSwicth等于StopSwitch时，表示停车状态，会将一些参数和状态重置为0。当uStateSwicth等于CascadeSwitch时，表示小车控制速度状态，会调用SpeedX_Control和SpeedY_Control函数。

接下来，在注释掉的代码之后，通过位置PID控制来计算小车的速度控制信号。具体来说，通过Position_PID函数计算出目标偏航角与当前偏航角的差值，并将其作为输入量。然后将计算得到的速度控制信号赋值给xCarParam.Speed_Z。

然后，将xCarParam.Speed_Y和xCarParam.Speed_X设置为0。

接着，对三个速度进行限幅处理，确保其在设定的上限和下限范围内。

然后，根据计算得到的速度以及电机的布置方式，计算出每个电机的占空比。

接下来，使用增量式PID控制算法对每个电机进行调速。通过Increment_PID函数计算出每个电机的控制量。

最后，使用setMotorDuty函数将计算得到的控制量设置给每个电机。

这段代码实现了电机的控制逻辑，根据目标偏航角和速度的设定，通过PID控制算法实现电机的调速。