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.
88 lines
2.6 KiB
88 lines
2.6 KiB
2 weeks ago
|
/*
|
||
|
|
* motors.c
|
||
|
|
*
|
||
|
|
* Created on: 4.3, 2026
|
||
|
|
* Author: SHI
|
||
|
|
* MKS驱动器，42步进电机
|
||
|
|
* 波特率 115200
|
||
|
|
*/
|
||
|
|
#include "motors.h"
|
||
|
|
|
||
|
|
struct UARTHandler *Motor_485_Controller;
|
||
|
|
DispacherController *Motor_485_DispacherController;
|
||
|
|
|
||
|
|
void MotorCommandsLoop();
|
||
|
|
void Motor_485_Decode(uint8_t *buffer, uint16_t length);
|
||
|
|
int32_t speed_mpmin_to_rpmin(double speed_m_per_min);
|
||
|
|
|
||
|
|
void Motor_Controller_intialize(struct UARTHandler *Handler)//485
|
||
|
|
{
|
||
|
|
//初始化
|
||
|
|
|
||
|
|
Motor_485_Controller = Handler;
|
||
|
|
// Motor_485_Controller->UART_Decode = Motor_485_Decode;
|
||
|
|
Motor_485_Controller->Wait_time = 6; //等待10ms 最低不要低于4；
|
||
|
|
|
||
|
|
Motor_485_DispacherController = Handler->dispacherController;
|
||
|
|
Motor_485_DispacherController->Dispacher_Enable = 1;//不周期性发送
|
||
|
|
Motor_485_DispacherController->DispacherCallTime = 100;
|
||
|
|
// Motor_485_DispacherController->DispacherCallTime = 0;
|
||
|
|
Motor_485_DispacherController->Add_Dispatcher_List(Motor_485_DispacherController,
|
||
|
|
MotorCommandsLoop);
|
||
|
|
//将 MotorCommandsLoop 函数注册到分发控制器dispacherController中，使其按照设定的周期被自动调用
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Motor_485_1",
|
||
|
|
0, ComError_LS_LeftMotor);
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "Motor_485_2",
|
||
|
|
0, ComError_LS_RightMotor);
|
||
|
|
LOG("Motor_485 initialized success");
|
||
|
|
// for (int i = 1; i < 3; i++)
|
||
|
|
// {
|
||
|
|
//
|
||
|
|
// GetCSPByCommand(i, Motor_485_Controller, 4); //get the data from the motors
|
||
|
|
//
|
||
|
|
// Motor_ClearFault(i, Motor_485_Controller, 4);
|
||
|
|
// //Motor_SetVelocityModeAndTargetVelocity(i,Motor[i]->Target_Velcity);
|
||
|
|
// }
|
||
|
|
|
||
|
|
}
|
||
|
|
void MotorCommandsLoop()
|
||
|
|
{
|
||
|
|
int32_t Target_Velcity_Left = speed_mpmin_to_rpmin(Motor485[1]->Target_Velcity);
|
||
|
|
int32_t Target_Velcity_Right = speed_mpmin_to_rpmin(Motor485[2]->Target_Velcity);
|
||
|
|
|
||
|
|
speedModeRun_485(Motor485[1]->MotorID, Target_Velcity_Left, Motor_485_Controller); //从机地址，速度，
|
||
|
|
speedModeRun_485(Motor485[2]->MotorID, Target_Velcity_Right, Motor_485_Controller); //从机地址，速度，
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Motor_485_Decode(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
uint8_t canID = buffer[1];
|
||
|
|
switch (canID)
|
||
|
|
{
|
||
|
|
case 1:
|
||
|
|
{
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
"Motor_485_1", 1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
{
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
"Motor_485_2", 1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
Decode485Motor(buffer, length);
|
||
|
|
}
|
||
|
|
|
||
|
|
int32_t speed_mpmin_to_rpmin(double speed_m_per_min)
|
||
|
|
{
|
||
|
|
return (int32_t) (speed_m_per_min * CV.wheel_Reduction_Ratio
|
||
|
|
/ (3.14159f * CV.wheel_Diameter_m));
|
||
|
|
//speed_m_per_min=10 对应 796 RPM
|
||
|
|
}
|