You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
152 lines
3.9 KiB
152 lines
3.9 KiB
/*
|
|
* LS_CAN_Motor_Frames.c
|
|
*
|
|
* Created on: Mar 21, 2025
|
|
* Author: BingooRobotFJX
|
|
*/
|
|
|
|
#include <motors.h>
|
|
#include "fsm_state_control.h"
|
|
#include "BHBF_ROBOT.h"
|
|
FDCANHandler *LS_Motor_Controller;
|
|
DispacherController *LS_DispacherController;
|
|
|
|
void MotorCommandsLoop();
|
|
void MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);
|
|
int32_t speed_E01m_per_min_to_pulse_s(int32_t speed_E01m_per_min);
|
|
|
|
|
|
|
|
char LS_Motor_Need_To_Activate = 0;
|
|
char LS_Motor_Config_Count = 0;
|
|
|
|
/*
|
|
*
|
|
* */
|
|
void LS_Motor_Controller_intialize(FDCANHandler *Handler)
|
|
{
|
|
|
|
|
|
//初始化
|
|
LS_Motor_Controller = Handler;
|
|
LS_Motor_Controller->CAN_Decode = MotorDecodeCAN;
|
|
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
|
"LS_CAN_ID1_LeftMotor", 0, ComError_LS_LeftMotor);
|
|
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
|
"LS_CAN_ID2_RightMotor", 0, ComError_LS_RightMotor);
|
|
|
|
LS_DispacherController = Handler->dispacherController;
|
|
LS_DispacherController->DispacherCallTime = 2;
|
|
LS_DispacherController->Add_Dispatcher_List(LS_DispacherController,
|
|
MotorCommandsLoop);
|
|
}
|
|
|
|
/*
|
|
*
|
|
* */
|
|
void MotorCommandsLoop()
|
|
{
|
|
if (LS_Motor_Need_To_Activate == 1)
|
|
{
|
|
|
|
|
|
LS_Reset_All_Motors(LS_Motor_Controller, 1000);//等待1s
|
|
LS_Reset_All_Motors(LS_Motor_Controller, 500);
|
|
|
|
LS_Activate_All_Motors(LS_Motor_Controller, 1000);
|
|
LS_Activate_All_Motors(LS_Motor_Controller, 1000);
|
|
LS_Activate_All_Motors(LS_Motor_Controller, 500);
|
|
LS_Activate_All_Motors(LS_Motor_Controller, 500);
|
|
LS_Activate_All_Motors(LS_Motor_Controller, 500);
|
|
|
|
|
|
LS_Set_Current_Positon_Zero(GV.LeftMotor.MotorID, LS_Motor_Controller, 50);
|
|
LS_Set_Current_Positon_Zero(GV.RightMotor.MotorID, LS_Motor_Controller, 50);
|
|
LS_SpeedModeSetup(GV.LeftMotor.MotorID, LS_Motor_Controller, 10, 1500000, 1500000,
|
|
0);
|
|
LS_SpeedModeSetup(GV.RightMotor.MotorID, LS_Motor_Controller, 10, 1500000, 1500000,
|
|
0);
|
|
|
|
LS_Motor_Need_To_Activate = 0;
|
|
}
|
|
else
|
|
{
|
|
if(LS_Motor_Config_Count <= 200) /*confirm complete config -->stop send*/
|
|
{
|
|
LS_Motor_Config_Count++;
|
|
LS_Set_WatchDogTime(GV.LeftMotor.MotorID, LS_Motor_Controller, 5);
|
|
LS_Set_WatchDogTime(GV.RightMotor.MotorID, LS_Motor_Controller, 5);
|
|
|
|
}
|
|
LS_Request_Position(GV.LeftMotor.MotorID, LS_Motor_Controller, 2);
|
|
LS_Request_Fault(GV.LeftMotor.MotorID, LS_Motor_Controller, 2);
|
|
LS_Request_Velocity(GV.LeftMotor.MotorID, LS_Motor_Controller, 2);
|
|
LS_Request_Position(GV.RightMotor.MotorID, LS_Motor_Controller, 2);
|
|
LS_Request_Fault(GV.RightMotor.MotorID, LS_Motor_Controller, 2);
|
|
|
|
LS_SpeedMode_Set_TargetSpeed(GV.LeftMotor.MotorID, LS_Motor_Controller, 2,
|
|
speed_E01m_per_min_to_pulse_s(GV.LeftMotor.Target_Velcity) );
|
|
|
|
LS_SpeedMode_Set_TargetSpeed(GV.RightMotor.MotorID, LS_Motor_Controller, 2,
|
|
speed_E01m_per_min_to_pulse_s(GV.RightMotor.Target_Velcity) );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/*
|
|
* 解析函数
|
|
*
|
|
* */
|
|
void MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
|
|
{
|
|
switch (canID - 0x580)
|
|
{
|
|
|
|
case 1:
|
|
{
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
|
"LS_CAN_ID1_LeftMotor", 1);
|
|
LS_Analytic_Fun(GV.LeftMotor.MotorID, buffer); //ID 和数据
|
|
}
|
|
break;
|
|
case 2:
|
|
{
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
|
"LS_CAN_ID2_RightMotor", 1);
|
|
LS_Analytic_Fun(GV.RightMotor.MotorID, buffer);
|
|
}
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
/* DO输出
|
|
* @state: 1:ON; 0:OFF;
|
|
* */
|
|
void LS_Motor_DOSet(uint8_t state)
|
|
{
|
|
if (state == 1)
|
|
{
|
|
LS_DO_Set(GV.LeftMotor.MotorID, 1, state, LS_Motor_Controller, 10);
|
|
}
|
|
else if (state == 0)
|
|
{
|
|
LS_DO_Set(GV.LeftMotor.MotorID, 1, state, LS_Motor_Controller, 10);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* SpeedMPMin:0.1 m/min
|
|
* P*60/10000/70/(Πd)=V
|
|
* return:1 pulse/s
|
|
* */
|
|
int32_t speed_E01m_per_min_to_pulse_s(int32_t speed_E01m_per_min)
|
|
{
|
|
return (int32_t) (speed_E01m_per_min*0.1 *CV.pulse_Per_Circle * CV.wheel_Reduction_Ratio / 60
|
|
/ (3.14 * CV.wheel_Diameter_m));
|
|
}
|
|
|
|
|
|
|