小车电机和舵机控制

void Drive_Motor(float Vx,float Vy,float Vz)
{
		float amplitude=3.5; //Wheel target speed limit //车轮目标速度限幅
 
		//Ackermann structure car
		//阿克曼小车
		if (Car_Mode==Akm_Car) 
		{
			//Ackerman car specific related variables //阿克曼小车专用相关变量
			int K=1000;
			float Ratio=1, Angle;
 
			// For Ackerman small car, Vz represents the front wheel steering Angle
			//对于阿克曼小车Vz代表前轮转向角度
			Angle=Vz;
 
			// Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad
			//前轮转向角度限幅(舵机控制前轮转向角度)，单位：rad
			Angle=target_limit_float(Angle,-0.35f,0.35f);
 
			// The software compensates for the front wheel steering Angle due to mechanical structure limitations
			//机械结构限制，软件对前轮转向角度进行补偿
			if(Angle<0)Ratio=1.054;
			else if(Angle>0)Ratio=0.838;
			else Ratio=0;
 
			//Inverse kinematics //运动学逆解
			MOTOR_A.Target   = Vx*(1-Wheel_spacing*tan(Angle)/2/Axle_spacing);
			MOTOR_B.Target   = Vx*(1+Wheel_spacing*tan(Angle)/2/Axle_spacing);
			// The PWM value of the servo controls the steering Angle of the front wheel
			//舵机PWM值，舵机控制前轮转向角度
			Servo=(SERVO_INIT-Angle*K*Ratio); 
 
			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
			MOTOR_C.Target=0; //Out of use //没有使用到
			MOTOR_D.Target=0; //Out of use //没有使用到
			Servo=target_limit_int(Servo,900,2000);	//Servo PWM value limit //舵机PWM值限幅
		}		
}

void Balance_task(void *pvParameters)
{ 
	  u32 lastWakeTime = getSysTickCnt();
    while(1)
    {	
			// This task runs at a frequency of 100Hz (10ms control once)
			//此任务以100Hz的频率运行（10ms控制一次）
			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); 
 
			//Time count is no longer needed after 30 seconds
			//时间计数，30秒后不再需要
			if(Time_count<3000)Time_count++;
 
			//Get the encoder data, that is, the real time wheel speed, 
			//and convert to transposition international units
			//获取编码器数据，即车轮实时速度，并转换位国际单位
			Get_Velocity_Form_Encoder();   
 
			if(Check==0) //If self-check mode is not enabled //如果没有启动自检模式
			{
				if      (APP_ON_Flag)      Get_RC();         //Handle the APP remote commands //处理APP遥控命令
				else if (Remote_ON_Flag)   Remote_Control(); //Handle model aircraft remote commands //处理航模遥控命令
				else if (PS2_ON_Flag)      PS2_control();    //Handle PS2 controller commands //处理PS2手柄控制命令
 
				//CAN, Usart 1, Usart 3 control can directly get the three axis target speed, 
				//without additional processing
				//CAN、串口1、串口3(ROS)控制直接得到三轴目标速度，无须额外处理
				else                      Drive_Motor(Move_X, Move_Y, Move_Z);
 
				//Click the user button to update the gyroscope zero
				//单击用户按键更新陀螺仪零点
				Key(); 
 
				//If there is no abnormity in the battery voltage, and the enable switch is in the ON position,
        //and the software failure flag is 0
				//如果电池电压不存在异常，而且使能开关在ON档位，而且软件失能标志位为0
				if(Turn_Off(Voltage)==0) 
				 { 			
           //Speed closed-loop control to calculate the PWM value of each motor, 
					 //PWM represents the actual wheel speed					 
					 //速度闭环控制计算各电机PWM值，PWM代表车轮实际转速
					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
 
					 //Set different PWM control polarity according to different car models
					 //根据不同小车型号设置不同的PWM控制极性
					 switch(Car_Mode)
					 {
							case Akm_Car:       Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, Servo); break; //Ackermann structure car //阿克曼小车
					 }
				 }
				 //If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0
				 //如果Turn_Off(Voltage)返回值为1，不允许控制小车进行运动，PWM值设置为0
				 else	Set_Pwm(0,0,0,0,0); 
			 }	
			 //If self-check mode is enabled, the run self-check function is executed
			 //如果开启了自检模式，则执行运行自检函数
       else CheckTask(); 
		 }  
}
函数功能：增量式PI控制器
入口参数：编码器测量值(实际速度)，目标速度
返回  值：电机PWM
根据增量式离散PID公式 
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI_A (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
	 return Pwm;    
}
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
{
	//Forward and reverse control of motor
	//电机正反转控制
  if(motor_a<0)		AIN2=0,		AIN1=1;
	else				    AIN2=1,		AIN1=0;
	//Motor speed control 
	//电机转速控制
	PWMA=abs(motor_a);
 
	//Forward and reverse control of motor
	//电机正反转控制	
	if(motor_b<0)		BIN2=1,		BIN1=0;
	else 	          BIN2=0,		BIN1=1;
	//Motor speed control 
	//电机转速控制
	PWMB=abs(motor_b);
 
	//Forward and reverse control of motor
	//电机正反转控制	
	if(motor_c>0)		CIN2=0,		CIN1=1;
	else 	          CIN2=1,		CIN1=0;
	//Motor speed control 
	//电机转速控制
  PWMC=abs(motor_c);
 
	//Forward and reverse control of motor
	//电机正反转控制
	if(motor_d>0)		DIN2=0,		DIN1=1;
	else 	          DIN2=1,		DIN1=0;
	//Motor speed control 
	//电机转速控制
	PWMD=abs(motor_d);
 
	//Servo control
	//舵机控制
	Servo_PWM =servo;
}