Motor Board Code

The code running on my 2 motor boards is available here.

It sets the motor PWM values and reads the encoders. It has a PID controller for controlling the speed. It supports a simple interface using commands.

Command Interface

Command Structure

All commands are of the form

<commandId,commandName,param1,param2,param3,param4,param5,param6,param7,param8>

  • The commandId is an integer which is chosen by whoever issues the command, and can be used to link the response to the command.

  • The commandName is a String

  • param1..param8 are optional. They should be numbers that can be casted into integers.

Commands

The examples below all use 17 as the command id:

Command Description

<17,SPEED_SET,100,150,1000>

Set the speed of the left motor to 100mm/sec and the right motor to 150mm/sec. After 1000ms stop the motors. If the last parameter is omitted, the command resolves immediately but never issues a stop on the motors. If motorX and motorY are enabled, the command becomes <17, SPEED_SET, 100, 150, 160, 170, 1000> (duration as last parameter).

<17,STOP>

Stop the motors

<17,DRIVE_DIST,100,150>

Drive 100mm with the left motor and (simultaneously) 150mm with the right motor then stop. If motorX and motorY are enabled, the command takes four speeds instead of 2.

<17,PWM_SET,0,255,0,255>

Set the raw PWM values. If motorX and motorY are enabled, the command takes 8 parameters instead of 4.

<17,PID_TUNE,100,200,340>

Retune the PID to Kp=1.00, Ki=2.00, Kd=3.40

<17,SPEED_SET2,100,10>

Set the speed to 100mm/sec linear velocity and 10 deg/sec angular velocity

<17,TURN,20>

Turn the robot 20 degrees CCW (use negative number for CW rotation)

<17,SETUP,140,3576,47>

Set the robot dimensions: 140mm distance between left and right wheel, 3576 encoder pulses per revolution, wheels of 47mm diameter. When SETUP is not used, (140,3576,47) are the default values used.

<17,SET_POSE,0,10,45>

Set the assumed robot pose to x=0mm, y=10mm, phi=45deg

<17,SET_GOAL,0,10,45>

Set the desired goal to x=0mm, y=10mm, phi=45deg. The phi parameter is not used at the moment. Go-to-goal behaviour will start immediately and stops once the goal has been reached.

<17,CLEAR_GOAL>

Stop go-to-goal behaviour.

Incoming information:

Message Description

<OK,17>

Command with id 17 executed sucessfully. Some (most) command return immediately, but TURN and DRIVE_DIST will send this after the distance has been driven.

<NOK,17,This is very wrong>

The command is not recognized or some other error message for command with id 17.

<DIST,200,10>

The left wheel has travelled 200mm and the right wheel 10mm. If motorX and motorY are enabled, their distances will be in the third and four parameters.

<POSE,0,10,45>

The estimated robot pose is now x=0mm, y=10mm, phi=45deg

Communicator

The communicator is what is used for sending commands and retrieving information.

Choose Serial port

The default command interface is Serial1. It can be overwritten by calling setCommunicator after setup has been called.

    ItsyBitsyMotorBoard motorBoard;
    ...
    motorBoard.setup(pulsesPerRev, wheelDiameterMillimeters, leftRightWheelDistanceMillimeters);
    communicator = new SerialCommunicator(&Serial2);
    motorBoard.setCommunicator(communicator);

Use a websocket to send commands and receive data

On the ESP32 version, you can also use a websocket instead of using Serial.

Custom pin config

Before calling setup, you can customize the pins used for the motors and encoders.

motorBoard.settings.motorA.enabled = true;
motorBoard.settings.motorA.pwmPin2 = 25;
motorBoard.settings.motorA.pwmPin1 = 27;
motorBoard.settings.motorA.encoderPin1 = 23;
motorBoard.settings.motorA.encoderPin2 = 19;

motorBoard.settings.motorB.enabled = true;
motorBoard.settings.motorB.pwmPin2 = 32;
motorBoard.settings.motorB.pwmPin1 = 4;
motorBoard.settings.motorB.encoderPin1 = 18;
motorBoard.settings.motorB.encoderPin2 = 26;

// drive four motors instead of 2
motorBoard.settings.motorX.enabled = true;
motorBoard.settings.motorY.enabled = true;

// this will create the motor and encoder objects using the pins configured above
motorBoard.setup(pulsesPerRev, wheelDiameterMillimeters, leftRightWheelDistanceMillimeters);

Library Usage

#include <Arduino.h>
#include <ItsyBitsyMotorBoard.h>

ItsyBitsyMotorBoard motorBoard;

void setup() {
    motorBoard.setup(
    298L * 12L, // long pulsesPerRev,
    47, //int wheelDiameterMillimeters,
    147, //int leftRightWheelDistanceMillimeters
    );

    // how often are the encoder values updated and how often are they sent out
    // (note: no encoder ticks will be missed when increasing throttleMs, it will just
    // not flag it as an update immediately)
    motorBoard.encoderSender->setThrottle(
        40, // int throttleMs
        1000 // int serialThrottleMs
    );
}

void loop() {
    motorBoard.loop();
    Command incomingCommand = motorBoard.commandParser.update();
    if (incomingCommand.isValid) {
        bool handled = motorBoard.handleCommand(&incomingCommand);
        if (!handled) {
            Serial.println("ERR_UNHANDLED:");
            Serial.println(incomingCommand.name);
        }
    }
}

Handle custom commands in your own code

You can add support for additional commands you handle your own. Define a handler function, and call it from loop.

bool handleCommandCustom(Command *theCommand) {
if (!theCommand->isValid) {
    return false;
}
if (strcmp(theCommand->name, "START_DANCE") == 0) {
    // do something
    motorBoard.resolveCommandId(theCommand->id);
    return true;
}

  String temp = "Unknown Command \"";
  temp += theCommand->name;
  temp += "\"";
  motorBoard.rejectCommandId(theCommand->id, temp);

  return false;
}


void loop() {
    motorBoard.loop();
    Command incomingCommand = motorBoard.commandParser.update();
    if (incomingCommand.isValid) {
        bool handled = motorBoard.handleCommand(&incomingCommand);
        if (!handled) {
            handled = handleCommandCustom(&incomingCommand);
        }
        if (!handled) {
            Serial.println("ERR_UNHANDLED:");
            Serial.println(incomingCommand.name);
        }
    }
}