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.
106 lines
2.8 KiB
106 lines
2.8 KiB
3 weeks ago
|
/*
|
||
|
|
* motors.c
|
||
|
|
*
|
||
|
|
* Created on: Nov 11, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
#include "motors.h"
|
||
|
|
#include "msp_TTMotor_ZQ.h"
|
||
|
|
|
||
|
|
char TT_Motor_Need_To_Activate = 0;
|
||
|
|
|
||
|
|
FDCANHandler *Roughening_Motor_Controller;
|
||
|
|
|
||
|
|
DispacherController *Roughening_DispacherController;
|
||
|
|
|
||
|
|
#define LeftMotorID 1
|
||
|
|
#define RightMotorID 2
|
||
|
|
|
||
|
|
void MotorCommandsLoop();
|
||
|
|
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);
|
||
|
|
|
||
|
|
void Roughening_Motor_Controller_intialize(FDCANHandler *Handler)
|
||
|
|
{
|
||
|
|
//初始化
|
||
|
|
|
||
|
|
Roughening_Motor_Controller = Handler;
|
||
|
|
Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN;
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
||
|
|
"ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor);
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
||
|
|
"ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor);
|
||
|
|
|
||
|
|
Roughening_DispacherController = Handler->dispacherController;
|
||
|
|
Roughening_DispacherController->DispacherCallTime = 2;
|
||
|
|
Roughening_DispacherController->Add_Dispatcher_List(
|
||
|
|
Roughening_DispacherController, MotorCommandsLoop);
|
||
|
|
|
||
|
|
LOGFF(DL_WARN,"TT_Motors_intialize");
|
||
|
|
|
||
|
|
}
|
||
|
|
void MotorCommandsLoop()
|
||
|
|
{
|
||
|
|
|
||
|
|
if (TT_Motor_Need_To_Activate == 1)
|
||
|
|
{
|
||
|
|
// ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 6000);
|
||
|
|
|
||
|
|
|
||
|
|
ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000);
|
||
|
|
ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 150, 150, 0);
|
||
|
|
SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 150, 150, 0);
|
||
|
|
// SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0);
|
||
|
|
// SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0);
|
||
|
|
TT_Motor_Need_To_Activate = 2;
|
||
|
|
}
|
||
|
|
else if (TT_Motor_Need_To_Activate == 2)
|
||
|
|
{
|
||
|
|
for (int i = 1; i < 3; i++)
|
||
|
|
{
|
||
|
|
TT_Request_Position(i, Roughening_Motor_Controller, 6);
|
||
|
|
TT_Request_Velocity(i, Roughening_Motor_Controller, 6);
|
||
|
|
TT_Request_Current(i, Roughening_Motor_Controller, 6);
|
||
|
|
TT_Request_Fault(i, Roughening_Motor_Controller, 6);
|
||
|
|
TT_Request_Tempature(i, Roughening_Motor_Controller, 6);
|
||
|
|
}
|
||
|
|
|
||
|
|
TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 6,
|
||
|
|
GV.LeftMotor.Target_Velcity);
|
||
|
|
TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 6,
|
||
|
|
-GV.RightMotor.Target_Velcity);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
char a[10];
|
||
|
|
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
|
||
|
|
{
|
||
|
|
|
||
|
|
memcpy(a,buffer,8);
|
||
|
|
switch (canID - 0x580)
|
||
|
|
{
|
||
|
|
|
||
|
|
case LeftMotorID:
|
||
|
|
{
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
"ZQ_CAN_ID2_LeftMotor", 1);
|
||
|
|
TT_Analytic_Fun(LeftMotorID, buffer);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case RightMotorID:
|
||
|
|
{
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
"ZQ_CAN_ID3_RightMotor", 1);
|
||
|
|
TT_Analytic_Fun(RightMotorID, buffer);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|