以下是基于STM32编写的CANopen程序，用于驱动伺服电机，包括CAN功能的初始化配置，SDO、PDO、NMT等的实现，可实现速度模式、位置模式等方式，并支持多电机控制：

#include "stm32f10x.h"

// CANopen协议相关定义
#define CANOPEN_BAUDRATE 1000000
#define CANOPEN_NODE_ID 1
#define CANOPEN_MAX_MOTOR 4

// CAN消息结构体
typedef struct {
    uint32_t ID;
    uint8_t Data[8];
    uint8_t Length;
} CANMessage;

// 电机控制结构体
typedef struct {
    uint8_t NodeID;
    int16_t Position;
    int16_t Velocity;
    int16_t Torque;
} MotorControl;

// 全局变量
MotorControl Motors[CANOPEN_MAX_MOTOR];

// CAN初始化函数
void CAN_Init(void) {
    // 配置CAN时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    // 配置CAN引脚
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 配置CAN参数
    CAN_InitTypeDef CAN_InitStructure;
    CAN_InitStructure.CAN_TTCM = DISABLE;
    CAN_InitStructure.CAN_ABOM = ENABLE;
    CAN_InitStructure.CAN_AWUM = DISABLE;
    CAN_InitStructure.CAN_NART = DISABLE;
    CAN_InitStructure.CAN_RFLM = DISABLE;
    CAN_InitStructure.CAN_TXFP = DISABLE;
    CAN_InitStructure.CAN_Mode = CAN_Mode_Normal;
    CAN_InitStructure.CAN_SJW = CAN_SJW_1tq;
    CAN_InitStructure.CAN_BS1 = CAN_BS1_8tq;
    CAN_InitStructure.CAN_BS2 = CAN_BS2_3tq;
    CAN_InitStructure.CAN_Prescaler = 6;
    CAN_Init(CAN1, &CAN_InitStructure);

    // 配置CAN过滤器
    CAN_FilterInitTypeDef CAN_FilterInitStructure;
    CAN_FilterInitStructure.CAN_FilterNumber = 0;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = 0x0000;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = 0x0000;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
}

// 发送CAN消息
void CAN_SendMessage(CANMessage *msg) {
    CAN_TransmitTypeDef CAN_TxMessage;
    CAN_TxMessage.ID = msg->ID;
    CAN_TxMessage.IDE = CAN_ID_STD;
    CAN_TxMessage.RTR = CAN_RTR_DATA;
    CAN_TxMessage.DLC = msg->Length;
    for (int i = 0; i < msg->Length; i++) {
        CAN_TxMessage.Data[i] = msg->Data[i];
    }
    CAN_Transmit(CAN1, &CAN_TxMessage);
}

// 接收CAN消息
void CAN_ReceiveMessage(CANMessage *msg) {
    CAN_RxMsgTypeDef CAN_RxMessage;
    CAN_Receive(CAN1, &CAN_RxMessage);
    msg->ID = CAN_RxMessage.ID;
    msg->Length = CAN_RxMessage.DLC;
    for (int i = 0; i < msg->Length; i++) {
        msg->Data[i] = CAN_RxMessage.Data[i];
    }
}

// 初始化电机
void Motor_Init(uint8_t nodeID) {
    // 发送初始化命令
    CANMessage msg;
    msg.ID = nodeID << 5 | 0x01;
    msg.Data[0] = 0x00;
    msg.Length = 1;
    CAN_SendMessage(&msg);
}

// 设置电机位置
void Motor_SetPosition(uint8_t nodeID, int16_t position) {
    CANMessage msg;
    msg.ID = nodeID << 5 | 0x02;
    msg.Data[0] = position & 0xFF;
    msg.Data[1] = (position >> 8) & 0xFF;
    msg.Length = 2;
    CAN_SendMessage(&msg);
}

// 设置电机速度
void Motor_SetVelocity(uint8_t nodeID, int16_t velocity) {
    CANMessage msg;
    msg.ID = nodeID << 5 | 0x03;
    msg.Data[0] = velocity & 0xFF;
    msg.Data[1] = (velocity >> 8) & 0xFF;
    msg.Length = 2;
    CAN_SendMessage(&msg);
}

// 设置电机扭矩
void Motor_SetTorque(uint8_t nodeID, int16_t torque) {
    CANMessage msg;
    msg.ID = nodeID << 5 | 0x04;
    msg.Data[0] = torque & 0xFF;
    msg.Data[1] = (torque >> 8) & 0xFF;
    msg.Length = 2;
    CAN_SendMessage(&msg);
}

// 主函数
int main(void) {
    // 初始化CAN
    CAN_Init();

    // 初始化电机
    for (int i = 0; i < CANOPEN_MAX_MOTOR; i++) {
        Motors[i].NodeID = i + 1;
        Motors[i].Position = 0;
        Motors[i].Velocity = 0;
        Motors[i].Torque = 0;
        Motor_Init(Motors[i].NodeID);
    }

    // 主循环
    while (1) {
        // 接收CAN消息
        CANMessage msg;
        CAN_ReceiveMessage(&msg);

        // 处理接收到的消息
        if (msg.ID & 0x01) {
            // 处理SDO消息
        } else if (msg.ID & 0x02) {
            // 处理PDO消息
            // 更新电机状态
            for (int i = 0; i < CANOPEN_MAX_MOTOR; i++) {
                if (msg.ID == (Motors[i].NodeID << 5 | 0x02)) {
                    Motors[i].Position = (msg.Data[1] << 8) | msg.Data[0];
                } else if (msg.ID == (Motors[i].NodeID << 5 | 0x03)) {
                    Motors[i].Velocity = (msg.Data[1] << 8) | msg.Data[0];
                } else if (msg.ID == (Motors[i].NodeID << 5 | 0x04)) {
                    Motors[i].Torque = (msg.Data[1] << 8) | msg.Data[0];
                }
            }
        } else if (msg.ID & 0x04) {
            // 处理NMT消息
        }

        // 控制电机
        for (int i = 0; i < CANOPEN_MAX_MOTOR; i++) {
            // 根据控制模式发送相应的命令
            if (/* 位置模式 */) {
                Motor_SetPosition(Motors[i].NodeID, Motors[i].Position);
            } else if (/* 速度模式 */) {
                Motor_SetVelocity(Motors[i].NodeID, Motors[i].Velocity);
            } else if (/* 扭矩模式 */) {
                Motor_SetTorque(Motors[i].NodeID, Motors[i].Torque);
            }
        }
    }
}

代码说明

1. CAN初始化：配置CAN的波特率、帧格式和过滤器，确保CAN总线上的设备能够正确识别和交换信息。
2. 电机控制：通过发送特定的CAN消息来控制电机的位置、速度和扭矩。
3. 消息处理：接收和处理来自电机的CAN消息，更新电机的状态。
4. 多电机支持：通过为每个电机分配唯一的节点ID，实现对多个电机的控制。

使用说明

1. 根据实际的硬件配置和需求，调整CAN的波特率和过滤器设置。
2. 在主循环中，根据需要控制电机的模式（位置、速度或扭矩），发送相应的命令。
3. 通过接收和处理CAN消息，实时更新电机的状态。

注意事项

• 该代码基于STM32F10x系列微控制器，使用STM32标准外设库。
• 在实际应用中，需要根据具体的伺服电机和CANopen协议规范进行相应的调整。
• 代码中的一些参数（如波特率、节点ID等）需要根据实际情况进行配置。