STM32三輪全向底盤

最近在實驗室培訓，玩過麥克納姆四輪底盤後，玩三輪全向底盤也有一段時間了，於是來分享一下自己的心得。

附圖
這是去年參加robocon的底盤，三個大疆3508電機，一個全場定位模組，一個實驗室焊的遙控器[^1]: （怕出問題所以沒用DEVO10）。

1.底盤公式

三輪全向底盤論壇上已經有很多的前輩寫了很多好的文章，自己就不再重複論述了。

附上一篇前輩的博文：https://blog.csdn.net/qq_40696002/article/details/107856908

同樣附上視訊鏈接：https://v.youku.com/v_show/id_XMjY2NTIxMjUyNA==.html?qq-pf-to=pcqq.c2c

鏈接的視訊我個人覺得是非常好的，我當初看很多部落格都看不懂，是看了這個視訊才懂的。

關於三輪底盤的功能，特殊性在上視訊中都有詳細的闡述，唯一注意的是電機的序號。

2.程式實現手動模式

1.總體思路：遙控撥桿發送底盤整體的速度——通過底盤公式分解出個個電機的速度——各電機的速度通過PID計算髮送各電機。
2.遙控器部分：通過CAN發送撥桿的通道值各底盤公式。

#define  w1                 (3.14159f/3.0f)               /*60度弧度*/
#define  w2                 (IMU.REAL.A*3.1415926f/180.0f)/*全場定位輸出弧度*/ 
		
for (motor_num=1;motor_num<4;motor_num++) //清除程式之前的PID參數
		{
				PID_SPEED[motor_num].I_OUT = 0;
				PID_SPEED[motor_num].OUT = 0;
		}
		speed[1]   = (-my_cos(w1 + w2)*RC.CH3*1.5f + my_sin(w1 + w2)*RC.CH4*2.5f - RC.CH1 )*2.0f; //*1***********2*/
		speed[2]   = (-my_cos(w1 - w2)*RC.CH3*1.5f - my_sin(w1 - w2)*RC.CH4*2.5f - RC.CH1 )*2.0f;
		speed[3]   = (+my_cos(w2     )*RC.CH3*1.5f - my_sin(w2     )*RC.CH4*2.5f - RC.CH1 )*2.0f;       //3

		
	}

如沒有感測器計算角度，則 w2 = 0。

3.PID計算電機速度

  p[1] = 3.4;		// left
  p[2] = 3.2;	//right
  p[3] = 10;	//behind
	
	PID_Cal( &PID_SPEED[1], MOTOR_FEEDBACK[1].speed_rpm, speed[1], p[1], 5000);
	PID_Cal( &PID_SPEED[2], MOTOR_FEEDBACK[2].speed_rpm, speed[2], p[2], 5000);
	PID_Cal( &PID_SPEED[3], MOTOR_FEEDBACK[3].speed_rpm, speed[3], p[3], 9000); //3號速度>1號2號
	
	Motor_Set_Current(PID_SPEED[1].OUT,PID_SPEED[2].OUT,PID_SPEED[3].OUT,0); 			

p[ ] 是PID的比例係數，我自己微調了一下P目的是讓電機跑的更直。
關於大疆電機的使用方法可以看我之前的部落格。

3.程式實現自動模式

1.總體思路：位置式PID通過距離先計算出底盤整體的移動速度——通過底盤公式分解出個個電機的速度——各電機的速度通過PID計算髮送各電機。

2. 位置式PID計算底盤的移動速度
   關於位置式PID的程式在論壇上同樣有很多，我自己弄的也不是明明白白，就不獻醜了。

void MOVE_SET(float goal_X,float goal_Y,float goal_A,unsigned short int mode) //(x,y,angle.mode) 
{
	i = mode;
	
	MOVE.GOAL_A = goal_A;
	MOVE.GOAL_Y = goal_Y;
	MOVE.GOAL_X = goal_X;	
}


MOVE_SET(0,8000,0,0); //通過自己寫的MOVE——SET程式發送座標給全場定位。


if(MOVE.Mode==0)//Y是主軸,X是輔助軸:X,Y都是位置式PID; 在通過位置PID計算出底盤在X.Y軸的速度
{
	MOVE.VX = PID_Cal(&VXPID,X_LONG,MOVE.GOAL_X,3,3000);//4,2000
	MOVE.VY = PID_Cal(&VYPID,Y_LONG,MOVE.GOAL_Y,3,5000);//（8.2000）
	MOVE.VZ = PID_Cal(&VAPID,IMU.REAL.A,MOVE.GOAL_A,65,1500);//110,2500
}

3.底盤公式計算電機速度

	speed[1]   = ( my_cos((60+IMU.REAL.A)*Angle_To_Rad)*MOVE.VX + my_sin((60+IMU.REAL.A)*Angle_To_Rad)*MOVE.VY - MOVE.VZ );
	speed[2]   = ( my_cos((60-IMU.REAL.A)*Angle_To_Rad)*MOVE.VX - my_sin((60-IMU.REAL.A)*Angle_To_Rad)*MOVE.VY - MOVE.VZ );
	speed[3]   = (-my_cos(    IMU.REAL.A *Angle_To_Rad)*MOVE.VX - my_sin(    IMU.REAL.A *Angle_To_Rad)*MOVE.VY - MOVE.VZ );

4.PID速度環計算各電機速度

	V_Limit();		/* 限  速 */
    p[1] = 3.4;		// left
	p[2] = 3.2;	//right
    p[3] = 10;	//behind
	
	PID_Cal( &PID_SPEED[1], MOTOR_FEEDBACK[1].speed_rpm, speed[1], p[1], 5000);
	PID_Cal( &PID_SPEED[2], MOTOR_FEEDBACK[2].speed_rpm, speed[2], p[2], 5000);
	PID_Cal( &PID_SPEED[3], MOTOR_FEEDBACK[3].speed_rpm, speed[3], p[3], 9000); //3號速度>1號2號


	Motor_Set_Current(PID_SPEED[1].OUT,PID_SPEED[2].OUT,PID_SPEED[3].OUT,0); 

4.底盤自動跑點

1.思路：通過位置式PID定座標，再通過全場定位模組反饋底盤實際座標來實現自動跑點。

if(step == 1)
	{
		
			MOVE_SET(0,8000,0,0);
		
	//		MOVE_SET(Profile_SET(R0.X0,0),R1.Y0,0,2);		//跑曲線		
		
			if(Y_LONG >= 7975)
		{
			step = 2;
		}
	
	}
	
	else if( step == 2)
	{		
		
		MOVE_SET(-4000,8000,0,10);
		
		if( X_LONG <= -3950)
		{
			step = 3;
		}
	}

上文若有所錯誤，敬請指出，共同學習，共同進步。
ps：感謝實驗室的學長