Micro Maestro Servo Controller: Arduino code

Hi,

I’ve been working on a pseudo-library (a sketch you just drop beside yours) to use the Micro Maestro (or several of them daisy chained) with an Arduino. The code is somehow similar to the one released by Mike Crist (2009-01-03) for the Pololu Micro Serial Servo Controller as it has been my starting point.

It has all the serial commands implemented in both Compact and Pololu protocol, as well as a few shortcuts like functions for setting off servos.

You can either use it with a software serial library (prefer NewSoftSerial; using with SoftwareSerial may need some changes on the commands that return a value since it has no available() function) or a hardware serial. You just need to comment out the SOFTWARE SERIAL ONLY or HARDWARE SERIAL ONLY at the beginning of the file. Edit the settings (pins, baudrate, use of the /RST pin, serial name, etc) at the top of the file to match your installation.

Here is the current version, feel free to help test it and/or comment. Any help to improve it much appreciated !

//------------------------------------------------
// Communication with Pololu Micro Maestro Servo Controller
//------------------------------------------------
// https://www.pololu.com/catalog/product/1350
// Documentation : https://www.pololu.com/docs/0J40
// based on the Pololu micro serial servo controller by mike crist 2009-01-03
//
// Xevel, 2010-03-01
// http://xevel.fr

//----------------------------------------------------------------------
//                           Serial settings           
//----------------------------------------------------------------------

// GENERAL SETTINGS
//#define MAESTRO_DETECT_BAUDRATE    // comment if using "UART, fixed baud rate"
#define MAESTRO_BAUDRATE         57600  // set to the baudrate used.

#define MAESTRO_USE_RST_PIN  // uncomment if you want to be able to reset with the RST pin of the board
#define servoControllerResetPin  8  // digital pin connected to Maestro RST

//---------------------------------------------------------------------
// SOFTWARE SERIAL ONLY. Comment this block if using a hardware serial.

#include <NewSoftSerial.h>
#define servoControllerRxPin     10 // digital pin connected to Maestro TX
#define servoControllerTxPin     9  // digital pin connected to Maestro RX
NewSoftSerial MaestroSerial =  NewSoftSerial(servoControllerRxPin, servoControllerTxPin);

#define MAESTRO_USE_SOFT_SERIAL
#define MAESTRO_START_TRANSMISSION() noInterrupts()
#define MAESTRO_END_TRANSMISSION()   interrupts()

//---------------------------------------------------------------------


//---------------------------------------------------------------------
// HARDWARE SERIAL ONLY. Comment this block if using a software serial
/*
  #define MaestroSerial Serial // set to the name of the serial you want to use
  #define MAESTRO_START_TRANSMISSION()
  #define MAESTRO_END_TRANSMISSION()
*/
//---------------------------------------------------------------------

//----------------------------------------------------------------------
//                         Function declarations           
//----------------------------------------------------------------------

void maestro_init_serial();
void maestro_hard_reset();

// Servo commands
void maestro_set_target(byte servo, int angle);
void maestro_set_target(byte device, byte servo, int angle);
void maestro_set_target_minissc(byte servo, byte angle);
void maestro_set_speed(byte servo, int speedVal);
void maestro_set_speed(byte device, byte servo, int speedVal);
void maestro_set_acceleration(byte servo, byte accel);
void maestro_set_acceleration(byte device, byte servo, byte accel);
void maestro_go_home();
void maestro_go_home(byte device);
void maestro_set_servo_off(byte servo);
void maestro_set_servo_off(byte device, byte servo);
void maestro_set_all_servos_off();
int maestro_get_position(byte servo);
int maestro_get_position(byte device, byte servo);
byte maestro_get_moving_state();
byte maestro_get_moving_state(byte device);
int maestro_get_errors();
int maestro_get_errors(byte device);

// Script commands
void maestro_stop_script();
void maestro_stop_script(byte device);
byte maestro_get_script_running();
byte maestro_get_script_running(byte device);
void maestro_restart_script_param(byte subroutine, int param);
void maestro_restart_script_param(byte device, byte subroutine, int param);
void maestro_restart_script(byte subroutine);
void maestro_restart_script(byte device, byte subroutine);

//-----------------------------------------------------------------------
//-----------------------------------------------------------------------
//-----------------------------------------------------------------------


void maestro_init_serial(){
#ifdef MAESTRO_USE_SOFT_SERIAL
  // talking to Pololu Serial Servo Controller
  pinMode(servoControllerRxPin, INPUT);
  digitalWrite(servoControllerTxPin, HIGH); // keeps pololu board from getting spurious signal. Serial is idle high.
  pinMode(servoControllerTxPin, OUTPUT);
#endif

#ifdef MAESTRO_USE_RST_PIN
  pinMode(servoControllerResetPin,OUTPUT);
  maestro_hard_reset(); // reset the servo controller before speaking to it
#endif

  MaestroSerial.begin(MAESTRO_BAUDRATE); // init the serial communication to the board
  delay(5); // give a little time to the Maestro to breeze 
  
#ifdef MAESTRO_DETECT_BAUDRATE
  MaestroSerial.print(0xAA, BYTE);    //start byte to init baudrate
#endif
}



void maestro_send_header(byte cmd, byte device){
  // sends the header of the command, depending ont the protocol
  
  if ( (cmd & 0x80) == 0 ) {   // Pololu protocol
    MaestroSerial.print(0xAA, BYTE);    //start byte
    MaestroSerial.print(device, BYTE);  //device
  }
  MaestroSerial.print(cmd, BYTE);
}

void maestro_send_command(byte cmd, byte device){
  // sends a command without data
  
  MAESTRO_START_TRANSMISSION();
  maestro_send_header(cmd, device);
  MAESTRO_END_TRANSMISSION();
}

void maestro_send_command(byte cmd, byte val1, byte device){
  // sends a command with 1 byte of data

  MAESTRO_START_TRANSMISSION();
  maestro_send_header(cmd, device);
  MaestroSerial.print(val1, BYTE);
  MAESTRO_END_TRANSMISSION();
}

void maestro_send_command(byte cmd, byte val1, int val2, byte device){
  // sends a command with 3 byte of data

  MAESTRO_START_TRANSMISSION();
  maestro_send_header(cmd, device);
  MaestroSerial.print(val1, BYTE);
  MaestroSerial.print(val2 & 0x7f, BYTE); //data lower 7 bits
  MaestroSerial.print((val2 >> 7) & 0x7f, BYTE); //data bits 7-13
  MAESTRO_END_TRANSMISSION();
}


int maestro_receive_int(){
  int low, high;
  
  while(!MaestroSerial.available()); // wait until some data is present
  
  high = MaestroSerial.read();       // read the first byte
  while(MaestroSerial.available()){  // while the rx buffer is not empty
    low=high;                        // put the previous value read in "low" 
    high = MaestroSerial.read();     // put the last value in "high"
  }
  // at this point, "high" contains the last value received, and "low" the one
  // before that. Any garbage that could have been in the rx buffer has been
  // effectively thrown away.
  
  return low | (high << 8);
}

byte maestro_receive_byte(){
  byte data;
  
  while(!MaestroSerial.available()); // wait until some data is present
  
  while(MaestroSerial.available()){  // while the rx buffer is not empty
    data = MaestroSerial.read();     // put the last value in "data"
  }
  // at this point, "data" contains the last value received. Any garbage
  // that could have been in the rx buffer has been effectively thrown away.
  
  return data;
}


void maestro_hard_reset(){
#ifdef MAESTRO_USE_RST_PIN
  digitalWrite(servoControllerResetPin, LOW);
  delay(50);
  digitalWrite(servoControllerResetPin, HIGH);
#endif
}

//------------------------------------------------------------
//                     Servo commands
//------------------------------------------------------------
void maestro_set_target(byte servo, int angle){
  //set the target for a servo
  //servo is the servo number (typically 0-5)
  //angle is the target, from 256 to 13120, in quarter-microseconds
  
  maestro_send_command(0x84, servo, angle, 0);
}

void maestro_set_target(byte device, byte servo, int angle){
  //set the target for a servo on a particular device
  //device is the id of the Maestro device (default: 12)
  //servo is the servo number (typically 0-5)
  //angle is the target, from 256 to 13120, in quarter-microseconds
  
  maestro_send_command(0x04, servo, angle, device);
}

void maestro_set_target_minissc(byte servo, byte angle){
  // set the target using the Mini-SSC protocol
  //servo is the sum of the servo number (typically 0-5) and the servo offset of the device
  //angle is the target, from 0 to 254, mapped between neutral-range and neutral+range. 127 is neutral.

  //Send a Mini-SSC command
  MAESTRO_START_TRANSMISSION();
  MaestroSerial.print(0xFF, BYTE);    //start byte
  MaestroSerial.print(servo, BYTE);   //servo number + device offset
  MaestroSerial.print(angle, BYTE);   //8-bit target
  MAESTRO_END_TRANSMISSION();
}


void maestro_set_speed(byte servo, int speedVal){
  //set the speed for a servo
  //servo is the servo number (typically 0-5)
  //speedVal is servo speed (0=full, 1=slower), in (0.25 μs)/(10 ms)
  
  maestro_send_command(0x87, servo, speedVal, 0);
}

void maestro_set_speed(byte device, byte servo, int speedVal){
  //set the speed for a servo on a particular device
  //device is the id of the Maestro device (default: 12)
  //servo is the servo number (typically 0-5)
  //speedVal is servo speed (0=full, 1=slower), in (0.25 μs)/(10 ms)
  
  maestro_send_command(0x07, servo, speedVal, device);
}


void maestro_set_acceleration(byte servo, byte accel){
  //set the acceleration for a servo
  //servo is the servo number (typically 0-5)
  //accel is servo acceleration (0=full, 1=slower), in (0.25 μs)/(10 ms)/(80 ms)
  
  maestro_send_command(0x89, servo, accel, 0);
}

void maestro_set_acceleration(byte device, byte servo, byte accel){
  //set the acceleration for a servo on a particular device
  //device is the id of the Maestro device (default: 12)
  //servo is the servo number (typically 0-5)
  //accel is servo acceleration (0=full, 1=slower), in (0.25 μs)/(10 ms)/(80 ms)
  
  maestro_send_command(0x09, servo, accel, device);
}


void maestro_go_home(){
  //set all servos to their home position. Servos set to "ignore" will remain unchanged.
  
  maestro_send_command(0xA2, 0);
}

void maestro_go_home(byte device){
  //set all servos to their home position on a particular device. Servos set to "ignore" will remain unchanged.
  //device is the id of the Maestro device (default: 12)
  //servo is the servo number (typically 0-5)
  
  maestro_send_command(0x22, device);
}


void maestro_set_servo_off(byte servo){
  //stop a servo (and leave it limp) 
  //servo is the servo number (typically 0-5)
 
  // To set off a servo, set its target to 0.
  maestro_set_target(servo, 0);
}

void maestro_set_servo_off(byte device, byte servo){
  //stop a servo (and leave it limp) on a particular device
  //device is the id of the Maestro device (default: 12)
  //servo is the servo number (typically 0-5)

  // To set off a servo, set its target to 0.
  maestro_set_target(device, servo, 0);
}

void maestro_set_all_servos_off(){
  for(int i = 0; i<6; i++){
    maestro_set_servo_off(i);
  }
}

int maestro_get_position(byte servo){
  // gets the current position of a servo
  
  maestro_send_command(0x90, servo, 0);
  return maestro_receive_int();
}

int maestro_get_position(byte device, byte servo){
  // gets the current position of a servo on a particular device
  //device is the id of the Maestro device (default: 12)
  
  maestro_send_command(0x10, servo, device);
  return maestro_receive_int();
}

byte maestro_get_moving_state(){
  // checks if some servos are moving on a particular device
  
  maestro_send_command(0x93, 0);
  return maestro_receive_byte();
}

byte maestro_get_moving_state(byte device){
  // checks if some servos are moving on a particular device
  //device is the id of the Maestro device (default: 12)
  
  maestro_send_command(0x13, device);
  return maestro_receive_byte();
}

int maestro_get_errors(){
  // gets the current error flags
  //device is the id of the Maestro device (default: 12)
  
  maestro_send_command(0xA1, 0);
  return maestro_receive_int();
}

int maestro_get_errors(byte device){
  // gets the current error flags on a particular device
  //device is the id of the Maestro device (default: 12)
  
  maestro_send_command(0x21, device);
  return maestro_receive_int();
}


//------------------------------------------------------------
//                     Script commands
//------------------------------------------------------------

void maestro_stop_script(){
  //stop the script, if it is currently running

  maestro_send_command(0xA4, 0);
}

void maestro_stop_script(byte device){
  //stop the script, if it is currently running
  //device is the id of the Maestro device (default: 12)

  maestro_send_command(0x24, device);
}


void maestro_restart_script(byte subroutine){
  //start a script
  //subroutine is the point where the script will start.
  
  maestro_send_command(0xA7, subroutine, 0);
}


void maestro_restart_script(byte device, byte subroutine){
  //start a script on a particular device
  //device is the id of the Maestro device (default: 12)
  //subroutine is the point where the script will start.
  
  maestro_send_command(0x27, subroutine, device);
}

void maestro_restart_script_param(byte subroutine, int param){
  //start a script with a parameter
  //subroutine is the point where the script will start.
  //param is the argument (between 0 and 16383)
  
  maestro_send_command(0xA8, subroutine, param, 0);
}


void maestro_restart_script_param(byte device, byte subroutine, int param){
  //start a script with a parameter on a particular device
  //device is the id of the Maestro device (default: 12)
  //subroutine is the point where the script will start.
  //param is the argument (between 0 and 16383)
  
  maestro_send_command(0x28, subroutine, param, device);
}


byte maestro_get_script_running(){
  // checks if a script is running
  
  maestro_send_command(0xAE, 0);
  return maestro_receive_byte();
}

byte maestro_get_script_running(byte device){
  // checks if a script is running on a particular device
  //device is the id of the Maestro device (default: 12)
  
  maestro_send_command(0x2E, device);
  return maestro_receive_byte();
}

You can find the development version as well as an example of use here.

Enjoy!

EDIT: changed a comment about the hardware serial

Hello,
This looks great! I hope it will be useful to people. In the comments, you mention that hardware serial is useful on the mega - do any of the other Arduinos make it at all possible to use the hardware UART with the Maestro? If not, can you write a note about that so that so people are not disappointed when they try? I have noticed at least one person getting strange behavior when trying to use the UART on an Arduino where it was already connected to the USB-serial chip.

Thanks,
-Paul

It will work with the hardware serial on any arduino, but as you point out, connecting the serial pins to a Maestro and trying to use the usb at the same time is not gonna end well since the FTDI chip and Maestro would be fighting for the control of the lines… If you do not intend to connect both at the same time (aka you have to physically unplug the one you don’t use), there is no reason it won’t work.

I’ll change this comment as it can be misleading.

EDIT:
A note on performance: with the NewSoftSerial lib, I managed to speak to the Maestro up to 57600 bauds in “detect baud rate”, and up to 115200 in “fixed baud rate” without problems.

But you can’t physically unplug the FTDI chip from the Arduino board, can you? I would expect it to drive its TX line high even when USB is not connected, which means that it will actually be fighting with the Maestro, right?

-Paul

Ok, now I am in doubt. I have to get some more information on the subject, but I suspect the FTDI chip knows if the usb is plugged or not, and is smart enough not to drive the TX line high when it has no use for it.

I’ve had this problem with another device that has an FTDI USB-Serial adapter chip connected to a microcontroller’s hardware UART. You can listen to the device’s TX line in parallel to the FTDI chip with no problems, but it has been my experience that the FTDI drives its TX line (the device’s RX line) high whenever it is powered, regardless of whether it has a USB data connection or not.

-Adam

P.S. If you don’t mind hacking at your Arduino board the problem can be solved with two diodes and a resistor!

I did a few more tests :

  • with an Arduino Duemilanove, as far as I can tell, I have no problem exchanging information with the maestro (sending commands and receiving data) using the hardware serial port.
  • receiving data with the NewSoftSerial library has some problems at 115200. Better not go above 57600 bauds.

just tested with an arduino mega and one of its hardware serial TX (Serial3), the lib works great.
and the micro maestro isnt bad either :slight_smile:

Hey Guys,
I’m super interested in your pseudo library. What I’m trying to do is have several sequences saved on the maestro and call certain ones from an arduino, as well as other basic options like stopping a sequence or looping one. Is there a simple way to do this?
thanks,
Brian