Forum

Notifications
Clear all

CAN Bus Communication for Servo 28D/35D/42D/57D Series Closed Loop Stepper Driver

1 Posts
1 Users
0 Reactions
25 Views
T Zhao
Posts: 2
Admin
Topic starter
Member
Joined: 5 days ago
[#81]

Case 1,  Single Drive over CAN Bus

Hardware:  Nema17 with Servo42D, MKS CANable V2.0 USB-CAN, 12V DC Power Power Supply

Wiring: Refer gitbub user manual: Github User Manual, (Note: MKS_ServoD_Control software is not suitable for testing the CAN bus, use CNGaroo or Python)

Step one:  Test the communication, Here, I am using Python to do the test, without using software tool such as CANgaroo.

  • Use USB cable connect the CANable adaper to PC
  • Use three dopont wires to connect CANable adaper and MKS Servo42D Motor Driver(CAN-H, CAN-L and GRD)
  • 12V DC Power to Servo42D Motor Driver
  • The following code to test the communication, only CAN ID received is successful. 
import can
import time

bus = can.Bus(
    interface="slcan",
    channel="COM5",       # change to your COM port
    bitrate=500000
)

print("CAN bus opened")

# Send Servo42D encoder request
msg = can.Message(
    arbitration_id=0x01,
    data=[0x30, 0x31],
    is_extended_id=False
)

print("Sending:", msg)
bus.send(msg)

print("Waiting for response...")

# Listen for up to 3 seconds
start = time.time()

while time.time() - start < 3:
    response = bus.recv(timeout=0.5)

    if response is not None:
        print("RECEIVED:", response)
        print("ID:", hex(response.arbitration_id))
        print("DATA:", response.data.hex(" ").upper())
        print("DLC:", response.dlc)

print("Done")

bus.shutdown()

Step two,  Run the motor.  

The following code will control the stepper motor by selecting its direction and speed:

# MKS Servo42D/57D CAN Bus Testing

import can
import time

# =========================
# CAN configuration
# =========================
COM_PORT = "COM5"       # <-- change this based on your USB Port
CAN_BITRATE = 500000
CAN_ID = 0x01

# =========================
# Open CAN bus
# =========================
bus = can.Bus(
    interface="slcan",
    channel=COM_PORT,
    bitrate=CAN_BITRATE
)

print("CAN bus opened")
print(f"Port: {COM_PORT}")
print(f"CAN ID: 0x{CAN_ID:X}")
print(f"Bitrate: {CAN_BITRATE}")
print()


# =========================
# Send CAN frame
# =========================
def send(data):
    msg = can.Message(
        arbitration_id=CAN_ID,
        data=data,
        is_extended_id=False
    )

    print("TX:", " ".join(f"{x:02X}" for x in data))

    try:
        bus.send(msg)
    except can.CanError as e:
        print("CAN ERROR:", e)


# =========================
# Read CAN response
# =========================
def receive(timeout=1.0):
    msg = bus.recv(timeout=timeout)

    if msg is None:
        print("RX: no response")
        return None

    print(
        f"RX ID: 0x{msg.arbitration_id:X}  "
        f"DATA: {msg.data.hex(' ').upper()}  "
        f"DLC: {msg.dlc}"
    )

    return msg

# =========================
# Enable motor
# =========================
def enable():
    # F3 01 + checksum F5
    send([0xF3, 0x01, 0xF5])
    receive()


# =========================
# Disable motor
# =========================
def disable():
    # F3 00 + checksum F4
    send([0xF3, 0x00, 0xF4])
    receive()


# =========================
# Read encoder
# =========================
def read_encoder():
    # 30 31
    send([0x30, 0x31])
    receive()


# =========================
# Speed command
# =========================
def speed_command(rpm, direction=0, acceleration=2):

    if rpm < 0:
        rpm = -rpm
        direction = 1

    if rpm > 2047:
        print("RPM too high")
        return

    # 12-bit speed value
    speed = int(rpm)

    high = (speed >> 8) & 0x0F
    low = speed & 0xFF

    # Bit 7 = direction
    if direction:
        high |= 0x80

    # Calculate checksum
    checksum = (
        CAN_ID +
        0xF6 +
        high +
        low +
        acceleration
    ) & 0xFF

    data = [
        0xF6,
        high,
        low,
        acceleration,
        checksum
    ]

    send(data)


# =========================
# Stop motor
# =========================
def stop():
    # F6 00 00 00 + checksum
    checksum = (
        CAN_ID +
        0xF6
    ) & 0xFF

    send([
        0xF6,
        0x00,
        0x00,
        0x00,
        checksum
    ])

    receive()


# =========================
# Main menu
# =========================
try:

    while True:

        print()
        print("============================")
        print(" MKS SERVO42D CAN TEST")
        print("============================")
        print("1 - Enable motor")
        print("2 - Disable motor")
        print("3 - Read encoder")
        print("4 - CW 100 RPM")
        print("5 - CCW 100 RPM")
        print("6 - CW 300 RPM")
        print("7 - CCW 300 RPM")
        print("8 - STOP")
        print("9 - Exit")
        print("============================")

        choice = input("Select: ")
        if choice == "1":
            enable()

        elif choice == "2":
            disable()

        elif choice == "3":
            read_encoder()

        elif choice == "4":
            speed_command(100, direction=0)

        elif choice == "5":
            speed_command(100, direction=1)

        elif choice == "6":
            speed_command(300, direction=0)

        elif choice == "7":
            speed_command(300, direction=1)

        elif choice == "8":
            stop()

        elif choice == "9":
            stop()
            break

        else:
            print("Invalid selection")

finally:
    bus.shutdown()
    print("CAN bus closed")

 

Notes: If the communication cannot be created between CANable Adapter and Servo42D, check the resister (120 Ω) either in adapter or Servo42D board, at leaset one resistor should be connected for the single master motor. 


Share: