327 lines
15 KiB
C
327 lines
15 KiB
C
#include "stm32f4xx.h" // Device header
|
||
#include "stm32f4xx_conf.h"
|
||
#include <math.h>
|
||
#include "M3508.h"
|
||
#include "GM6020.h"
|
||
#include "Remote.h"
|
||
#include "PID.h"
|
||
#include "Parameter.h"
|
||
#include "RefereeSystem.h"
|
||
#include "Ultra_CAP.h"
|
||
#include "Mecanum.h"
|
||
|
||
#define Mecanum_PowerControlSpeedNormalizationValue 3.0f//功率控制归一化速度标准值,范围[0,3]
|
||
#define Mecanum_PowerControlGainCoefficientInitialValue 0.5f//功率控制增益系数初始值
|
||
#define Mecanum_PowerControl_T 5.0f//功率控制周期(T=Mecanum_PowerControl_T*2ms)
|
||
#define Mecanum_PowerControl_UseBuffer 30.0f//功率控制消耗的缓冲能量
|
||
#define Mecanum_PowerControl_PowerMax 4.0f//功率控制功率上限与裁判系统功率上限比值上限
|
||
#define Mecanum_PowerControl_UltraCAPPower 100.0f//使用超电增加功率
|
||
|
||
PID_PositionInitTypedef Mecanum_SpeedPID[4];//底盘四个电机的转速PID
|
||
PID_PositionInitTypedef Mecanum_TrackPID;//底盘跟随PID
|
||
float Mecanum_YawTheta;//底盘云台相对偏航角度
|
||
uint8_t Mecanum_GyroScopeFlag;//底盘小陀螺标志位
|
||
float Mecanum_Power;//底盘功率(软件计算值)
|
||
float Mecanum_Current,Mecanum_PowerLimit,Mecanum_CurrentLimit=16384*4.0f;//底盘电流总和,功率控制功率上限,底盘电流限幅
|
||
|
||
/*
|
||
*函数简介:麦轮初始化
|
||
*参数说明:无
|
||
*返回类型:无
|
||
*备注:即四个转速PID的初始化
|
||
*备注:四个电机的ID参照上方的宏定义,最好使用M3508电机的ID1~4,否则下方电机控制的代码需要修改
|
||
*/
|
||
void Mecanum_Init(void)
|
||
{
|
||
PID_PositionStructureInit(&Mecanum_SpeedPID[0],0);//左前轮
|
||
PID_PositionSetParameter(&Mecanum_SpeedPID[0],0.8,0,0);
|
||
PID_PositionSetEkRange(&Mecanum_SpeedPID[0],-5,5);
|
||
PID_PositionSetOUTRange(&Mecanum_SpeedPID[0],-15000,15000);
|
||
|
||
PID_PositionStructureInit(&Mecanum_SpeedPID[1],0);//右前轮
|
||
PID_PositionSetParameter(&Mecanum_SpeedPID[1],0.8,0,0);
|
||
PID_PositionSetEkRange(&Mecanum_SpeedPID[1],-5,5);
|
||
PID_PositionSetOUTRange(&Mecanum_SpeedPID[1],-15000,15000);
|
||
|
||
PID_PositionStructureInit(&Mecanum_SpeedPID[2],0);//左后轮
|
||
PID_PositionSetParameter(&Mecanum_SpeedPID[2],0.8,0,0);
|
||
PID_PositionSetEkRange(&Mecanum_SpeedPID[2],-5,5);
|
||
PID_PositionSetOUTRange(&Mecanum_SpeedPID[2],-15000,15000);
|
||
|
||
PID_PositionStructureInit(&Mecanum_SpeedPID[3],0);//右后轮
|
||
PID_PositionSetParameter(&Mecanum_SpeedPID[3],0.8,0,0);
|
||
PID_PositionSetEkRange(&Mecanum_SpeedPID[3],-5,5);
|
||
PID_PositionSetOUTRange(&Mecanum_SpeedPID[3],-15000,15000);
|
||
|
||
PID_PositionStructureInit(&Mecanum_TrackPID,Yaw_GM6020PositionValue);//底盘跟随
|
||
PID_PositionSetParameter(&Mecanum_TrackPID,0.007,0,0.8);
|
||
PID_PositionSetEkRange(&Mecanum_TrackPID,-1,1);
|
||
PID_PositionSetOUTRange(&Mecanum_TrackPID,-4,4);
|
||
}
|
||
|
||
/*
|
||
*函数简介:麦轮PID清理
|
||
*参数说明:无
|
||
*返回类型:无
|
||
*备注:清理四个转速位置式PID
|
||
*/
|
||
void Mecanum_CleanPID(void)
|
||
{
|
||
PID_PositionClean(&Mecanum_SpeedPID[0]);//左前轮
|
||
PID_PositionClean(&Mecanum_SpeedPID[1]);//右前轮
|
||
PID_PositionClean(&Mecanum_SpeedPID[2]);//左后轮
|
||
PID_PositionClean(&Mecanum_SpeedPID[3]);//右后轮
|
||
PID_PositionClean(&Mecanum_TrackPID);//底盘跟随
|
||
}
|
||
|
||
/*
|
||
*函数简介:麦轮速度控制
|
||
*参数说明:左前轮速度
|
||
*参数说明:右前轮速度
|
||
*参数说明:左后轮速度
|
||
*参数说明:右后轮速度
|
||
*返回类型:无
|
||
*备注:单独控制四个轮子的速度
|
||
*/
|
||
void Mecanum_ControlSpeed(int16_t LeftFrontSpeed,int16_t RightFrontSpeed,int16_t LeftRearSpeed,int16_t RightRearSpeed)
|
||
{
|
||
//更改期望
|
||
Mecanum_SpeedPID[0].Need_Value=LeftFrontSpeed;//左前轮
|
||
Mecanum_SpeedPID[1].Need_Value=RightFrontSpeed;//右前轮;
|
||
Mecanum_SpeedPID[2].Need_Value=LeftRearSpeed;//左后轮
|
||
Mecanum_SpeedPID[3].Need_Value=RightRearSpeed;//右后轮
|
||
|
||
//PID计算
|
||
PID_PositionCalc(&Mecanum_SpeedPID[0],M3508_MotorStatus[Mecanum_LeftFrontWheel-0x201].RotorSpeed);//左前轮
|
||
PID_PositionCalc(&Mecanum_SpeedPID[1],M3508_MotorStatus[Mecanum_RightFrontWheel-0x201].RotorSpeed);//右前轮
|
||
PID_PositionCalc(&Mecanum_SpeedPID[2],M3508_MotorStatus[Mecanum_LeftRearWheel-0x201].RotorSpeed);//左后轮
|
||
PID_PositionCalc(&Mecanum_SpeedPID[3],M3508_MotorStatus[Mecanum_RightRearWheel-0x201].RotorSpeed);//右后轮
|
||
|
||
Mecanum_Current=0;
|
||
for(uint8_t i=0;i<4;i++)
|
||
Mecanum_Current+=fabs(Mecanum_SpeedPID[i].OUT);
|
||
if(Mecanum_Current>Mecanum_CurrentLimit)
|
||
{
|
||
Mecanum_SpeedPID[0].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
Mecanum_SpeedPID[1].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
Mecanum_SpeedPID[2].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
Mecanum_SpeedPID[3].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
|
||
}
|
||
|
||
M3508_CANSetLIDCurrent(Mecanum_SpeedPID[1].OUT,Mecanum_SpeedPID[0].OUT,Mecanum_SpeedPID[2].OUT,Mecanum_SpeedPID[3].OUT);//M3508控制
|
||
}
|
||
|
||
/*
|
||
*函数简介:麦轮逆运动解算
|
||
*参数说明:x轴速度,单位m/s(以前为正)
|
||
*参数说明:y轴速度,单位m/s(以左为正)
|
||
*参数说明:z轴转速,单位rad/s(以逆时针为正)
|
||
*返回类型:无
|
||
*备注:速度转速转换系数:
|
||
* 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/100)=19×60×100×v/(2Π×R)=18143.663512×v/R,R单位cm
|
||
*备注:麦轮半径参数在上方宏定义Mecanum_WheelRadius修改,默认7cm,转换参数2591.95
|
||
*备注:底盘中心到轮子中心的距离由上方宏定义x轴分量Mecanum_rx和y轴分量Mecanum_ry决定
|
||
*/
|
||
void Mecanum_InverseMotionControl(float v_x,float v_y,float w)
|
||
{
|
||
//逆运动解算
|
||
int16_t LeftFrontSpeed=(int16_t)((-v_x-v_y-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//左前轮
|
||
int16_t RightFrontSpeed=(int16_t)((v_x-v_y-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//右前轮
|
||
int16_t LeftRearSpeed=(int16_t)((-v_x+v_y-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//左后轮
|
||
int16_t RightRearSpeed=(int16_t)((v_x+v_y-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//右后轮
|
||
|
||
Mecanum_ControlSpeed(LeftFrontSpeed,RightFrontSpeed,LeftRearSpeed,RightRearSpeed);//M3508速度控制
|
||
}
|
||
|
||
/*
|
||
*函数简介:麦轮功率控制
|
||
*参数说明:无
|
||
*返回类型:无
|
||
*备注:麦轮是否开启可功率控制模式决定于上方宏定义Mecanum_PowerControl
|
||
*备注:以Mecanum_PowerControl_T*2ms为控制周期,取底盘平均功率(通过M3508反馈数据计算),通过速度增益系数和速度归一化控制速度大小,速度增益系数在[0,2]范围内
|
||
*备注:速度归一化的标准值在上方宏定义Mecanum_PowerControlSpeedNormalizationValue修改,范围[0,3]
|
||
*备注:增益系数的初始值在上方宏定义Mecanum_PowerControlGainCoefficientInitialValue修改
|
||
*备注:设定功率上限最高为150W限幅
|
||
*备注:速度转速转换系数:
|
||
* 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/100)=19×60×100×v/(2Π×R)=18143.663512×v/R,R单位cm
|
||
*备注:麦轮半径参数在上方宏定义Mecanum_WheelRadius修改,默认7cm,转换参数2591.95
|
||
*备注:底盘中心到轮子中心的距离由上方宏定义x轴分量Mecanum_rx和y轴分量Mecanum_ry决定
|
||
*/
|
||
void Mecanum_PowerMoveControl(void)
|
||
{
|
||
static uint8_t Mecanum_GyroScopeCloseFlag=0;//小陀螺待关闭标志位
|
||
static float GainCoefficient=1.0f;//速度增益系数
|
||
static uint8_t Count=0;//计数器,以Mecanum_PowerControl_T*2ms为控制周期
|
||
|
||
/*==========三轴速度获取==========*/
|
||
float vx=1024-Remote_RxData.Remote_R_UD;
|
||
float vy=1024-Remote_RxData.Remote_R_RL;
|
||
float w=1024-Remote_RxData.Remote_L_RL;//获取三个轴的速度参量
|
||
float sigma=sqrtf(vx*vx+vy*vy);//获取xy轴速度归一化系数
|
||
if(sigma!=0)//x,y轴速度归一化(正交合成速度不变为标准值)
|
||
{
|
||
vx=vx/sigma*Mecanum_PowerControlSpeedNormalizationValue;
|
||
vy=vy/sigma*Mecanum_PowerControlSpeedNormalizationValue;
|
||
}
|
||
|
||
int16_t Raw_Theta=Yaw_GM6020PositionValue-GM6020_MotorStatus[0].Angle;//获取底盘云台相对角度原始数据
|
||
if(Raw_Theta<0)Raw_Theta+=8192;
|
||
//Mecanum_YawTheta=Raw_Theta/8192.0f*2.0f*3.141592653589793238462643383279f;
|
||
Mecanum_YawTheta=Raw_Theta*0.000766990393942820614859043794746f;//获取底盘云台相对角度
|
||
|
||
float vx_=vx,vy_=vy;
|
||
vx=vx_*cosf(Mecanum_YawTheta)-vy_*sinf(Mecanum_YawTheta);
|
||
vy=vx_*sinf(Mecanum_YawTheta)+vy_*cosf(Mecanum_YawTheta);//根据底盘云台相对角度修正xy轴速度
|
||
|
||
/*==========小陀螺处理==========*/
|
||
if(Yaw_GM6020PositionValue<4096)//正向小陀螺为逆时针
|
||
{
|
||
if((Remote_RxData.Remote_RS==2 || Remote_RxData.Remote_KeyPush_Ctrl==1) || (Mecanum_GyroScopeFlag==1 && Mecanum_GyroScopeCloseFlag==1 && GM6020_MotorStatus[0].Position<Yaw_GM6020PositionValue+500))//小陀螺模式
|
||
{
|
||
w=Mecanum_GyroScopeAngularVelocity;
|
||
Mecanum_GyroScopeFlag=1;//处于小陀螺状态
|
||
Mecanum_GyroScopeCloseFlag=1;//小陀螺处于待关闭状态
|
||
}
|
||
else if(Mecanum_GyroScopeFlag==1 && ((GM6020_MotorStatus[0].Angle>Yaw_GM6020PositionValue+100 || GM6020_MotorStatus[0].Angle<Yaw_GM6020PositionValue-100) || (GM6020_MotorStatus[0].Speed>2||GM6020_MotorStatus[0].Speed<-2)))//小陀螺关闭状态
|
||
{
|
||
PID_PositionSetOUTRange(&Mecanum_TrackPID,-2,2);
|
||
PID_PositionCalc(&Mecanum_TrackPID,GM6020_MotorStatus[0].Position);//底盘跟云台
|
||
w=-Mecanum_TrackPID.OUT;
|
||
Mecanum_GyroScopeCloseFlag=0;//小陀螺未处于待关闭状态
|
||
}
|
||
else if(Remote_RxData.Remote_RS==1)//反向小陀螺模式(仅检录使用)
|
||
w=-Mecanum_GyroScopeAngularVelocity;
|
||
else
|
||
{
|
||
PID_PositionSetOUTRange(&Mecanum_TrackPID,-5,5);
|
||
Mecanum_GyroScopeFlag=0;
|
||
PID_PositionCalc(&Mecanum_TrackPID,GM6020_MotorStatus[0].Position);//底盘跟云台
|
||
w=-Mecanum_TrackPID.OUT;
|
||
}
|
||
}
|
||
else//正向小陀螺为顺时针
|
||
{
|
||
if((Remote_RxData.Remote_RS==2 || Remote_RxData.Remote_KeyPush_Ctrl==1) || (Mecanum_GyroScopeFlag==1 && Mecanum_GyroScopeCloseFlag==1 && GM6020_MotorStatus[0].Position>Yaw_GM6020PositionValue-500))//小陀螺模式
|
||
{
|
||
w=-Mecanum_GyroScopeAngularVelocity;
|
||
Mecanum_GyroScopeFlag=1;//处于小陀螺状态
|
||
Mecanum_GyroScopeCloseFlag=1;//小陀螺处于待关闭状态
|
||
}
|
||
else if(Mecanum_GyroScopeFlag==1 && ((GM6020_MotorStatus[0].Angle>Yaw_GM6020PositionValue+5 || GM6020_MotorStatus[0].Angle<Yaw_GM6020PositionValue-5) || (GM6020_MotorStatus[0].Speed>1||GM6020_MotorStatus[0].Speed<-1)))//小陀螺关闭状态
|
||
{
|
||
PID_PositionSetOUTRange(&Mecanum_TrackPID,-2,2);
|
||
PID_PositionCalc(&Mecanum_TrackPID,GM6020_MotorStatus[0].Position);//底盘跟云台
|
||
w=-Mecanum_TrackPID.OUT;
|
||
Mecanum_GyroScopeCloseFlag=0;//小陀螺未处于待关闭状态
|
||
}
|
||
else if(Remote_RxData.Remote_RS==1)//反向小陀螺模式(仅检录使用)
|
||
w=Mecanum_GyroScopeAngularVelocity;
|
||
else
|
||
{
|
||
PID_PositionSetOUTRange(&Mecanum_TrackPID,-5,5);
|
||
Mecanum_GyroScopeFlag=0;
|
||
PID_PositionCalc(&Mecanum_TrackPID,GM6020_MotorStatus[0].Position);//底盘跟云台
|
||
w=-Mecanum_TrackPID.OUT;
|
||
}
|
||
}
|
||
|
||
/*==========功率上限处理==========*/
|
||
/*由缓冲能量得到功率控制功率上限*/
|
||
float Mecanum_PowerRef=1.0f/(60.0f-Mecanum_PowerControl_UseBuffer)*RefereeSystem_Buffer;//功率增益
|
||
if(Mecanum_PowerRef>Mecanum_PowerControl_PowerMax)Mecanum_PowerRef=Mecanum_PowerControl_PowerMax;
|
||
Mecanum_PowerLimit=Mecanum_PowerRef*RefereeSystem_Ref;
|
||
|
||
/*由运动状态约束功率控制功率上限*/
|
||
float Scale=sigma/660.0f;
|
||
if(Scale<1 && Scale>0)Mecanum_PowerLimit*=Scale;//平移约束
|
||
if(Mecanum_GyroScopeFlag==1)Mecanum_PowerLimit=Mecanum_PowerRef*RefereeSystem_Ref;//小陀螺约束
|
||
if(Mecanum_PowerLimit>150.0f)Mecanum_PowerLimit=150.0f;//限幅约束
|
||
|
||
/*由超电约束功率控制功率上限*/
|
||
if(Remote_RxData.Remote_KeyPush_Shift==1)//开启超电
|
||
{
|
||
if(Mecanum_PowerLimit>0.0f)
|
||
{
|
||
if(Mecanum_PowerLimit<RefereeSystem_Ref)Mecanum_PowerLimit+=Mecanum_PowerControl_UltraCAPPower;
|
||
else Mecanum_PowerLimit=RefereeSystem_Ref+Mecanum_PowerControl_UltraCAPPower;
|
||
}
|
||
Ultra_CAP_SetPower(RefereeSystem_Ref);
|
||
}
|
||
else
|
||
Ultra_CAP_SetPower(Mecanum_PowerLimit);
|
||
|
||
/*==========功率控制==========*/
|
||
static float PowerSum=0;//控制周期内的功率积分
|
||
Mecanum_Power=1.732050807568877f*(M3508_MotorStatus[Mecanum_LeftFrontWheel-0x201].Power \
|
||
+M3508_MotorStatus[Mecanum_RightFrontWheel-0x201].Power \
|
||
+M3508_MotorStatus[Mecanum_LeftRearWheel-0x201].Power \
|
||
+M3508_MotorStatus[Mecanum_RightRearWheel-0x201].Power);//获取底盘功率
|
||
PowerSum+=Mecanum_Power;
|
||
|
||
/*速度增益*/
|
||
if(Count==Mecanum_PowerControl_T)//一个控制周期
|
||
{
|
||
Count=0;//计数器清零
|
||
float Power_avg=PowerSum/Mecanum_PowerControl_T;
|
||
PowerSum=0;
|
||
|
||
if(Power_avg<Mecanum_PowerLimit/2.0f)
|
||
{
|
||
GainCoefficient+=0.05f;//增益系数递减
|
||
if(GainCoefficient>2)GainCoefficient=2;//增益系数限上幅2
|
||
}
|
||
else if(Power_avg<Mecanum_PowerLimit-5)
|
||
{
|
||
GainCoefficient+=0.02f;//增益系数递减
|
||
if(GainCoefficient>2)GainCoefficient=2;//增益系数限上幅2
|
||
}
|
||
else if(Power_avg>Mecanum_PowerLimit+5)
|
||
{
|
||
if(RefereeSystem_Buffer<60.0f-Mecanum_PowerControl_UseBuffer)GainCoefficient-=0.05f;//增益系数递增
|
||
else GainCoefficient-=0.02f;//增益系数递增
|
||
if(GainCoefficient<Mecanum_PowerControlGainCoefficientInitialValue/2.0f)GainCoefficient=Mecanum_PowerControlGainCoefficientInitialValue/2.0f;//增益系数限下幅
|
||
}
|
||
}
|
||
else if(vx!=0 || vy!=0 || Mecanum_GyroScopeFlag==1)//正常运动
|
||
Count++;
|
||
else//停止运动
|
||
GainCoefficient=Mecanum_PowerControlGainCoefficientInitialValue;//增益系数回归初始值
|
||
if(Mecanum_GyroScopeFlag==1 && (vx!=0 || vy!=0)){w*=0.6f;vx*=0.5f;vy*=0.5f;}//小陀螺移动直接降速,保证移动瞬间为缓冲能量增加
|
||
|
||
/*电流限幅*/
|
||
float Mecanum_BufferCurrent=Mecanum_PowerLimit*512.0f/15.0f;
|
||
float Mecanum_PowerCurrent=Mecanum_PowerLimit*512.0f/15.0f*5.0f;
|
||
float B,C=0.9f;
|
||
if(60.0f-Mecanum_PowerControl_UseBuffer-10.0f>10.0f)B=60.0f-Mecanum_PowerControl_UseBuffer-10.0f;
|
||
else B=10.0f;
|
||
if(RefereeSystem_Buffer>B)
|
||
{
|
||
float PowerScale=0;
|
||
if(Mecanum_Power>C*Mecanum_PowerLimit)
|
||
{
|
||
if(Mecanum_Power>Mecanum_PowerLimit)PowerScale=0;
|
||
else PowerScale=(Mecanum_PowerLimit-Mecanum_Power)/(Mecanum_PowerLimit-C*Mecanum_PowerLimit);
|
||
}
|
||
else PowerScale=1;
|
||
Mecanum_CurrentLimit=Mecanum_BufferCurrent+Mecanum_PowerCurrent*PowerScale;
|
||
}
|
||
else
|
||
{
|
||
if(RefereeSystem_Buffer<5.0f)Mecanum_CurrentLimit=Mecanum_BufferCurrent*0.5f;
|
||
else Mecanum_CurrentLimit=Mecanum_BufferCurrent*(RefereeSystem_Buffer+B-10.0f)/(2*B-10.0f);
|
||
}
|
||
if(Mecanum_CurrentLimit>16384*4)Mecanum_CurrentLimit=16384*4;
|
||
|
||
/*==========运动控制==========*/
|
||
vx*=GainCoefficient;vy*=GainCoefficient;
|
||
if(Mecanum_GyroScopeFlag==1)w*=GainCoefficient;
|
||
int16_t LeftFrontSpeed=(int16_t)((-vx-vy-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//左前
|
||
int16_t RightFrontSpeed=(int16_t)((vx-vy-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//右前
|
||
int16_t LeftRearSpeed=(int16_t)((-vx+vy-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//左后
|
||
int16_t RightRearSpeed=(int16_t)((vx+vy-w*(Mecanum_rx+Mecanum_ry)/100.0f)/Mecanum_WheelRadius*18143.663512f);//右后
|
||
|
||
Mecanum_ControlSpeed(LeftFrontSpeed,RightFrontSpeed,LeftRearSpeed,RightRearSpeed);
|
||
}
|