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.
235 lines
7.5 KiB
235 lines
7.5 KiB
/*
|
|
* msp_TTMotor_ZQ.c
|
|
*
|
|
* Created on: Oct 10, 2024
|
|
* Author: akeguo
|
|
*/
|
|
#include "msp_TTMotor_ZQ.h"
|
|
#include "bsp_FDCAN.h"
|
|
|
|
FDCANHandler *ZQ_Motor_Controller;
|
|
HardWareController *ZQ_HardController;
|
|
DispacherController *ZQ_DispacherController;
|
|
void Send1CANTest();
|
|
void ZQ_Motor_Controller_intialize(FDCANHandler *Handler)
|
|
{
|
|
|
|
ZQ_Motor_Controller = Handler;
|
|
ZQ_Motor_Controller->CAN_Decode = DecodeMotorCAN;
|
|
|
|
ZQ_HardController = Handler->canHardWareController;
|
|
//there are three motors, thus add three hardwares
|
|
ZQ_HardController->Add_PCOMHardWare(ZQ_HardController,
|
|
"ZQ_CAN_ID0_LeftMotor", 0);
|
|
ZQ_HardController->Add_PCOMHardWare(ZQ_HardController,
|
|
"ZQ_CAN_ID1_RightMotor", 0);
|
|
ZQ_HardController->Add_PCOMHardWare(ZQ_HardController,
|
|
"ZQ_CAN_ID2_SwingMotor", 0);
|
|
|
|
ZQ_DispacherController = Handler->dispacherController;
|
|
// ZQ_DispacherController->Add_Dispatcher_List(ZQ_DispacherController,
|
|
// Send1CANTest);
|
|
}
|
|
|
|
void Send1CANTest()
|
|
{
|
|
uint8_t data[8] =
|
|
{ 1, 2, 3, 4, 5, 6, 7, 8 };
|
|
ZQ_Motor_Controller->CAN_Send(ZQ_Motor_Controller, 20, 8, data);
|
|
}
|
|
|
|
void DecodeMotorCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
|
|
{
|
|
|
|
|
|
|
|
|
|
//buffer[0] is CAN ID;
|
|
if (1)
|
|
{
|
|
ZQ_HardController->Set_PCOMHardWare(ZQ_HardController,
|
|
"ZQ_CAN_ID0_LeftMotor", 1);
|
|
}
|
|
if (1)
|
|
{
|
|
ZQ_HardController->Set_PCOMHardWare(ZQ_HardController,
|
|
"ZQ_CAN_ID1_RightMotor", 1);
|
|
}
|
|
if (1)
|
|
{
|
|
ZQ_HardController->Set_PCOMHardWare(ZQ_HardController,
|
|
"ZQ_CAN_ID2_SwingMotor", 1);
|
|
}
|
|
}
|
|
|
|
//sub function region
|
|
//activate the motor
|
|
void ActivateMotor(int CANID)
|
|
{
|
|
|
|
memcpy(&ZQ_Motor_Controller->Tx_Buf, 0x10, 0x10);
|
|
ZQ_Motor_Controller->CAN_Send(ZQ_Motor_Controller, CANID, 2,
|
|
ZQ_Motor_Controller->Tx_Buf);
|
|
}
|
|
|
|
void CANSendMessageSDO(int CAN_ID, uint8_t Function, uint16_t ControlWord,
|
|
uint8_t subWord, int32_t ControlWordValue)
|
|
{
|
|
//copy the corresponsiding data to the send Array
|
|
ZQ_Motor_Controller->Tx_Buf[0] = Function;
|
|
memcpy(&ZQ_Motor_Controller->Tx_Buf[1], &ControlWord, 2);
|
|
ZQ_Motor_Controller->Tx_Buf[3] = subWord;
|
|
memcpy(&ZQ_Motor_Controller->Tx_Buf[4], &ControlWordValue, 4);
|
|
//send 8 bytes data
|
|
ZQ_Motor_Controller->CAN_Send(ZQ_Motor_Controller, 600 + CAN_ID, 8,
|
|
ZQ_Motor_Controller->Tx_Buf);
|
|
|
|
}
|
|
|
|
void CANSendMessageSDO_ADD_To_SendList(int CAN_ID, uint8_t Function,
|
|
uint16_t ControlWord, uint8_t subWord, int32_t ControlWordValue)
|
|
{
|
|
//copy the corresponsiding data to the send Array
|
|
ZQ_Motor_Controller->Tx_Buf[0] = Function;
|
|
memcpy(&ZQ_Motor_Controller->Tx_Buf[1], &ControlWord, 2);
|
|
ZQ_Motor_Controller->Tx_Buf[3] = subWord;
|
|
memcpy(&ZQ_Motor_Controller->Tx_Buf[4], &ControlWordValue, 4);
|
|
//send 8 bytes data
|
|
|
|
ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 600 + CAN_ID, 8,
|
|
ZQ_Motor_Controller->Tx_Buf, NULL);
|
|
}
|
|
|
|
void Postion_Velcocity_Run_SetMode(int CAN_ID)
|
|
{
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6060, 0x00, 1); //设置操作模式
|
|
}
|
|
|
|
void Postion_Velcocity_Run_SetParameter(int CAN_ID, int TargetPosition,
|
|
int TargetSpeed, int AccTime, int DecTime)
|
|
{
|
|
if (TargetSpeed >= 2500)
|
|
{
|
|
TargetSpeed = 2500;
|
|
}
|
|
if (TargetSpeed < 0)
|
|
{
|
|
TargetSpeed = 0;
|
|
}
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x6083, 0x00, AccTime); //设置加速时间
|
|
// Thread.Sleep(2);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x6084, 0x00, DecTime); //设置减速时间
|
|
//Thread.Sleep(2);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x607a, 0x00,
|
|
TargetPosition); //设置目标位置
|
|
// Thread.Sleep(2);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x6081, 0x00,
|
|
TargetSpeed * 10); //设置运行速度 0.1rpm
|
|
// Thread.Sleep(2);
|
|
|
|
}
|
|
|
|
void Postion_Velcocity_Run_Start(int CAN_ID)
|
|
{
|
|
//Thread.Sleep(1);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x80); ///6:清除异常，向控制字
|
|
//Thread.Sleep(1);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x06); ///伺服准备
|
|
//Thread.Sleep(1);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x07); ///：伺服等待使能
|
|
|
|
//Thread.Sleep(1);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x2f); ///：
|
|
//Thread.Sleep(1);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x3f); ///：
|
|
// Thread.Sleep(1);
|
|
//Thread.Sleep(1); CANSendMessageSDO(CAN_ID, 0x2b, 0x6040, 0x00, 0x1f);///：
|
|
}
|
|
Postion_Velcocity_Run_Set_Position(int CAN_ID, int TargetPosition)
|
|
{
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x23, 0x607a, 0x00,
|
|
TargetPosition); //设置目标位置
|
|
//Thread.Sleep(2);
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x0f); ///：
|
|
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x3f); ///：
|
|
}
|
|
void Driver_ReadError(int CAN_ID)
|
|
{
|
|
//CANSendMessageSDO_ADD_To_SendList(CAN_ID, new byte[8] { 0x40, 0x0A, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00 });//40 0A 30 00 00 00 00 00
|
|
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x40, 0x300a, 0x00, 0); //40 0A 30 00 00 00 00 00
|
|
}
|
|
void SetMotorTargetPosition(int MotorID, int32_t TargetPosition)
|
|
{
|
|
|
|
Postion_Velcocity_Run_Set_Position(MotorID, TargetPosition);
|
|
|
|
}
|
|
/// <summary>
|
|
/// 读取是否到最终达位置
|
|
/// </summary>
|
|
/// <param name="CAN_ID"></param>
|
|
void Driver_Read_Read_ReachedEnd(int CAN_ID)
|
|
{
|
|
// CANSendMessageSDO(CAN_ID, new byte[8] { 0x40, 0x0A, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00 });//40 0A 30 00 00 00 00 00
|
|
//Thread.Sleep(1);
|
|
CANSendMessageSDO(CAN_ID, 0x40, 0x6041, 0x00, 0x00); ///：
|
|
|
|
}
|
|
void Postion_Velcocity_Stop(int CANID)
|
|
{
|
|
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6085, 0x00, 200); // 6085 的减速时间100ms
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x605a, 0x00, 6); // 0x06：按索引 6085 的减速时间进行减速，停下时电机 轴处于锁轴状态。
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x02); //快速停机
|
|
}
|
|
|
|
void SetCurrentPositionZero(int CANID)
|
|
{
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6098, 0x00, 0x23); //1：设置定位的方式,向索 引0x6098:00写入0X23
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6060, 0x00, 6); //2：设置操作模式，向索引0x6060:00写入0x06
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x80); //3:清除异常，向控制字 0x6040:00写入0x80
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x06); //4：伺服准备，向控制字 0x6040:00写入0x06
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x07); //5：伺服等待使能，向控制字0x6040:00写入0x07
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2b, 0x6040, 0x00, 0x0F); //6：伺服使能，向控制字0x6040:00写入0x0F
|
|
CANSendMessageSDO_ADD_To_SendList(CAN_ID, 0x2f, 0x6040, 0x00, 0x1F); //7：启动原点定位,向控制字0x6040:00写入0x1F
|
|
}
|
|
void Intialize(int CANID, int intializePosition, int Speed, int AccTime,
|
|
int DecTime, int waitTime)
|
|
{
|
|
// CANSendMessage(CAN_ID, 0, new byte[] { 0x10, 0x10 }, 2);//Wait 2ms Bootloader
|
|
|
|
//copy the corresponsiding data to the send Array
|
|
ZQ_Motor_Controller->Tx_Buf[0] = 0x10;
|
|
ZQ_Motor_Controller->Tx_Buf[1] = 0x10;
|
|
|
|
ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, CAN_ID, 2,
|
|
ZQ_Motor_Controller->Tx_Buf, NULL);
|
|
|
|
|
|
Postion_Velcocity_Stop(CANID);
|
|
}
|
|
|
|
void BootLoader(int CAN_ID)
|
|
{
|
|
ZQ_Motor_Controller->Tx_Buf[0] = 0x10;
|
|
ZQ_Motor_Controller->Tx_Buf[1] = 0x10;
|
|
|
|
ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, CAN_ID, 2,
|
|
ZQ_Motor_Controller->Tx_Buf, NULL);
|
|
}
|
|
|
|
MotorParameters motorParameters[4];
|
|
/// <summary>
|
|
/// 电机驱动器状态刷新
|
|
/// </summary>
|
|
/// <param name="Data">数据</param>
|
|
/// <param name="len">数据长度</param>
|
|
void MotorStatusRefresh(char* Data, uint8_t len)
|
|
{
|
|
|
|
|
|
|
|
}
|
|
|
|
|