Build a Robot

5 min readMay 2, 2024

Let’s build a robot.

We’ll start with some actuators.

The MyActuator RMD-X10–100 looks good. It has a built-in motor controller, a 35:1 gearbox, so is super strong, and also I now have two of them, courtesy of K-Scale Labs.

To control it, we can send it RS-485 (differential serial) commands, or we can use a CAN bus. Let’s use CAN.

To use a CAN bus, let’s get a CAN transceiver with a built-in microcontroller, like the RP2040 feather CAN board. I bought one here: https://www.adafruit.com/product/5724

Once it arrives, let’s connect the controller board to the motor, along with a 48V power supply. Connect motor CAN_H to board CAN_H, motor CAN_L to board CAN_L. Connect positive from power supply to motor positive, and negative to negative.

Then, we’ll start reading parameters from the motor controller, to check communication is working, and then start sending it current control commands.

Open up the Arduino IDE, click Tools -> Board -> Boards Manager, install Raspberry Pi Mbed OS RP2040 Boards and Raspberry Pi Pico/RP2040. Then click Tools -> Manage Libraries…, and install the Adafruit MCP2515 library. See this guide for more info on installing.

Then we put this code in:

/*
 * CAN communication to MyActuator motors
 * Thomas Jacobs 2024
 */

#include <Adafruit_MCP2515.h>

// CAN comms
#define CAN_BAUDRATE (1000000)
Adafruit_MCP2515 mcp(PIN_CAN_CS);

void setup() {
  // Print
  Serial.begin(115200);
  while(!Serial) delay(10);
  Serial.println("CAN motor comms");

  // Init CAN
  if (!mcp.begin(CAN_BAUDRATE)) {
    Serial.println("Error initializing CAN chip.");
    while(1) delay(10);
  }
  Serial.println("CAN chip found");
}

void loop() {
  // Send packet
  Serial.print("Sending CAN packet... ");
  mcp.beginPacket(0x141); // Assume motor ID 1
  mcp.write(0x30);        // Read PID params
  mcp.write(0x0);
  mcp.write(0x0);
  mcp.write(0x0);
  mcp.write(0x0);
  mcp.write(0x0);
  mcp.write(0x0);
  mcp.write(0x0);
  mcp.endPacket();
  Serial.println("Sent");

  // Receive packet
  int packetSize = mcp.parsePacket();
  if (packetSize) {
    // Received a packet
    Serial.print("Received packet with id 0x");
    Serial.print(mcp.packetId(), HEX);
    Serial.print(": ");

    // Print hex bytes
    while (mcp.available()) {
      Serial.print(" 0x");
      Serial.print((char)mcp.read(), HEX);
    }
    Serial.println();
  }

  // Wait
  delay(1000);
}

Flash this onto your RP2040 CAN board, and open up the serial console in Arduino IDE. If your motor is powered on, connected correctly, and has CAN ID 1, it should start saying:

CAN motor comms
CAN chip found
Sending CAN packet... Sent
Received packet with id 0x241: 0x30 0x0 0x55 0x19 0x55 0x19 0x55 0x19

Good. Let’s upgrade this code to get and set CAN IDs and control the motor:

/*
 * CAN communication to MyActuator motors
 * Thomas Jacobs 2024
 */

#include <Adafruit_MCP2515.h>

// CAN comms
#define CAN_BAUDRATE (1000000)
Adafruit_MCP2515 mcp(PIN_CAN_CS);

// Commands
#define GET_PID_PARAMS 0x30
#define GET_ANGLE 0x94
#define STOP_MOTOR 0x81
#define SET_TORQUE 0xA1
#define SET_ANGLE 0xA6
#define GET_SET_ID 0x79
#define SET_ID 0x0
#define GET_ID 0x1

// IDs
#define MOTOR 0x140
#define MOTOR_REPLY 0x240
#define ALL_MOTORS 0x280
#define MAX_MOTORS 32

void setup() {
  // Print
  Serial.begin(115200);
  while(!Serial) delay(10);
  Serial.println("CAN motor comms");

  // Set up blink LED
  pinMode(LED_BUILTIN, OUTPUT);

  // Init CAN
  if (!mcp.begin(CAN_BAUDRATE)) {
    Serial.println("Error initializing CAN chip.");
    while(1) delay(10);
  }
  Serial.println("CAN chip found");
}

void sendCANCommand(int id, uint8_t command, int param1, int param2, int param3) {
  // Pack packet
  uint8_t data1 = 0, data2 = 0, data3 = 0, data4 = 0, data5 = 0, data6 = 0, data7 = 0;
  if (command == GET_SET_ID) {
    data2 = param1;
    data7 = param2;
  }
  else if (command == SET_TORQUE) {
    data4 = (uint8_t)(param1);
    data5 = (uint8_t)(param1>>8);
  }
  else if (command == SET_ANGLE) {
    data1 = param1;               // spinDirection
    data2 = (uint8_t)(param2);    // maxSpeed
    data3 = (uint8_t)(param2>>8);
    data4 = (uint8_t)(param3);    // angleControl
    data5 = (uint8_t)(param3>>8);
  }

  // Send packet
  Serial.print("Sending CAN packet 0x");
  Serial.println(command, HEX);
  mcp.beginPacket(id);
  mcp.write(command);
  mcp.write(data1);
  mcp.write(data2);
  mcp.write(data3);
  mcp.write(data4);
  mcp.write(data5);
  mcp.write(data6);
  mcp.write(data7);
  mcp.endPacket();
}

bool receiveCANPacket() {
  // Receive packet
  int packetSize = mcp.parsePacket();
  if (packetSize) {
    // Received a packet
    Serial.print("Received packet with id 0x");
    Serial.print(mcp.packetId(), HEX);
    Serial.print(": ");

    // Parse packet
    if (mcp.packetId() >= MOTOR_REPLY && mcp.packetId() <= MOTOR_REPLY + MAX_MOTORS) {
      uint8_t reply = (uint8_t)mcp.read();
      if (reply == GET_PID_PARAMS) {
        Serial.print("Motor params:");
      }
      else if (reply == GET_SET_ID) {
        Serial.print("ID:");
      }
      else if (reply == SET_TORQUE) {
        Serial.print("Torque:");
      }
      else if (reply == GET_ANGLE) {
        Serial.print("Get angle: ");
        mcp.read();
        mcp.read();
        mcp.read();
        mcp.read();
        mcp.read();
        int data6 = mcp.read();
        int data7 = mcp.read();
        int angle = (data7 << 8) + data6;
        Serial.print(angle);
      }
      else if (reply == SET_ANGLE) {
        Serial.print("Set angle:");
      }
      else if (reply == STOP_MOTOR) {
        Serial.print("Stop motor:");
      }
    }

    // Print hex bytes
    while (mcp.available()) {
      Serial.print(" 0x");
      Serial.print((uint8_t)mcp.read(), HEX);
    }
    Serial.println();

    // We got a packet
    return true;
  }
  
  // No packet found
  return false;
}

void loop() {
  // Motor
  int motorNum = 1;

  // Get ID
  //sendCANCommand(MOTOR + motorNum, GET_SET_ID, GET_ID, 0, 0);

  // Get motor PID params
  //sendCANCommand(MOTOR + motorNum, GET_PID_PARAMS, 0, 0, 0);

  // Get motor angle command
  sendCANCommand(MOTOR + motorNum, GET_ANGLE, 0, 0, 0);

  // Set action
  static int state = 1;
  static int t = 0;
  t++;
  if (t > 1) {
    // Flip
    if (state == 1) state = 2;
    else if (state == 2) state = 1;
    t = 0;
  }

  // Do action
  if (state == 1) {
    // Set motor torque command
    sendCANCommand(MOTOR + motorNum, SET_TORQUE, 50, 0, 0);
  }
  else if (state == 2) {
    // Set motor torque command
    sendCANCommand(MOTOR + motorNum, SET_TORQUE, -50, 0, 0);
  }
  else if (state == 3) {
    // Set motor angle command
    sendCANCommand(MOTOR + motorNum, SET_ANGLE, 1, 100, 10000);
  }
  else if (state == 4) {
    // Stop motor
    sendCANCommand(MOTOR + motorNum, STOP_MOTOR, 0, 0, 0);
  }

  // Receive packets
  while( receiveCANPacket() ) { };

  // Wait and blink
  digitalWrite(LED_BUILTIN, HIGH);
  delay(500);
  digitalWrite(LED_BUILTIN, LOW);
  delay(500);
}

This should tell the motor to start moving back and forth every two seconds:

CAN motor comms
CAN chip found
Sending CAN packet 0x94
Sending CAN packet 0xA1
Received packet with id 0x241: Torque: 0x19 0xB5 0xFF 0x1A 0xFF 0xFC 0x0
Received packet with id 0x241: Get angle: 39864

And voilà, you have a giant dangerous self-fidgeting spinner:

Next up, part II: The actual robot building.

Build a Robot

Written by Tom Jacobs