177 lines
6.5 KiB
C
177 lines
6.5 KiB
C
#include "Classis_Wheels.h"
|
||
#include "can.h"
|
||
#include "Parameter.h"
|
||
#include <math.h>
|
||
|
||
PID_PositionInitTypedef Wheels_SpeedPID[4];
|
||
|
||
extern M3508_Motor Can1_M3508_MotorStatus[8];
|
||
extern M3508_Motor Can2_M3508_MotorStatus[8];
|
||
extern M6020_Motor Can1_M6020_MotorStatus[7];//GM6020电机状态数组
|
||
extern M6020_Motor Can2_M6020_MotorStatus[7];//GM6020电机状态数组
|
||
extern Remote_Data Remote_RxData;//遥控器接收数据
|
||
extern CAN_HandleTypeDef hcan1;
|
||
|
||
//-------------------测试区
|
||
#define BigYaw_MediumPosition -114//大yaw中位位置(直接读大yaw电机角度)
|
||
#define Mecanum_rx 0.24f //底盘中心到轮子中心的距离的x轴分量(m)
|
||
#define Mecanum_ry 0.24f //底盘中心到轮子中心的距离的y轴分量(m)
|
||
#define Mecanum_WheelRadius 0.07f //轮子半径(m)
|
||
#define MAX_RPM 2000 // 最大转速，根据电机调整
|
||
#define L 0.2f // 轮距（单位：米）
|
||
float K_yaw = 10.0f; // 旋转速度的比例系数
|
||
|
||
//#define Wheel_RightFront 0x201//右前轮
|
||
//#define Wheel_LeftFront 0x202//左前轮
|
||
//#define Wheel_LeftRear 0x203//左后轮
|
||
//#define Wheel_RightRear 0x204//右后轮
|
||
|
||
void Classis_Wheels_Init(void)
|
||
{
|
||
PID_PositionStructureInit (&Wheels_SpeedPID[0],0);
|
||
PID_PositionSetParameter (&Wheels_SpeedPID[0],10,0,0);
|
||
PID_PositionSetOUTRange (&Wheels_SpeedPID[0],-20000,20000);
|
||
PID_PositionSetEkRange (&Wheels_SpeedPID[0], -3.0f, 3.0f);
|
||
|
||
PID_PositionStructureInit (&Wheels_SpeedPID[1],0);
|
||
PID_PositionSetParameter (&Wheels_SpeedPID[1],10,0,0);
|
||
PID_PositionSetOUTRange (&Wheels_SpeedPID[1],-20000,20000);
|
||
PID_PositionSetEkRange (&Wheels_SpeedPID[1], -3.0f, 3.0f);
|
||
|
||
PID_PositionStructureInit (&Wheels_SpeedPID[2],0);
|
||
PID_PositionSetParameter (&Wheels_SpeedPID[2],10,0,0);
|
||
PID_PositionSetOUTRange (&Wheels_SpeedPID[2],-20000,20000);
|
||
PID_PositionSetEkRange (&Wheels_SpeedPID[2],-3.0f, 3.0f);
|
||
|
||
PID_PositionStructureInit (&Wheels_SpeedPID[3],0);
|
||
PID_PositionSetParameter (&Wheels_SpeedPID[3],10,0,0);
|
||
PID_PositionSetOUTRange (&Wheels_SpeedPID[3],-20000,20000);
|
||
PID_PositionSetEkRange (&Wheels_SpeedPID[3], -3.0f, 3.0f);
|
||
|
||
// PID_PositionStructureInit(&Mecanum_TrackPID,4096);
|
||
// PID_PositionSetParameter(&Mecanum_TrackPID,0,0,0);
|
||
// PID_PositionSetEkRange(&Mecanum_TrackPID,-5,5);
|
||
// PID_PositionSetOUTRange(&Mecanum_TrackPID,-1,1);
|
||
|
||
}
|
||
//-------------------测试区
|
||
|
||
// void Classis_Wheels_Control(void)
|
||
// {
|
||
// extern const RC_ctrl_t *local_rc_ctrl;
|
||
|
||
// // 1. 获取左摇杆（ch[2]=左右, ch[3]=上下）
|
||
// float vy_chassis = -(float)Remote_RxData.Remote_R_RL; // 左右 → 横向
|
||
// float vx = -(float)Remote_RxData.Remote_R_UD; // 上下 → 纵向
|
||
|
||
// // 3. 麦轮解算 —— 用一个缩放系数 K 代替所有几何参数
|
||
// const float K = 4.0f;
|
||
|
||
// int16_t fl = (int16_t)((vx - vy_chassis) * K); // 左前
|
||
// int16_t fr = (int16_t)((vx + vy_chassis) * K); // 右前
|
||
// int16_t bl = (int16_t)((vx + vy_chassis) * K); // 左后
|
||
// int16_t br = (int16_t)((vx - vy_chassis) * K); // 右后
|
||
|
||
// // 4. 发送目标转速（单位：RPM）
|
||
// Classis_Wheels_Setspeed(fl, fr, bl, br);
|
||
|
||
// }
|
||
/*-------------------测试区
|
||
↑y
|
||
|
|
||
------------------>x ↑w
|
||
|
|
||
|
|
||
|
||
-------------------测试区*/
|
||
void Classis_Wheels_Control(void)
|
||
{
|
||
// 1. 通过遥控器获取车体运动的期望速度:x轴前后平动速度,y轴左右平动速度,转动角速度(前右为正,逆时针为正)
|
||
float vy_chassis = (float)Remote_RxData.Remote_R_UD / 660.0f;//范围-1~1，归一化为-1~1的速度比例
|
||
float vx_chassis = (float)Remote_RxData.Remote_R_RL / 660.0f;
|
||
float sigma = sqrtf(vx_chassis*vx_chassis + vy_chassis*vy_chassis);//归一化系数
|
||
float w_chassis = 0.0f; // 旋转速度暂时设为3
|
||
float theta = BigYaw_MediumPosition - Can2_M6020_MotorStatus[0].ANgle; //云台底盘相对角度
|
||
if(theta > 180.0f) theta -= 360.0f; // 将角度调整到[-180, 180]范围内
|
||
if(theta < -180.0f) theta += 360.0f;
|
||
|
||
vy_chassis = vy_chassis / sigma; // 归一化到最大转速
|
||
vx_chassis = vx_chassis / sigma; // 归一化到最大转速
|
||
|
||
float vx_=vx_chassis,vy_=vy_chassis;
|
||
vy_chassis = vy_*cosf(theta)-vx_*sinf(theta);//根据底盘云台相对角度修正xy轴速度
|
||
vx_chassis = vx_*cosf(theta)+vy_*sinf(theta);
|
||
|
||
//2.
|
||
|
||
vy_chassis *= 0.2;
|
||
vx_chassis *= 0.2;
|
||
int16_t RightFrontSpeed =(int16_t)((-vx_chassis+vy_chassis+w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//右前
|
||
int16_t LeftFrontSpeed =(int16_t)((+vx_chassis+vy_chassis-w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//左前
|
||
int16_t LeftRearSpeed =(int16_t)((-vx_chassis+vy_chassis-w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//左后
|
||
int16_t RightRearSpeed =(int16_t)((+vx_chassis+vy_chassis+w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//右后
|
||
//w' = v/R (rad/s) = v/(2Π×R) (r/s)(圈每秒) = 60×v/(2Π×R) (r/min)=1/19 w
|
||
//⇒ w=19 × 60 × v/(2Π×R)=19×60×v/(2Π×R)=181.43663512×v/R (圈每分),R单位m
|
||
Classis_Wheels_Setspeed(RightFrontSpeed,LeftFrontSpeed,LeftRearSpeed,RightRearSpeed);
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
void Classis_Wheels_Setspeed(int16_t LeftFrontSpeed,int16_t RightFrontSpeed,int16_t LeftRearSpeed,int16_t RightRearSpeed)
|
||
{
|
||
|
||
//更改期望
|
||
Wheels_SpeedPID[0].Need_Value =LeftFrontSpeed;//左前轮
|
||
Wheels_SpeedPID[1].Need_Value =RightFrontSpeed;//右前轮;
|
||
Wheels_SpeedPID[2].Need_Value =LeftRearSpeed;//左后轮
|
||
Wheels_SpeedPID[3].Need_Value =RightRearSpeed;//右后轮
|
||
|
||
|
||
//PID计算
|
||
PID_PositionCalc(&Wheels_SpeedPID[0],Can1_M3508_MotorStatus[0].RotorSpeed);//右前轮
|
||
PID_PositionCalc(&Wheels_SpeedPID[1],Can1_M3508_MotorStatus[1].RotorSpeed);//左前轮
|
||
PID_PositionCalc(&Wheels_SpeedPID[2],Can1_M3508_MotorStatus[2].RotorSpeed);//左后轮
|
||
PID_PositionCalc(&Wheels_SpeedPID[3],Can1_M3508_MotorStatus[3].RotorSpeed);//右后轮
|
||
|
||
// Mecanum_Current=0;
|
||
|
||
// for(uint8_t i=0;i<4;i++)
|
||
// Mecanum_Current+=fabs(Wheels_SpeedPID[i].OUT);
|
||
// if(Mecanum_Current>Mecanum_CurrentLimit)
|
||
// {
|
||
// Wheels_SpeedPID[0].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
// Wheels_SpeedPID[1].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
// Wheels_SpeedPID[2].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
// Wheels_SpeedPID[3].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
// }
|
||
//
|
||
Motor_3508_Current1((int16_t)Wheels_SpeedPID[0].OUT , (int16_t)Wheels_SpeedPID[1].OUT,
|
||
(int16_t)Wheels_SpeedPID[2].OUT , (int16_t)Wheels_SpeedPID[3].OUT,
|
||
&hcan1);//M3508控制
|
||
}
|
||
|
||
|
||
|