CAN ICD

This log is a mixed bag of a variety of related efforts. The first task was to generate a proper CAN interface control document (ICD). I had previously been adding new CAN messages simply by manually synchronizing the Arduino code for the ESP32 controller and the Atmel Studio project for the SAMD21 on the motor controller. Instead, these should both reference back to a ground-truth ICD. This document is a work in progress and will continue to be modified, but revision 1.0 is on the project page.

While I intend to eventually build a bipedal robot, I want to start with a simpler version first. I plan to build a small quadruped robot that I want to be able to, at minimum:

• Walk forward
• Walk backwards
• Turn in place

I also want to dig into some more complicated and difficult tasks:

• Walking on non-level or cluttered surfaces
  • Detect foot contact with current increase?
• Ambulate with various gaits

In preparation for this, I needed something far less janky than my cardboard structure previously shown. I had a square of HDPE that I will use as the backbone, and will use 3D printed legs. The previous 3D printed legs I had used had a large amount of backlash built in. This was because the motors were not actually bolted to any frame. Instead, a structure was printed to jam the motor in place, and the set screws on the hubs held the assembly together. The image below shows this at the knee joint.

 Clearly, that is not a great long-term solution. The motors' front plates have two M1.6 mounting screws. I purchased some of those, and printed new parts with counterbored holes for those screws. I am still waiting on the screws to arrive, but the following image compares the two "femur" designs. Obviously these are not great feats of mechanical engineering, but to start out I think they should work OK.

The legs will attach to 3D printed mounts bolted to the HDPE sheet backbone. The image below shows the underside of this with the mounts attached. The mounts all have the appropriate counterbored holes for the motor mounting screws.

Although a square clearly is an odd shape for the backbone, it provides copious space to mount all of the electronics. With four legs, we need 4 dual motor controllers. We also have the ESP32 main controller in the center. All of these PCBs are ziptied to the sheet. I only have 6 motors so far, 1 of which is 50:1 (the rest are 150:1), so I can't test it fully yet.

I did however, run motion primitives on all 3 controllers with attached motors and it works as expected. As per the CAN ICD, I added a message to set the motor ticks-per-revolution, so I can configure the one controller with the 50:1 gearmotor to still follow the motion primitive properly, and that works great.

For now, I am waiting on more motors and mounting screws, and then I'll be able to assemble it. I may even be able to get a 3-legged walker going first.

Many industrial robots meant to operate near people (so called co-bots) have a mode where the user can move around the arm, it saves the motion, then it plays it back. Since this motor controller sends its position in telemetry feedback periodically to the ESP32 main controller, we can do something like this.

I wrote this setup all in Arduino. Basically, we want to periodically save keyframes which consist of the time and the positions of both motors. In order to save memory, we filter out keyframes that are too similar to the previous keyframe. Then, on playback, we command the keyframes at the appropriate time. A better setup might interpolate between keyframes and get less jerky motion, but this demo does show the promise of this use case.

The code to perform this is here. It is not pretty, but gets the job done.

#include <CAN.h>
#include "dual_motor_can.h"

#define CAN_RX_PIN      4
#define CAN_TX_PIN      5
#define LED2_PIN              16

#define FRONT_LEFT            (1)
#define FRONT_RIGHT           (2)
#define REAR_LEFT             (4)
#define REAR_RIGHT            (3)

#define MOTOR_0                (0)
#define MOTOR_1                (1)

#define TICKS_PER_REV       (4200.0) // 28 CPR, 150:1
#define NUM_MOTORS          (2)
#define NUM_DEVICES         (5) // Include main controller

#define MAX_FRAMES          (1000)

#define FRAME_MIN_DELTA_DEG (3.0)

typedef enum
{
  NONE,
  RECORD,
  PLAYBACK
} STATE;

typedef struct
{
  float m_0;
  float m_1;
  uint32_t time_since_start;
} keyframe_t;

typedef struct
{
  uint16_t num_frames;
  uint16_t current_frame;
  uint32_t start_time;
  keyframe_t frames[MAX_FRAMES];
} profile_t;

static volatile STATE state = NONE;
static volatile uint32_t record_start_time = 0;
static volatile uint32_t playback_start_time = 0;
static volatile bool frame_received = false;
static can_frame_t frame;

static profile_t profiles[NUM_DEVICES];

static void reset_profiles(void)
{
  // Command to open loop duty mode so it doesn't fight
  send_duty_command(2, 0, 0);
  delay(25);
  send_duty_command(2, 1, 0);
  delay(25);
  for (int i = 0; i < NUM_DEVICES; i++)
  {
    profiles[i].num_frames = 0;
    profiles[i].current_frame = 0;
    profiles[i].start_time = 0;
    for (int j = 0; j < MAX_FRAMES; j++)
    {
      profiles[i].frames[j].m_0 = 0.0;
      profiles[i].frames[j].m_1 = 0.0;
      profiles[i].frames[j].time_since_start = 0;
    }
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(LED2_PIN, OUTPUT);
  digitalWrite(LED2_PIN, LOW);

  Serial.println("CAN position control demo");

  CAN.setPins(CAN_RX_PIN, CAN_TX_PIN);

  // start the CAN bus at 1000 kbps
  if (!CAN.begin(1000E3)) {
    Serial.println("Starting CAN failed!");
    while (1);
  }

  reset_profiles();

  delay(2000);

  // register the receive callback
  CAN.onReceive(onReceive);

}

void loop() {
  static uint32_t last_time = 0;

  uint32_t current_time = millis();

  if (current_time - last_time > 1000)
  {
    digitalWrite(LED2_PIN, !digitalRead(LED2_PIN));
    last_time = current_time;
  }

  if (Serial.available())
  {
    char c = Serial.read();

    if ( c == 'r')
    {
      Serial.println("Recording");
      state = RECORD;
      record_start_time = current_time;
      reset_profiles();
    }
    else if (c == 'p')
    {
      Serial.println("Playing back");
      state = PLAYBACK;
      playback_start_time = current_time;
      for (int i = 0; i < NUM_DEVICES; i++)
      {
        profiles[i].current_frame = 0;
      }
    }
  }

  // playback if needed
  if (state == PLAYBACK)
  {
    uint32_t time_since_start = current_time - playback_start_time;
    for (int i = 0; i < NUM_DEVICES; i++)
    {
      uint16_t num_frames = profiles[i].num_frames;
      uint16_t current_frame = profiles[i].current_frame;
      if (current_frame == num_frames || current_frame >= MAX_FRAMES)
      {
        continue;
      }
      if (time_since_start >= profiles[i].frames[current_frame].time_since_start)
      {
        // Send commands
        Serial.print("Sending device ");
        Serial.print(i);
        Serial.print(" frame ");
        Serial.println(current_frame);
        send_position_command(i, 0, profiles[i].frames[current_frame].m_0);
        delay(25);
        send_position_command(i, 1, profiles[i].frames[current_frame].m_1);
        delay(25);
        profiles[i].current_frame++;
      }
    }
  }

  if (state == RECORD && frame_received)
  {
    can_message_id_t msg_id;
    msg_id.raw_id = frame.id;
    // Unpack
    unpack_can_message(&msg_id);

    if (msg_id.can_msg_type == CAN_MSG_TYPE_INFO && msg_id.can_class == CAN_MSG_CLASS_INFO_TELEMETRY &&
        msg_id.can_index == CAN_MSG_INDEX_INFO_POSITION)
    {
      uint8_t device = msg_id.can_device;

      if (device >= NUM_DEVICES)
      {
        return;
      }

      int32_t position_0, position_1;
      memcpy(&position_0, &frame.data[0], 4);
      memcpy(&position_1, &frame.data[4], 4);

      uint32_t frame_index = profiles[device].current_frame;

      if (frame_index >= MAX_FRAMES)
      {
        Serial.print("Frames full for device ");
        Serial.println(device);
        return;
      }

      if (frame_index == 0)
      {
        profiles[device].start_time = current_time;
      }

      float pos_0 = 360.0 * position_0 / (TICKS_PER_REV); // Save position in degrees
      float pos_1 = 360.0 * position_1 / (TICKS_PER_REV); // Save position in degrees

      bool save_frame = true;

      if (frame_index > 0)
      {
        // Only save frames with a big enough difference
        save_frame = false;
        if (abs(pos_0 - profiles[device].frames[frame_index - 1].m_0) > FRAME_MIN_DELTA_DEG ||
            abs(pos_1 - profiles[device].frames[frame_index - 1].m_1) > FRAME_MIN_DELTA_DEG)
        {
          save_frame = true;
        }
      }

      if (save_frame)
      {
        //Save frame
        Serial.print("Saving device ");
        Serial.print(device);
        Serial.print(" frame ");
        Serial.println(frame_index);
        profiles[device].frames[frame_index].m_0 = pos_0; // Save position in degrees
        profiles[device].frames[frame_index].m_1 = pos_1;
        profiles[device].frames[frame_index].time_since_start = current_time - profiles[device].start_time;

        profiles[device].current_frame++;
        profiles[device].num_frames++;
      }
    }
  }

  frame_received = false;
}

void onReceive(int packetSize) {

  frame.id = CAN.packetId();
  frame.dlc = CAN.packetDlc();

  if (frame.dlc > 8)
  {
    frame.dlc = 8;
  }

  for (int i = 0; i < frame.dlc; i++)
  {
    frame.data[i] = CAN.read();
  }
  frame_received = true;
}