CANopen_Port.c
放入就是针对不同平台实现的钩子函数(CAN
驱动注册,定时器初始化,回调函数注册)并提供Initialize
接口如下。 /* Hooks ---------------------------------------------------------------------*/void vCANOPEN_PORT_Init(void){//first of all setup callback OD_Data->nodeguardError = master402_nodeguardError; OD_Data->heartbeatError = master402_heartbeatError; OD_Data->initialisation = master402_initialisation; OD_Data->preOperational = master402_preOperational; OD_Data->operational = master402_operational; OD_Data->stopped = master402_stopped;// OD_Data->post_sync = master402_post_sync;// OD_Data->post_TPDO = master402_post_TPDO; OD_Data->storeODSubIndex = (storeODSubIndex_t)master402_storeODSubIndex; OD_Data->post_emcy = (post_emcy_t)master402_post_emcy; OD_Data->post_SlaveBootup = master402_post_slaveBootup; OD_Data->post_SlaveStateChange = master402_post_slaveStateChange; /* Init stack timer */ TimerInit(); /* Open the Peak CANOpen device */ IF(!canOpen(&Board,&RPR_Master_Data)) { printf("Check CAN-USB-01 is connected!"); return; } setNodeId(OD_Data, HOST_ID); StartTimerLoop(&Init);} CANopen_CB.c就是初始化注册到协议栈的回调函数。举例如下:
void master402_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){ MSG("received EMCY message. Node: %2.2x ErrorCode: %4.4x ErrorRegister: %2.2x\n", nodeID, errCode, errReg);}void master402_post_slaveBootup(CO_Data* d, UNS8 SlaveID){ MSG("slave bootup Node: %2.2x.\n",SlaveID);}void master402_post_slaveStateChange(CO_Data* d, UNS8 SlaveID,e_nodeState newNodeState){ MSG("slave state changed Node: %2.2x New State: %2.2x.\n",SlaveID,newNodeState);}
app_motor.c就是处理和电机驱动器相关的一些逻辑了,NMT状态机,CIA402状态机,各种模式下的控制逻辑和模式切换逻辑状态机。
app_motor.h是对
电机控制对象建立的数据结构
/* Define to prevent recursive inclusion -------------------------------------*/#
ifndef __MOTOR_H_#
define __MOTOR_H_#
ifdef __cplusplusextern "C" {#
endif/* Includes ------------------------------------------------------------------*/#
include "app_utils.h"/* Exported constants --------------------------------------------------------*/#
define PROFILE_POSITION_MODE 1#
define PROFILE_VELOCITY_MODE 3#
define HOME_MODE 6/* Exported types ------------------------------------------------------------*/typedef enum CONFIG_TABLE{
CONFIG_SERVO_DISABLE_TPDO1,
CONFIG_SERVO_DISABLE_TPDO2,
CONFIG_SERVO_DISABLE_TPDO3,
CONFIG_SERVO_DISABLE_TPDO4,
CONFIG_SERVO_DISABLE_RPDO1,
CONFIG_SERVO_DISABLE_RPDO2,
CONFIG_SERVO_DISABLE_RPDO3,
CONFIG_SERVO_DISABLE_RPDO4,
CONFIG_SERVO_TX1_TYPE,
CONFIG_SERVO_RX1_TYPE,
CONFIG_SERVO_TX2_TYPE,
CONFIG_SERVO_RX2_TYPE,
CONFIG_SERVO_TMAP1_CNT_RESET,
CONFIG_SERVO_TMAP1_PARA1_STATUSWORD,
CONFIG_SERVO_TMAP1_PARA2_MODE,
CONFIG_SERVO_TMAP1_PARA3_POSITION,
CONFIG_SERVO_TMAP1_CNT_SET,
CONFIG_SERVO_RMAP1_CNT_RESET,
CONFIG_SERVO_RMAP1_PARA1_CONTROLWORD,
CONFIG_SERVO_RMAP1_PARA2_MODE,
CONFIG_SERVO_RMAP1_PARA3_POSITION,
CONFIG_SERVO_RMAP1_CNT_SET,
CONFIG_SERVO_TMAP2_CNT_RESET,
CONFIG_SERVO_TMAP2_PARA1_STATUSWORD,
CONFIG_SERVO_TMAP2_PARA2_MODE,
CONFIG_SERVO_TMAP2_PARA3_VELOCITY,
CONFIG_SERVO_TMAP2_CNT_SET,
CONFIG_SERVO_RMAP2_CNT_RESET,
CONFIG_SERVO_RMAP2_PARA1_CONTROLWORD,
CONFIG_SERVO_RMAP2_PARA2_MODE,
CONFIG_SERVO_RMAP2_PARA3_VELOCITY,
CONFIG_SERVO_RMAP2_CNT_SET,
CONFIG_SERVO_ENABLE_TPDO1,
CONFIG_SERVO_ENABLE_RPDO1,
CONFIG_SERVO_ENABLE_TPDO2,
CONFIG_SERVO_ENABLE_RPDO2,
CONFIG_SERVO_PROFILE_VELOCITY,
CONFIG_NUM,
//100*20 2s}
ConfigTable_t;
typedef enum MOTOR_INDEX{
MOTOR1 =
0,
// MOTOR2,// MOTOR3,// MOTOR4,// MOTOR5, MOTOR_NUM}
MotorIndex_e;
typedef struct CSW_BITS{
unsigned int m_switch_on_s:
1;
unsigned int m_enable_voltage_s:
1;
unsigned int m_quick_stop_r:
1;
unsigned int m_enable_amplifier_s:
1;
unsigned int m_mode_specific:
3;
unsigned int m_clear_fault_r_e:
1;
unsigned int m_halt_s:
1;
unsigned int :
7;
unsigned int m_switch_on_is_ready_s:
1;
unsigned int m_switched_on_s:
1;
unsigned int m_operation_enabLED_s:
1;
unsigned int m_fault_present_s:
1;
unsigned int m_voltage_enabled_s:
1;
unsigned int m_quick_stoping_r:
1;
unsigned int m_switch_on_disable_s:
1;
unsigned int m_warning_present_s:
1;
unsigned int m_trajectory_aborted_s:
1;
unsigned int m_canopen_ctrling_s:
1;
unsigned int m_target_reached_s:
1;
unsigned int m_limit_present:
1;
unsigned int m_mode_specific_1:
2;
unsigned int m_moving_s:
1;
unsigned int m_home_captured_s:
1;
}
__attribute__((packed))
CSW_Bits_t;
typedef struct CSW_ELEMENTS{
uint16_t m_u16CW;
uint16_t m_u16SW;}
__attribute__((packed))
CSW_Elements_t;
typedef union CSW{
CSW_Elements_t m_xElements;
CSW_Bits_t m_xFields;}
CSW_t;
typedef struct MOTOR_OBJ{
bool m_bIsQuickStoped;
bool m_bIsSDOCfgPending;
bool m_bIsSDOCfgSuccessful;
uint8_t m_u8ModeSet;
uint8_t m_u8ModeGet;
int32_t m_i32PositionSet;
int32_t m_i32PositionGet;
int32_t m_i32VelocitySet;
int32_t m_i32VelocityGet;
int32_t m_i32HomeOffset;
CSW_t m_xCSW;
V_U8_FUNCTION_SYMBOL m_xTask;
bool m_bIsHomeCplt;
uint8_t m_u8CfgCplt;
Timer_t m_xDispatchInterval;
Timer_t m_xDaemonTimer;
V_U8_FUNCTION_SYMBOL m_xInit;}
MO_t;
typedef struct MOTOR_ALL_OBJ{
MO_t m_xMotor[
MOTOR_NUM];
V_NULL_FUNCTION_SYMBOL m_xInitialize;
V_NULL_FUNCTION_SYMBOL m_xRun;
V_U8_I32_FUNCTION_SYMBOL m_setHome;
V_U8_FUNCTION_SYMBOL m_setStop;
V_U8_I32_FUNCTION_SYMBOL m_setPosition;
V_U8_I32_FUNCTION_SYMBOL m_setSpeed;}
MO_All_t;
/* Exported variables --------------------------------------------------------*/extern MO_All_t motor;
/* Exported macro ------------------------------------------------------------*//* Exported functions prototypes ---------------------------------------------*/#
ifdef __cplusplus}#
endif#
endif
Hmi.cpp
从控制台接收各种控制命令,继承了QThread
,重写run
虚函数实现,和电机控制相关任务线程之间交互采用信号和槽机制。
void HMI::run(){ /* Enter in a loop to read stdin command until "quit" is called */ char command[200]; int ret = 0; forever{ // wait on stdin for string command fgets(command, sizeof(command), stdin); system(CLEARSCREEN); ret = ProcessCommand(command); fflush(stdout); if(ret == QUIT) { emit quit(); return; }
switch(u8Cmd){ case CMD_HOME: emit has_home_cmd(u8Index,i32Value); break; case CMD_POSITION: emit has_position_cmd(u8Index,i32Value); break; case CMD_STOP: emit has_quick_stop_cmd(u8Index); break; case CMD_SPEED: emit has_speed_cmd(u8Index,i32Value); break; default: if(u8Cmd!=0) printf("Unknown command,press \"Enter\" for help info \n"); break; } u8Cmd = 0; }}
RPR_Master.c是CANfestival的EDS编辑工具生成的对象字典,其包括了通讯中使用的很多参数,接下来详细介绍一下。
一、首先,我使用从心跳和节点守护中选择节点守护来保证各个节点的状态监控。对应的对象字典编辑如下:
需要在通讯参数中添加0x100CGuard Time和0x100DLife TimeFactor,就是经过那个时间,主站发送对应的远程帧到总线上,连续经过那个Factor里面的次数都没有收到对象从机的消息,则认为掉线变成Unknownstate,字典中对应的地方如下:
/* index 0x100C : Guard Time. */ UNS16 RPR_Master_obj100C = 0x0; /* 0 */ subindex RPR_Master_Index100C[] = { { RW, uint16, sizeof (UNS16), (void*)&RPR_Master_obj100C } };/* index 0x100D : Life Time Factor. */ UNS8 RPR_Master_obj100D = 0x5; /* 5 */ subindex RPR_Master_Index100D[] = { { RW, uint8, sizeof (UNS8), (void*)&RPR_Master_obj100D } };
这里设置了GuardTime为0,就是暂时禁用了该功能,我在后面合适的时机通过WriteLocalDictionary启动。
二、接下来需要SDO进行参数配置和读取在Pre-Opreational就可用,实际拿来操作包括PDO的参数映射,PDO的禁用启用等,需要先通过SDO来完成,PDO功能需要在设备进入Opreational才启用,五个AXIS,那么就需要搞五个SDOClient分别对应每一个AXIS,从机不需要访问主机,不需要一个SDOServer。
三、我实际使用了PDO
且和利用SYNC
来同步对各个从机的状态和命令。那就需要配置0x1005SYNC COB ID
和0x1006Communication/Cycle Period
参数,嗯,就是用那个频率来同步的意思。字典中对应的地方如下: /* index 0x1005 : SYNC COB ID. */ UNS32 RPR_Master_obj1005 = 0x80; /* 128 */ ODCallback_t RPR_Master_Index1005_callbacks[] = { NULL, }; subindex RPR_Master_Index1005[] = { { RW, uint32, sizeof (UNS32), (void*)&RPR_Master_obj1005 } };/* index 0x1006 : Communication / Cycle Period. */ UNS32 RPR_Master_obj1006 = 0x0; /* 0 */ ODCallback_t RPR_Master_Index1006_callbacks[] = { NULL, }; subindex RPR_Master_Index1006[] = { { RW, uint32, sizeof (UNS32), (void*)&RPR_Master_obj1006 } };
同样的,后面配置好所有轴后,在合适的时机,启动。
四、建立变量,可以放在自定义的地方,或者放在子协议定义的地方,因为你要操作的变量控制字和状态字标准设备协议里面有了,编辑的时候可以快点,放在自定义0x2000开始的地方也行,稍慢一丢丢,可以建立数组和结构体,用面向对象的方式,毕竟不是单轴完了,虽说条条大道通罗马,还是要注重效率。这里就用自定义区建立数组变量来实现了:
五、配置PDO,配置五个轴的活,需要编辑的有点多,应为有TPDO还有RPDO,应为有PDO参数和PDO映射,因为我需要同步控制字,状态字,操作模式,和模式下的变量如位置,速度,电流,所以我需要不止一个PDO,在不同的模式下进行切换就不细写了,写累了如下:
结论:实际测试运行很完美。当然了电机驱动器匹配对应的电机参数,Servo模式下的各个环的PI、PID参数要设置好,保存在驱动器就可以了,附上可执行程序,创龙提供的系统默认环境可运行,可以测试,具体功能见Help,对了使用前需要手动设置好can0的波特率,并使能哦。
转至————电子发烧友