小车电机和舵机控制

最新推荐文章于 2023-05-22 23:01:45 发布

慢慢的回味

最新推荐文章于 2023-05-22 23:01:45 发布

阅读量1.8k

点赞数

分类专栏： ROS小车

本文链接：https://blog.csdn.net/bbxfnet/article/details/118255942

版权

本文链接地址: 小车电机和舵机控制

通过运动学方程，从Move_X, Move_Y, Move_Z中求解出阿克曼小车电机MOTOR_A，MOTOR_B的目标速度值和舵机Servo的PWM值。

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)