CAN communication using Teensy 4.1 for Cubemars Motor AK70-10(HELP NEEDED)

iplaycsgo4evr · Feb 22, 2025

i have been trying to do can communication with the motor but am not getting any responses im particularly new to this and would appreciate some help in understanding whats wrong with my code
i am Using pins 30 and 31 which are CRX3 and CTRX3 resp which are connected to the CRX and CTX pins of the MCP 2551 and the CANH and CANL pins of the MCP 2551 are connected to the Motor
if anyone has worked with these motors any help would be appreciated
THANK YOU
#include <FlexCAN_T4.h>

// ====== Define CubeMars CAN Packet IDs ======
enum CAN_PACKET_ID {
CAN_PACKET_SET_DUTY = 0,
CAN_PACKET_SET_CURRENT = 1,
CAN_PACKET_SET_CURRENT_BRAKE = 2,
CAN_PACKET_SET_RPM = 3,
CAN_PACKET_SET_POS = 4,
CAN_PACKET_SET_ORIGIN_HERE = 5,
CAN_PACKET_SET_POS_SPD = 6
};

// ====== Motor Data Structure ======
struct Axis {
uint8_t packet[8];
float serial_pos;
float motor_vel;
int motor_dir;
uint16_t controller_id;
int16_t motor_pos;
int16_t motor_spd;
int16_t motor_cur;
int8_t motor_temp;
int8_t motor_error;
};

// ====== Initialize FlexCAN Bus Object ======
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> canBus; // Teensy's built-in CAN1
Axis motor;

void setup() {
Serial.begin(115200);
Serial.println("Starting...");
delay(100);

canBus.begin();
canBus.setBaudRate(1000000); // Set CAN baud rate to 1Mbps

// Setup motor with CubeMars motor ID
setup_motor(&motor,0x00, 1); // Controller ID = 0, Direction = 1

// Enter motor control mode
uint8_t enter_control_mode[8] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFC};
send_can_packet(motor.controller_id, enter_control_mode, sizeof(enter_control_mode));

Serial.println("Sent: Enter Motor Control Mode");
delay(100);
}

void loop() {
// Handle CAN events

comm_can_set_rpm(motor, 1000.0); // Example: Set motor RPM to 1000

// Listen for incoming messages
CAN_message_t rxMsg;
if (canBus.read(rxMsg)) {
process_can_message(&motor, rxMsg);
debug_motor(&motor);
}

delay(100); // Delay to prevent spamming the CAN bus
}

// ====== Motor Initialization ======
void setup_motor(Axis *axis, uint8_t id, int dir) {
axis->controller_id = id;
axis->motor_dir = dir;
}

// ====== Send a CAN Packet ======
void send_can_packet(uint32_t id, uint8_t *data, uint8_t len) {
CAN_message_t txMsg;
txMsg.id = id;
txMsg.len = len;
txMsg.flags.extended = 1; // Use Extended ID (needed for CubeMars motors)
memcpy(txMsg.buf, data, len);

canBus.write(txMsg);
}

// ====== Set Motor RPM ======
void comm_can_set_rpm(Axis &axis, float rpm) {
uint32_t id = axis.controller_id | (CAN_PACKET_SET_RPM << 8);

uint8_t buffer[4];
int32_t index = 0;
buffer_append_int32(buffer, (int32_t)(rpm * axis.motor_dir), &index);

send_can_packet(id, buffer, sizeof(buffer));
}

// ====== Process Incoming CAN Messages ======
void process_can_message(Axis *axis, const CAN_message_t &rxMsg) {
if ((rxMsg.id & 0xFF) == axis->controller_id) {
axis->motor_pos = (rxMsg.buf[0] << 8) | rxMsg.buf[1];
axis->motor_spd = (rxMsg.buf[2] << 8) | rxMsg.buf[3];
axis->motor_cur = (rxMsg.buf[4] << 8) | rxMsg.buf[5];
axis->motor_temp = rxMsg.buf[6];
axis->motor_error = rxMsg.buf[7];
}
}

// ====== Debug Function to Print Motor Data ======
void debug_motor(Axis *axis) {
Serial.print("Motor ID: "); Serial.println(axis->controller_id);
Serial.print("Position: "); Serial.println(axis->motor_pos);
Serial.print("Speed: "); Serial.println(axis->motor_spd);
Serial.print("Current: "); Serial.println(axis->motor_cur);
Serial.print("Temperature: "); Serial.println(axis->motor_temp);
Serial.print("Error: "); Serial.println(axis->motor_error);
}

// ====== Append int32 to Buffer ======
void buffer_append_int32(uint8_t *buffer, int32_t data, int32_t *index) {
buffer[(*index)++] = (data >> 24) & 0xFF;
buffer[(*index)++] = (data >> 16) & 0xFF;
buffer[(*index)++] = (data >> 8) & 0xFF;
buffer[(*index)++] = data & 0xFF;
}

BriComp · Feb 22, 2025

When posting your code, please post it using the </> button. This maintains the code formatting and makes it easier for others to understand your code and be able to help.
Unfortunately I have not used the CAN bus so I am unable to help you.
Below is what the code looks like when pasted correctly formatted.

Code:

#include <FlexCAN_T4.h>

// ====== Define CubeMars CAN Packet IDs ======
enum CAN_PACKET_ID {
    CAN_PACKET_SET_DUTY = 0,
    CAN_PACKET_SET_CURRENT = 1,
    CAN_PACKET_SET_CURRENT_BRAKE = 2,
    CAN_PACKET_SET_RPM = 3,
    CAN_PACKET_SET_POS = 4,
    CAN_PACKET_SET_ORIGIN_HERE = 5,
    CAN_PACKET_SET_POS_SPD = 6
};

// ====== Motor Data Structure ======
struct Axis {
    uint8_t packet[8];
    float serial_pos;
    float motor_vel;
    int motor_dir;
    uint16_t controller_id;
    int16_t motor_pos;
    int16_t motor_spd;
    int16_t motor_cur;
    int8_t motor_temp;
    int8_t motor_error;
};

// ====== Initialize FlexCAN Bus Object ======
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> canBus; // Teensy's built-in CAN1
Axis motor;

void setup() {
    Serial.begin(115200);
    Serial.println("Starting...");
    delay(100);

    canBus.begin();
    canBus.setBaudRate(1000000); // Set CAN baud rate to 1Mbps

    // Setup motor with CubeMars motor ID
    setup_motor(&motor, 0x00, 1); // Controller ID = 0, Direction = 1

    // Enter motor control mode
    uint8_t enter_control_mode[8] = { 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFC };
    send_can_packet(motor.controller_id, enter_control_mode, sizeof(enter_control_mode));

    Serial.println("Sent: Enter Motor Control Mode");
    delay(100);
}

void loop() {
    // Handle CAN events

    comm_can_set_rpm(motor, 1000.0); // Example: Set motor RPM to 1000

    // Listen for incoming messages
    CAN_message_t rxMsg;
    if (canBus.read(rxMsg)) {
        process_can_message(&motor, rxMsg);
        debug_motor(&motor);
    }

    delay(100); // Delay to prevent spamming the CAN bus
}

// ====== Motor Initialization ======
void setup_motor(Axis* axis, uint8_t id, int dir) {
    axis->controller_id = id;
    axis->motor_dir = dir;
}

// ====== Send a CAN Packet ======
void send_can_packet(uint32_t id, uint8_t* data, uint8_t len) {
    CAN_message_t txMsg;
    txMsg.id = id;
    txMsg.len = len;
    txMsg.flags.extended = 1; // Use Extended ID (needed for CubeMars motors)
    memcpy(txMsg.buf, data, len);

    canBus.write(txMsg);
}

// ====== Set Motor RPM ======
void comm_can_set_rpm(Axis& axis, float rpm) {
    uint32_t id = axis.controller_id | (CAN_PACKET_SET_RPM << 8);

    uint8_t buffer[4];
    int32_t index = 0;
    buffer_append_int32(buffer, (int32_t)(rpm * axis.motor_dir), &index);

    send_can_packet(id, buffer, sizeof(buffer));
}

// ====== Process Incoming CAN Messages ======
void process_can_message(Axis* axis, const CAN_message_t& rxMsg) {
    if ((rxMsg.id & 0xFF) == axis->controller_id) {
        axis->motor_pos = (rxMsg.buf[0] << 8) | rxMsg.buf[1];
        axis->motor_spd = (rxMsg.buf[2] << 8) | rxMsg.buf[3];
        axis->motor_cur = (rxMsg.buf[4] << 8) | rxMsg.buf[5];
        axis->motor_temp = rxMsg.buf[6];
        axis->motor_error = rxMsg.buf[7];
    }
}

// ====== Debug Function to Print Motor Data ======
void debug_motor(Axis* axis) {
    Serial.print("Motor ID: "); Serial.println(axis->controller_id);
    Serial.print("Position: "); Serial.println(axis->motor_pos);
    Serial.print("Speed: "); Serial.println(axis->motor_spd);
    Serial.print("Current: "); Serial.println(axis->motor_cur);
    Serial.print("Temperature: "); Serial.println(axis->motor_temp);
    Serial.print("Error: "); Serial.println(axis->motor_error);
}

// ====== Append int32 to Buffer ======
void buffer_append_int32(uint8_t* buffer, int32_t data, int32_t* index) {
    buffer[(*index)++] = (data >> 24) & 0xFF;
    buffer[(*index)++] = (data >> 16) & 0xFF;
    buffer[(*index)++] = (data >> 8) & 0xFF;
    buffer[(*index)++] = data & 0xFF;
}

PaulS · Feb 22, 2025

iplaycsgo4evr said:
i am Using pins 30 and 31 which are CRX3 and CTRX3 resp which are connected to the CRX and CTX pins of the MCP 2551 and the CANH and CANL pins of the MCP 2551 are connected to the Motor

If you're using CAN3 you should initialize the FlexCAN Bus object like so:

C++:

FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_16> canBus; // Teensy's built-in CAN3

Paul

skpang · Feb 23, 2025

The MCP2551 is a 5v device with a 5v on the logic pin. The Teensy 4.1 is not tolerant on its input, you will damage the Teensy if it receives 5v.

Try and use the MCP2562 instead. It has a separate VIO pin for the logic.

iplaycsgo4evr · Feb 24, 2025

skpang said:
The MCP2551 is a 5v device with a 5v on the logic pin. The Teensy 4.1 is not tolerant on its input, you will damage the Teensy if it receives 5v.

Try and use the MCP2562 instead. It has a separate VIO pin for the logic.

what if i use a logic level shifter ?

skpang · Feb 24, 2025

Yes, you can use a level shifter but your Teensy 4.1 might have already been damaged if you applied 5v to its input.

Try to use CAN1 or CAN2 instead of CAN3.

CAN communication using Teensy 4.1 for Cubemars Motor AK70-10(HELP NEEDED)

iplaycsgo4evr

New member

BriComp

Well-known member

PaulS

Well-known member

skpang

Well-known member

iplaycsgo4evr

New member

skpang

Well-known member