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.
243 lines
8.5 KiB
243 lines
8.5 KiB
/*
|
|
* motors.c
|
|
*
|
|
* Created on: Nov 11, 2024
|
|
* Author: akeguo
|
|
*/
|
|
#include "motors.h"
|
|
#include "msp_TTMotor_ZQ.h"
|
|
|
|
char TT_Motor_Need_To_Activate = 1;
|
|
char TT_Motor_Need_To_Activate_1 = 1;
|
|
FDCANHandler *Roughening_Motor_Controller;
|
|
|
|
FDCANHandler *Roughening_Motor_Controller_CAN2;
|
|
|
|
DispacherController *Roughening_DispacherController;
|
|
DispacherController *Roughening_DispacherController_CAN2;
|
|
void MotorCommandsLoop();
|
|
|
|
void MotorCommandsLoop_2();
|
|
void MotorCommandsLoop_2_Position();
|
|
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);
|
|
|
|
void Motor_Controller_intialize(FDCANHandler *Handler)
|
|
{
|
|
//初始化
|
|
|
|
Roughening_Motor_Controller = Handler;
|
|
Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN;
|
|
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 0, ComError_ZQ_CAN_ID1_LeftMotor);
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 0, ComError_ZQ_CAN_ID2_RightMotor);
|
|
// HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 0, ComError_ZQ_CAN_ID3_SwingMotor);
|
|
|
|
Roughening_DispacherController = Handler->dispacherController;
|
|
Roughening_DispacherController->DispacherCallTime = 2;
|
|
Roughening_DispacherController->Add_Dispatcher_List(Roughening_DispacherController, MotorCommandsLoop);
|
|
|
|
}
|
|
void Motor_Controller_intialize_CAN2(FDCANHandler *Handler)
|
|
{
|
|
//初始化
|
|
|
|
Roughening_Motor_Controller_CAN2 = Handler;
|
|
Roughening_Motor_Controller_CAN2->CAN_Decode = Roughening_MotorDecodeCAN;
|
|
|
|
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 0, ComError_ZQ_CAN_ID3_SwingMotor);
|
|
|
|
Roughening_DispacherController_CAN2 = Handler->dispacherController;
|
|
Roughening_DispacherController_CAN2->DispacherCallTime = 2;
|
|
Roughening_DispacherController_CAN2->Add_Dispatcher_List(Roughening_DispacherController_CAN2, MotorCommandsLoop_2_Position);
|
|
|
|
|
|
}
|
|
|
|
|
|
void MotorCommandsLoop()
|
|
{
|
|
static int Heartbeat_Flag_01;
|
|
if (TT_Motor_Need_To_Activate == 1)
|
|
{
|
|
ActivateMotor(1, Roughening_Motor_Controller, 6000);
|
|
ActivateMotor(2, Roughening_Motor_Controller, 2000);
|
|
|
|
Enable_NMT(000,Roughening_Motor_Controller,01,1000);
|
|
Enable_NMT(000,Roughening_Motor_Controller,02,1000);
|
|
|
|
Configure_Asynchronous_Mode(1, Roughening_Motor_Controller, 7,600);
|
|
Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4);
|
|
Configure_Asynchronous_Mode(2, Roughening_Motor_Controller, 8,600);
|
|
Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4);
|
|
SpeedModeSetup(1, Roughening_Motor_Controller, 6, 500, 500, 0);
|
|
Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller, 4);
|
|
SpeedModeSetup(2, Roughening_Motor_Controller, 6, 500, 500, 0);
|
|
Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4);
|
|
Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller, 4);
|
|
TT_Motor_Need_To_Activate = 0;
|
|
}
|
|
else
|
|
{
|
|
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_SpeedMode_Set_TargetSpeed(1, Roughening_Motor_Controller, 6,GV.LeftMotor.Target_Velcity);
|
|
TT_SpeedMode_Set_TargetSpeed(2, Roughening_Motor_Controller, 6,GV.RightMotor.Target_Velcity);
|
|
}
|
|
|
|
Heartbeat_Flag_01++;
|
|
if(Heartbeat_Flag_01%7==0)
|
|
{
|
|
Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller, 4);
|
|
Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller, 4);
|
|
Heartbeat_Flag_01=0;
|
|
}
|
|
}
|
|
|
|
void MotorCommandsLoop_2_Position()
|
|
{
|
|
static int Heartbeat_Flag;
|
|
static int Lag_Sent_Num=0;
|
|
static int32_t Speed_Last=0;
|
|
static int32_t Position_Last=0;
|
|
|
|
if (TT_Motor_Need_To_Activate_1 == 1)
|
|
{
|
|
ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000);
|
|
ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
|
|
ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
|
|
// Enable_NMT(000,Roughening_Motor_Controller_CAN2,03,1000);
|
|
// Configure_Asynchronous_Mode(3, Roughening_Motor_Controller_CAN2, 9,500);
|
|
// Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2, 4);
|
|
|
|
|
|
// void SpeedModeSetup(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime, int32_t Acc, int32_t Dec, int32_t TargetVelocity) //设定速度模式，并更改相关速度
|
|
// SpeedModeSetup(3, Roughening_Motor_Controller_CAN2, 12, 500, 500, 0);
|
|
|
|
Postion_Velcocity_Run_SetParameter( 3, 0, 0, 500, 500, Roughening_Motor_Controller_CAN2, 100 );
|
|
// void Postion_Velcocity_Run_SetParameter(int32_t MotorID, int32_t TargetPosition, int32_t TargetSpeed, int32_t AccTime, int32_t DecTime, FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime)
|
|
TT_Motor_Need_To_Activate_1 = 0;
|
|
}
|
|
else
|
|
{
|
|
for (int i = 3; i <4 ; i++)//前两个电机
|
|
{
|
|
TT_Request_Position(3, Roughening_Motor_Controller_CAN2, 10);
|
|
// TT_Request_Current(3, Roughening_Motor_Controller_CAN2, 10);
|
|
// TT_Request_Velocity(3, Roughening_Motor_Controller_CAN2, 10);
|
|
TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 10);
|
|
}
|
|
|
|
switch(GV.SwingMotor.Position_immediately1_Lag2)
|
|
{
|
|
case 1:
|
|
// if((Position_Last!=GV.SwingMotor.Tar_Position_count)||(Speed_Last!=GV.SwingMotor.Tar_Position_Velcity_RPM))
|
|
// {
|
|
// Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,20);
|
|
// Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
|
|
// }
|
|
// Position_Last=GV.SwingMotor.Tar_Position_count;
|
|
// Speed_Last=GV.SwingMotor.Tar_Position_Velcity_RPM;
|
|
Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
|
|
break;
|
|
case 2:
|
|
// if((Position_Last!=GV.SwingMotor.Tar_Position_count)||(Speed_Last!=GV.SwingMotor.Tar_Position_Velcity_RPM))
|
|
// {
|
|
// Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
|
|
//// Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
|
|
// }
|
|
// Position_Last=GV.SwingMotor.Tar_Position_count;
|
|
// Speed_Last=GV.SwingMotor.Tar_Position_Velcity_RPM;
|
|
|
|
Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Heartbeat_Flag++;
|
|
// if(Heartbeat_Flag%5==0)
|
|
// {
|
|
// Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2, 4);
|
|
// Heartbeat_Flag=0;
|
|
// }
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void MotorCommandsLoop_2()
|
|
{
|
|
static int Heartbeat_Flag;
|
|
if (TT_Motor_Need_To_Activate_1 == 1)
|
|
{
|
|
|
|
ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000);
|
|
ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
|
|
ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
|
|
Enable_NMT(000,Roughening_Motor_Controller_CAN2,03,1000);
|
|
Configure_Asynchronous_Mode(3, Roughening_Motor_Controller_CAN2, 9,1000);
|
|
SpeedModeSetup(3, Roughening_Motor_Controller_CAN2, 12, 500, 500, 0);
|
|
TT_Motor_Need_To_Activate_1 = 0;
|
|
}
|
|
else
|
|
{
|
|
for (int i = 3; i <4 ; i++)//前两个电机
|
|
{
|
|
TT_Request_Position(3, Roughening_Motor_Controller_CAN2, 6);
|
|
TT_Request_Current(3, Roughening_Motor_Controller_CAN2, 6);
|
|
TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 6);
|
|
}
|
|
TT_SpeedMode_Set_TargetSpeed(3, Roughening_Motor_Controller_CAN2, 6,GV.SwingMotor.Target_Velcity);
|
|
Heartbeat_Flag++;
|
|
if(Heartbeat_Flag%15==0)
|
|
{
|
|
Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2, 4);
|
|
Heartbeat_Flag=0;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
char a[10];
|
|
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
|
|
{
|
|
|
|
memcpy(a,buffer,8);
|
|
switch (canID - 0x580)
|
|
{
|
|
|
|
case 1:
|
|
{
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
|
|
TT_Analytic_Fun(1, buffer);
|
|
}
|
|
break;
|
|
case 2:
|
|
{
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1);
|
|
TT_Analytic_Fun(2, buffer);
|
|
}
|
|
break;
|
|
case 3:
|
|
{
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1);
|
|
TT_Analytic_Fun(3, buffer);
|
|
}
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|