ELCT/25-sentry-chassis
6
0
Fork 0
Code Issues Pull Requests Activity

首次提交

Browse Source
This commit is contained in:
Huang Haoyu
2026-05-17 21:07:34 +08:00
commit bc5bd0c72e
1493 changed files with 1059050 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

319
A-DEvice/Motor.c Normal file
View File

@@ -0,0 +1,319 @@
#include "Motor.h"
M3508_Motor Can1_M3508_MotorStatus[8];//M3508电机状态数组
M3508_Motor Can2_M3508_MotorStatus[8];//M3508电机状态数组
M6020_Motor Can1_M6020_MotorStatus[7];//GM6020电机状态数组
M6020_Motor Can2_M6020_MotorStatus[7];//GM6020电机状态数组
M2006_Motor Can1_M2006_MotorStatus[8];//M2006电机状态数组
M2006_Motor Can2_M2006_MotorStatus[8];//M2006电机状态数组
#define MOTOR_ENCODER_ECD_MAX 8192.0f
#define MOTOR_ENCODER_HALF_ECD_MAX 4096.0f
void Motor_3508_Current1( int16_t device1, int16_t device2,
int16_t device3, int16_t device4,
CAN_HandleTypeDef *hcan)//CANID:0x200 电机ID:1-4 (-16384,16384)
{
CAN_TxHeaderTypeDef Tx_Message;
uint8_t Can_Send_Data[8];
uint32_t send_mail_box;
Tx_Message.StdId = 0x200;
Tx_Message.IDE = CAN_ID_STD;
Tx_Message.RTR = CAN_RTR_DATA;
Tx_Message.DLC = 0x08;
Can_Send_Data[0] = device1 >> 8;
Can_Send_Data[1] = device1;
Can_Send_Data[2] = device2 >> 8;
Can_Send_Data[3] = device2;
Can_Send_Data[4] = device3 >> 8;
Can_Send_Data[5] = device3;
Can_Send_Data[6] = device4 >> 8;
Can_Send_Data[7] = device4;
HAL_CAN_AddTxMessage(hcan, &Tx_Message, Can_Send_Data, &send_mail_box);
}
void Motor_3508_Current2( int16_t device1, int16_t device2,
int16_t device3, int16_t device4,
CAN_HandleTypeDef *hcan)//CANID:0x1FF 电机ID:5-8 (-16384,16384)
{
CAN_TxHeaderTypeDef Tx_Message;
uint8_t Can_Send_Data[8];
uint32_t send_mail_box;
Tx_Message.StdId = 0x1FF;
Tx_Message.IDE = CAN_ID_STD;
Tx_Message.RTR = CAN_RTR_DATA;
Tx_Message.DLC = 0x08;
Can_Send_Data[0] = device1 >> 8;
Can_Send_Data[1] = device1;
Can_Send_Data[2] = device2 >> 8;
Can_Send_Data[3] = device2;
Can_Send_Data[4] = device3 >> 8;
Can_Send_Data[5] = device3;
Can_Send_Data[6] = device4 >> 8;
Can_Send_Data[7] = device4;
HAL_CAN_AddTxMessage(hcan, &Tx_Message, Can_Send_Data, &send_mail_box);
}
void CAN1_M3508_DataProcess(M3508_ID ID,uint8_t *Data)
{
uint16_t M3508_RotorNowAngle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//本次转子机械角度原始数据
if(M3508_RotorNowAngle-Can1_M3508_MotorStatus[ID-0x201].RawRotorAngle>4000 && Can1_M3508_MotorStatus[ID-0x201].First_Flag==1) Can1_M3508_MotorStatus[ID-0x201].Rotor_r--;//本次转子机械角度原始数据和上次转子机械角度原始数据出现跃变
else if(Can1_M3508_MotorStatus[ID-0x201].RawRotorAngle-M3508_RotorNowAngle>4000 && Can1_M3508_MotorStatus[ID-0x201].First_Flag==1) Can1_M3508_MotorStatus[ID-0x201].Rotor_r++;
else if(Can1_M3508_MotorStatus[ID-0x201].First_Flag!=1) Can1_M3508_MotorStatus[ID-0x201].First_Flag=1;
Can1_M3508_MotorStatus[ID-0x201].RawRotorAngle =M3508_RotorNowAngle;//转子机械角度原始数据
Can1_M3508_MotorStatus[ID-0x201].RotorAngle =M3508_RotorNowAngle*0.0439453125f;//=M3508_RotorNowAngle/8192.0f*360.0f;//转子机械角度
Can1_M3508_MotorStatus[ID-0x201].RawRotorPosition =8192*Can1_M3508_MotorStatus[ID-0x201].Rotor_r+M3508_RotorNowAngle;//转子角度位置原始数据
Can1_M3508_MotorStatus[ID-0x201].RotorPosition =360.0f*Can1_M3508_MotorStatus[ID-0x201].Rotor_r+Can1_M3508_MotorStatus[ID-0x201].RotorAngle;//转子角度位置
Can1_M3508_MotorStatus[ID-0x201].RotorSpeed =(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转子转速原始数据
Can1_M3508_MotorStatus[ID-0x201].ShaftPosition =Can1_M3508_MotorStatus[ID-0x201].RotorPosition*0.0520746310219994f;//=Can1_M3508_MotorStatus[ID-0x201].RotorPosition/M3508_ReductionRatio;//转轴角度位置
Can1_M3508_MotorStatus[ID-0x201].Shaft_r =(int64_t)(Can1_M3508_MotorStatus[ID-0x201].ShaftPosition)/360;
if(Can1_M3508_MotorStatus[ID-0x201].ShaftPosition<0 && Can1_M3508_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can1_M3508_MotorStatus[ID-0x201].Shaft_r<0)Can1_M3508_MotorStatus[ID-0x201].Shaft_r--;//获取转轴圈数
Can1_M3508_MotorStatus[ID-0x201].ShaftAngle=Can1_M3508_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can1_M3508_MotorStatus[ID-0x201].Shaft_r;//转轴机械角度
Can1_M3508_MotorStatus[ID-0x201].ShaftSpeed=Can1_M3508_MotorStatus[ID-0x201].RotorSpeed*0.0520746310219994f;//=Can1_M3508_MotorStatus[ID-0x201].RotorSpeed/M3508_ReductionRatio;//转轴转速
Can1_M3508_MotorStatus[ID-0x201].RawCurrent=(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//转矩电流原始数据
Can1_M3508_MotorStatus[ID-0x201].Current=Can1_M3508_MotorStatus[ID-0x201].RawCurrent*0.001220703125f;//=Can1_M3508_MotorStatus[ID-0x201].RawCurrent/16384.0f*20.0f;//转矩电流
Can1_M3508_MotorStatus[ID-0x201].Power=Can1_M3508_MotorStatus[ID-0x201].ShaftSpeed*Can1_M3508_MotorStatus[ID-0x201].Current*0.031413612565445f;//=Can1_M3508_MotorStatus[ID-0x201].ShaftSpeed*Can1_M3508_MotorStatus[ID-0x201].Current*M3508_TorqueConstant/9.55f;//电机功率(功率P(kW)=转轴转速v(RPM)*转矩T(N·m)/9550,转矩T=转矩电流*转矩常数)
if(Can1_M3508_MotorStatus[ID-0x201].Power<0)Can1_M3508_MotorStatus[ID-0x201].Power*=-1;//功率去负数化
Can1_M3508_MotorStatus[ID-0x201].Temperature=Data[6];//电机温度
}
void CAN2_M3508_DataProcess(M3508_ID ID,uint8_t *Data)
{
uint16_t M3508_RotorNowAngle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//本次转子机械角度原始数据
if(M3508_RotorNowAngle-Can2_M3508_MotorStatus[ID-0x201].RawRotorAngle>4000 && Can2_M3508_MotorStatus[ID-0x201].First_Flag==1) Can2_M3508_MotorStatus[ID-0x201].Rotor_r--;//本次转子机械角度原始数据和上次转子机械角度原始数据出现跃变
else if(Can2_M3508_MotorStatus[ID-0x201].RawRotorAngle-M3508_RotorNowAngle>4000 && Can2_M3508_MotorStatus[ID-0x201].First_Flag==1) Can2_M3508_MotorStatus[ID-0x201].Rotor_r++;
else if(Can2_M3508_MotorStatus[ID-0x201].First_Flag!=1) Can2_M3508_MotorStatus[ID-0x201].First_Flag=1;
Can2_M3508_MotorStatus[ID-0x201].RawRotorAngle =M3508_RotorNowAngle;//转子机械角度原始数据
Can2_M3508_MotorStatus[ID-0x201].RotorAngle =M3508_RotorNowAngle*0.0439453125f;//=M3508_RotorNowAngle/8192.0f*360.0f;//转子机械角度
Can2_M3508_MotorStatus[ID-0x201].RawRotorPosition =8192*Can2_M3508_MotorStatus[ID-0x201].Rotor_r+M3508_RotorNowAngle;//转子角度位置原始数据
Can2_M3508_MotorStatus[ID-0x201].RotorPosition =360.0f*Can2_M3508_MotorStatus[ID-0x201].Rotor_r+Can2_M3508_MotorStatus[ID-0x201].RotorAngle;//转子角度位置
Can2_M3508_MotorStatus[ID-0x201].RotorSpeed =(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转子转速原始数据
Can2_M3508_MotorStatus[ID-0x201].ShaftPosition =Can2_M3508_MotorStatus[ID-0x201].RotorPosition*0.0520746310219994f;//=Can2_M3508_MotorStatus[ID-0x201].RotorPosition/M3508_ReductionRatio;//转轴角度位置
Can2_M3508_MotorStatus[ID-0x201].Shaft_r =(int64_t)(Can2_M3508_MotorStatus[ID-0x201].ShaftPosition)/360;
if(Can2_M3508_MotorStatus[ID-0x201].ShaftPosition<0 && Can2_M3508_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can2_M3508_MotorStatus[ID-0x201].Shaft_r<0)Can2_M3508_MotorStatus[ID-0x201].Shaft_r--;//获取转轴圈数
Can2_M3508_MotorStatus[ID-0x201].ShaftAngle=Can2_M3508_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can2_M3508_MotorStatus[ID-0x201].Shaft_r;//转轴机械角度
Can2_M3508_MotorStatus[ID-0x201].ShaftSpeed=Can2_M3508_MotorStatus[ID-0x201].RotorSpeed*0.0520746310219994f;//=Can2_M3508_MotorStatus[ID-0x201].RotorSpeed/M3508_ReductionRatio;//转轴转速
Can2_M3508_MotorStatus[ID-0x201].RawCurrent=(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//转矩电流原始数据
Can2_M3508_MotorStatus[ID-0x201].Current=Can2_M3508_MotorStatus[ID-0x201].RawCurrent*0.001220703125f;//=Can2_M3508_MotorStatus[ID-0x201].RawCurrent/16384.0f*20.0f;//转矩电流
Can2_M3508_MotorStatus[ID-0x201].Power=Can2_M3508_MotorStatus[ID-0x201].ShaftSpeed*Can2_M3508_MotorStatus[ID-0x201].Current*0.031413612565445f;//=Can2_M3508_MotorStatus[ID-0x201].ShaftSpeed*Can2_M3508_MotorStatus[ID-0x201].Current*M3508_TorqueConstant/9.55f;//电机功率(功率P(kW)=转轴转速v(RPM)*转矩T(N·m)/9550,转矩T=转矩电流*转矩常数)
if(Can2_M3508_MotorStatus[ID-0x201].Power<0)Can2_M3508_MotorStatus[ID-0x201].Power*=-1;//功率去负数化
Can2_M3508_MotorStatus[ID-0x201].Temperature=Data[6];//电机温度
}
void Motor_6020_Voltage1( int16_t device1, int16_t device2,
int16_t device3, int16_t device4,
CAN_HandleTypeDef *hcan)//CANID:0x1FF 电机ID:1-4 (-25000,25000)
{
CAN_TxHeaderTypeDef Tx_Message;
uint8_t Can_Send_Data[8];
uint32_t send_mail_box;
Tx_Message.StdId = 0x1FF;
Tx_Message.IDE = CAN_ID_STD;
Tx_Message.RTR = CAN_RTR_DATA;
Tx_Message.DLC = 0x08;
Can_Send_Data[0] = device1 >> 8;
Can_Send_Data[1] = device1;
Can_Send_Data[2] = device2 >> 8;
Can_Send_Data[3] = device2;
Can_Send_Data[4] = device3 >> 8;
Can_Send_Data[5] = device3;
Can_Send_Data[6] = device4 >> 8;
Can_Send_Data[7] = device4;
HAL_CAN_AddTxMessage(hcan, &Tx_Message, Can_Send_Data, &send_mail_box);
}
void Motor_6020_Voltage2( int16_t device1, int16_t device2,
int16_t device3, int16_t device4,
CAN_HandleTypeDef *hcan)//CANID:0x2FF 电机ID:5-8 (-25000,25000)
{
CAN_TxHeaderTypeDef Tx_Message;
uint8_t Can_Send_Data[8];
uint32_t send_mail_box;
Tx_Message.StdId = 0x2FF;
Tx_Message.IDE = CAN_ID_STD;
Tx_Message.RTR = CAN_RTR_DATA;
Tx_Message.DLC = 0x08;
Can_Send_Data[0] = device1 >> 8;
Can_Send_Data[1] = device1;
Can_Send_Data[2] = device2 >> 8;
Can_Send_Data[3] = device2;
Can_Send_Data[4] = device3 >> 8;
Can_Send_Data[5] = device3;
Can_Send_Data[6] = device4 >> 8;
Can_Send_Data[7] = device4;
HAL_CAN_AddTxMessage(hcan, &Tx_Message, Can_Send_Data, &send_mail_box);
}
void CAN1_M6020_DataProcess(M6020_ID ID,uint8_t *Data)
{
uint16_t GM6020_NowAngle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//本次机械角度原始数据
if(GM6020_NowAngle-Can1_M6020_MotorStatus[ID-0x205].Angle>4000 && Can1_M6020_MotorStatus[ID-0x205].First_Flag==1 && Can1_M6020_MotorStatus[ID-0x205].r>0)
Can1_M6020_MotorStatus[ID-0x205].r--;//本次机械角度原始数据和上次机械角度原始数据出现跃变
else if(Can1_M6020_MotorStatus[ID-0x205].Angle-GM6020_NowAngle>4000 && Can1_M6020_MotorStatus[ID-0x205].First_Flag==1 && Can1_M6020_MotorStatus[ID-0x205].r<1)
Can1_M6020_MotorStatus[ID-0x205].r++;
else if(Can1_M6020_MotorStatus[ID-0x205].First_Flag!=1)
Can1_M6020_MotorStatus[ID-0x205].First_Flag++;
Can1_M6020_MotorStatus[ID-0x205].Angle =GM6020_NowAngle;//机械角度
Can1_M6020_MotorStatus[ID-0x205].ANgle =GM6020_NowAngle/8192.0f*360.0f;//机械角度
if(Can1_M6020_MotorStatus[ID-0x205].ANgle > 180) Can1_M6020_MotorStatus[ID-0x205].ANgle -= 360.0f;
else if(Can1_M6020_MotorStatus[ID-0x205].ANgle < -180) Can1_M6020_MotorStatus[ID-0x205].ANgle += 360.0f;
Can1_M6020_MotorStatus[ID-0x205].Position =8192*Can1_M6020_MotorStatus[ID-0x205].r+GM6020_NowAngle;//角度位置
Can1_M6020_MotorStatus[ID-0x205].Speed =(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转速
Can1_M6020_MotorStatus[ID-0x205].Current =(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//实际转矩电流
Can1_M6020_MotorStatus[ID-0x205].Temperature=Data[6];//电机温度
}
void CAN2_M6020_DataProcess(M6020_ID ID,uint8_t *Data)
{
uint16_t GM6020_NowAngle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//本次机械角度原始数据
if(GM6020_NowAngle-Can2_M6020_MotorStatus[ID-0x205].Angle>4000 && Can2_M6020_MotorStatus[ID-0x205].First_Flag==1 && Can2_M6020_MotorStatus[ID-0x205].r>0)
Can2_M6020_MotorStatus[ID-0x205].r--;//本次机械角度原始数据和上次机械角度原始数据出现跃变
else if(Can2_M6020_MotorStatus[ID-0x205].Angle-GM6020_NowAngle>4000 && Can2_M6020_MotorStatus[ID-0x205].First_Flag==1 && Can2_M6020_MotorStatus[ID-0x205].r<1)
Can2_M6020_MotorStatus[ID-0x205].r++;
else if(Can2_M6020_MotorStatus[ID-0x205].First_Flag!=1)
Can2_M6020_MotorStatus[ID-0x205].First_Flag++;
Can2_M6020_MotorStatus[ID-0x205].Angle =GM6020_NowAngle;//机械角度
Can2_M6020_MotorStatus[ID-0x205].ANgle =GM6020_NowAngle/8192.0f*360.0f;//机械角度
if(Can2_M6020_MotorStatus[ID-0x205].ANgle > 180) Can2_M6020_MotorStatus[ID-0x205].ANgle -= 360.0f;
else if(Can2_M6020_MotorStatus[ID-0x205].ANgle < -180) Can2_M6020_MotorStatus[ID-0x205].ANgle += 360.0f;
Can2_M6020_MotorStatus[ID-0x205].Position =8192*Can2_M6020_MotorStatus[ID-0x205].r+GM6020_NowAngle;//角度位置
Can2_M6020_MotorStatus[ID-0x205].Speed =(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转速
Can2_M6020_MotorStatus[ID-0x205].Current =(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//实际转矩电流
Can2_M6020_MotorStatus[ID-0x205].Temperature=Data[6];//电机温度
}
void Motor_2006_Current1( int16_t device1, int16_t device2,
int16_t device3, int16_t device4,
CAN_HandleTypeDef *hcan)//CANID:0x200 电机ID:1-4 (-10000,10000)
{
CAN_TxHeaderTypeDef Tx_Message;
uint8_t Can_Send_Data[8];
uint32_t send_mail_box;
Tx_Message.StdId = 0x200;
Tx_Message.IDE = CAN_ID_STD;
Tx_Message.RTR = CAN_RTR_DATA;
Tx_Message.DLC = 0x08;
Can_Send_Data[0] = device1 >> 8;
Can_Send_Data[1] = device1;
Can_Send_Data[2] = device2 >> 8;
Can_Send_Data[3] = device2;
Can_Send_Data[4] = device3 >> 8;
Can_Send_Data[5] = device3;
Can_Send_Data[6] = device4 >> 8;
Can_Send_Data[7] = device4;
HAL_CAN_AddTxMessage(hcan, &Tx_Message, Can_Send_Data, &send_mail_box);
}
void Motor_2006_Current2( int16_t device1, int16_t device2,
int16_t device3, int16_t device4,
CAN_HandleTypeDef *hcan)//CANID:0x1FF 电机ID:5-8 (-10000,10000)
{
CAN_TxHeaderTypeDef Tx_Message;
uint8_t Can_Send_Data[8];
uint32_t send_mail_box;
Tx_Message.StdId = 0x1FF;
Tx_Message.IDE = CAN_ID_STD;
Tx_Message.RTR = CAN_RTR_DATA;
Tx_Message.DLC = 0x08;
Can_Send_Data[0] = device1 >> 8;
Can_Send_Data[1] = device1;
Can_Send_Data[2] = device2 >> 8;
Can_Send_Data[3] = device2;
Can_Send_Data[4] = device3 >> 8;
Can_Send_Data[5] = device3;
Can_Send_Data[6] = device4 >> 8;
Can_Send_Data[7] = device4;
HAL_CAN_AddTxMessage(hcan, &Tx_Message, Can_Send_Data, &send_mail_box);
}
void CAN1_M2006_DataProcess(M2006_ID ID,uint8_t *Data)
{
uint16_t M2006_RotorNowAngle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//本次转子机械角度原始数据
if(M2006_RotorNowAngle-Can1_M2006_MotorStatus[ID-0x201].RawRotorAngle>4000 && Can1_M2006_MotorStatus[ID-0x201].First_Flag==1)Can1_M2006_MotorStatus[ID-0x201].Rotor_r--;//本次转子机械角度原始数据和上次转子机械角度原始数据出现跃变
else if(Can1_M2006_MotorStatus[ID-0x201].RawRotorAngle-M2006_RotorNowAngle>4000 && Can1_M2006_MotorStatus[ID-0x201].First_Flag==1)Can1_M2006_MotorStatus[ID-0x201].Rotor_r++;
else if(Can1_M2006_MotorStatus[ID-0x201].First_Flag!=1)Can1_M2006_MotorStatus[ID-0x201].First_Flag=1;
Can1_M2006_MotorStatus[ID-0x201].RawRotorAngle=M2006_RotorNowAngle;//转子机械角度原始数据
Can1_M2006_MotorStatus[ID-0x201].RotorAngle=M2006_RotorNowAngle*0.0439453125f;//=M2006_RotorNowAngle/8192.0f*360.0f;//转子机械角度
Can1_M2006_MotorStatus[ID-0x201].RawRotorPosition=8192*Can1_M2006_MotorStatus[ID-0x201].Rotor_r+M2006_RotorNowAngle;//转子角度位置原始数据
Can1_M2006_MotorStatus[ID-0x201].RotorPosition=360.0f*Can1_M2006_MotorStatus[ID-0x201].Rotor_r+Can1_M2006_MotorStatus[ID-0x201].RotorAngle;//转子角度位置
Can1_M2006_MotorStatus[ID-0x201].RotorSpeed=(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转子转速原始数据
Can1_M2006_MotorStatus[ID-0x201].ShaftPosition=Can1_M2006_MotorStatus[ID-0x201].RotorPosition*0.0277777777777778f;//=Can1_M2006_MotorStatus[ID-0x201].RotorPosition/M2006_ReductionRatio;//转轴角度位置
Can1_M2006_MotorStatus[ID-0x201].Shaft_r=(int64_t)(Can1_M2006_MotorStatus[ID-0x201].ShaftPosition)/360;
if(Can1_M2006_MotorStatus[ID-0x201].ShaftPosition<0 && Can1_M2006_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can1_M2006_MotorStatus[ID-0x201].Shaft_r<0)Can1_M2006_MotorStatus[ID-0x201].Shaft_r--;//获取转轴圈数
Can1_M2006_MotorStatus[ID-0x201].ShaftAngle=Can1_M2006_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can1_M2006_MotorStatus[ID-0x201].Shaft_r;//转轴机械角度
Can1_M2006_MotorStatus[ID-0x201].ShaftSpeed=Can1_M2006_MotorStatus[ID-0x201].RotorSpeed*0.0277777777777778f;//=Can1_M2006_MotorStatus[ID-0x201].RotorSpeed/M2006_ReductionRatio;//转轴转速
Can1_M2006_MotorStatus[ID-0x201].RawCurrent=(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//转矩电流原始数据
Can1_M2006_MotorStatus[ID-0x201].Current=Can1_M2006_MotorStatus[ID-0x201].RawCurrent*0.001f;//=Can1_M2006_MotorStatus[ID-0x201].RawCurrent/10000.0f*10.0f;//转矩电流
Can1_M2006_MotorStatus[ID-0x201].Power=Can1_M2006_MotorStatus[ID-0x201].ShaftSpeed*Can1_M2006_MotorStatus[ID-0x201].Current*0.018848167539267f;//=Can1_M2006_MotorStatus[ID-0x201].ShaftSpeed*Can1_M2006_MotorStatus[ID-0x201].Current*M2006_TorqueConstant/9.55f;//电机功率(功率P(kW)=转轴转速v(RPM)*转矩T(N·m)/9550,转矩T=转矩电流*转矩常数)
if(Can1_M2006_MotorStatus[ID-0x201].Power<0)Can1_M2006_MotorStatus[ID-0x201].Power*=-1;//功率去负数化
}
void CAN2_M2006_DataProcess(M2006_ID ID,uint8_t *Data)
{
uint16_t M2006_RotorNowAngle=(uint16_t)((((uint16_t)Data[0])<<8)|Data[1]);//本次转子机械角度原始数据
if(M2006_RotorNowAngle-Can2_M2006_MotorStatus[ID-0x201].RawRotorAngle>4000 && Can2_M2006_MotorStatus[ID-0x201].First_Flag==1)Can2_M2006_MotorStatus[ID-0x201].Rotor_r--;//本次转子机械角度原始数据和上次转子机械角度原始数据出现跃变
else if(Can2_M2006_MotorStatus[ID-0x201].RawRotorAngle-M2006_RotorNowAngle>4000 && Can2_M2006_MotorStatus[ID-0x201].First_Flag==1)Can2_M2006_MotorStatus[ID-0x201].Rotor_r++;
else if(Can2_M2006_MotorStatus[ID-0x201].First_Flag!=1)Can2_M2006_MotorStatus[ID-0x201].First_Flag=1;
Can2_M2006_MotorStatus[ID-0x201].RawRotorAngle=M2006_RotorNowAngle;//转子机械角度原始数据
Can2_M2006_MotorStatus[ID-0x201].RotorAngle=M2006_RotorNowAngle*0.0439453125f;//=M2006_RotorNowAngle/8192.0f*360.0f;//转子机械角度
Can2_M2006_MotorStatus[ID-0x201].RawRotorPosition=8192*Can2_M2006_MotorStatus[ID-0x201].Rotor_r+M2006_RotorNowAngle;//转子角度位置原始数据
Can2_M2006_MotorStatus[ID-0x201].RotorPosition=360.0f*Can2_M2006_MotorStatus[ID-0x201].Rotor_r+Can2_M2006_MotorStatus[ID-0x201].RotorAngle;//转子角度位置
Can2_M2006_MotorStatus[ID-0x201].RotorSpeed=(int16_t)((((uint16_t)Data[2])<<8)|Data[3]);//转子转速原始数据
Can2_M2006_MotorStatus[ID-0x201].ShaftPosition=Can2_M2006_MotorStatus[ID-0x201].RotorPosition*0.0277777777777778f;//=Can2_M2006_MotorStatus[ID-0x201].RotorPosition/M2006_ReductionRatio;//转轴角度位置
Can2_M2006_MotorStatus[ID-0x201].Shaft_r=(int64_t)(Can2_M2006_MotorStatus[ID-0x201].ShaftPosition)/360;
if(Can2_M2006_MotorStatus[ID-0x201].ShaftPosition<0 && Can2_M2006_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can2_M2006_MotorStatus[ID-0x201].Shaft_r<0)Can2_M2006_MotorStatus[ID-0x201].Shaft_r--;//获取转轴圈数
Can2_M2006_MotorStatus[ID-0x201].ShaftAngle=Can2_M2006_MotorStatus[ID-0x201].ShaftPosition-360.0f*Can2_M2006_MotorStatus[ID-0x201].Shaft_r;//转轴机械角度
Can2_M2006_MotorStatus[ID-0x201].ShaftSpeed=Can2_M2006_MotorStatus[ID-0x201].RotorSpeed*0.0277777777777778f;//=Can2_M2006_MotorStatus[ID-0x201].RotorSpeed/M2006_ReductionRatio;//转轴转速
Can2_M2006_MotorStatus[ID-0x201].RawCurrent=(int16_t)((((uint16_t)Data[4])<<8)|Data[5]);//转矩电流原始数据
Can2_M2006_MotorStatus[ID-0x201].Current=Can2_M2006_MotorStatus[ID-0x201].RawCurrent*0.001f;//=Can2_M2006_MotorStatus[ID-0x201].RawCurrent/10000.0f*10.0f;//转矩电流
Can2_M2006_MotorStatus[ID-0x201].Power=Can2_M2006_MotorStatus[ID-0x201].ShaftSpeed*Can2_M2006_MotorStatus[ID-0x201].Current*0.018848167539267f;//=Can2_M2006_MotorStatus[ID-0x201].ShaftSpeed*Can2_M2006_MotorStatus[ID-0x201].Current*M2006_TorqueConstant/9.55f;//电机功率(功率P(kW)=转轴转速v(RPM)*转矩T(N·m)/9550,转矩T=转矩电流*转矩常数)
if(Can2_M2006_MotorStatus[ID-0x201].Power<0)Can2_M2006_MotorStatus[ID-0x201].Power*=-1;//功率去负数化
}
float Motor_Encoder_Circle(float target_angle, float current_encoder_angle)
{
float err = target_angle - current_encoder_angle;
if (err > MOTOR_ENCODER_HALF_ECD_MAX) {
// 当前值落后一圈或多圈，加一个最大值使其追上
current_encoder_angle += MOTOR_ENCODER_ECD_MAX;
} else if (err < -MOTOR_ENCODER_HALF_ECD_MAX) {
// 当前值超前一圈或多圈，减一个最大值使其退回
current_encoder_angle -= MOTOR_ENCODER_ECD_MAX;
}
// 如果误差在范围内，则 current_encoder_angle 不变
return current_encoder_angle;
}