1
\$\begingroup\$

This wrapper library parses text into commands for the Adafruit Motor Shield v2 using the Regexp library. Currently, only the DC motor functions setSpeed and setDirection are implemented, but adding more functions is trivial.

The general command syntax is MSv2_<shield number in hex>_<command name>_<parameters separated by _s>

For example:

To set the speed to 255/255 for motor 1 on shield 96, pass MSv2_60_speed_1_FF to the message parameter of checkMotorShieldMessage.

To set the direction to FORWARD for motor 1 on shield 96, pass MSv2_60_direction_1_1.

A pointer to the toWrite String is passed so the function can dereference and alter it. In this way, toWrite can pass an error or success message back to where it was called.

motor_shield.ino

/* 
This is a test sketch for the Adafruit assembled Motor Shield for Arduino v2
It won't work with v1.x motor shields! Only for the v2's with built in PWM
control

For use with the Adafruit Motor Shield v2 
---->   http://www.adafruit.com/products/1438
*/

#include <Wire.h>
#include "MotorShieldv2Lib.h"

String toWrite;

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");
  toWrite = "";
}

void loop() {
   String usb = Serial.readString();
   if(checkMotorShieldMessage(usb, &toWrite)){
     //https://stackoverflow.com/questions/2229498/passing-by-reference-in-c
     //make sure this changes 
     Serial.print(toWrite);//passing the pointer
   }
}

MotorShieldv2Lib.h

#ifndef MotorShieldv2lib
#define MotorShieldv2lib

#if (ARDUINO >=100)
  #include <arduino.h>
#else
  #include "WProgram.h"
#endif

//#include <Wire.h> 
// the serial library?
// hardware vs software serial https://forum.arduino.cc/index.php?topic=407633.0
// maybe you don't need serial?

#include <Adafruit_MotorShield.h>
#include "MotorShieldv2Lib.h"

using namespace std;

/*
 * Take a message and a Stream (Serial object)
 *   - the message was received from the stream
 *   - The stream will send a message back if it has to (error code...)
 */
boolean checkMotorShieldMessage(String message, String *toWrite);

#endif

MotorShieldv2Lib.cpp

#include <Regexp.h>
//from here: https://github.com/nickgammon/Regexp installed in computer's Arduino library, so you'll have to do this

// https://www.youtube.com/watch?v=fE3Dw0slhIc
//#include <Wire.h> 
// the serial library?
#include <Adafruit_MotorShield.h>
#include "MotorShieldv2Lib.h"



static const char SHIELD_PATTERN_START [] = "^MSv2_[67][0-9A-Fa-f]_";//len == 8
static const char SPEED_PATTERN [] = "^MSv2_[67][0-9A-Fa-f]_speed_[1-4]_[0-9a-fA-F]{2,2}$";
//make sure you send hex bytes!
static const char DIR_PATTERN [] = "^MSv2_[67][0-9A-Fa-f]_direction_[1-4]_[0-2]$";

static Adafruit_MotorShield *shields [32] = {};
// Initialized as all null
//https://stackoverflow.com/questions/2615071/c-how-do-you-set-an-array-of-pointers-to-null-in-an-initialiser-list-like-way
  // the above link described this initialization
  // shields holds pointer to the shield objects.
  // shields are addressed 0x60 to 0x7F for a total of 32 unique addresses.
  // In this array, [0] == address 0x60, [31] == address 0x7F
  // note, static in this context means the array's pointer //can't change, the array values can


/*
 * Converts the message from the Serial port to its corresponding motor
 * 
 */
boolean getMotorShield(String message, Adafruit_MotorShield *shield){
// * https://stackoverflow.com/questions/45632093/convert-char-to-uint8-t-array-with-a-specific-format-in-c
// the above might help with the conversion
//https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/stacking-shields
//Note: 0x70 is the broadcast

//pointers: https://stackoverflow.com/questions/28778625/whats-the-difference-between-and-in-c
   String shieldAddress = message.substring(5,7);//make sure this is the right length
   char carr [2];
   shieldAddress.toCharArray(carr, 2);
   uint8_t addr = strtol(carr, NULL, 16);
   if(addr<96 || addr > 127){
     Serial.print(addr);
     Serial.print(shieldAddress +"\n");//not sure if this will work
     return false;
   }
   if(!shields[addr - 96]){//checks for null pointer
      //Adafruit_MotorShield *AMS = malloc(sizeof(Adafruit_MotorShield));
      //AMS->add
      Adafruit_MotorShield AMS = Adafruit_MotorShield(addr);
      shields[addr - 96] = &AMS;
   }
   *shield = *shields[addr - 96]; 
   return true;
};

/*
 * gets the motor, then sets the speed
 */
boolean setMotorSpeed(String message, Adafruit_MotorShield shield){
   String motorID = message.substring(15,16);//make sure this is the right length
   char carr [1];
   motorID.toCharArray(carr, 1);
   uint8_t motorAddr = strtol(carr, NULL, 16);

   String speedIn = message.substring(16,18);//make sure this is the right length
   char speedCarr [2];
   speedIn.toCharArray(speedCarr, 2);
   uint8_t intSpeed = strtol(speedCarr, NULL, 16);

   shield.getMotor(motorAddr)->setSpeed(intSpeed);
   return true;
}

/*
 * gets the motor, then sets the direction
 * 
 * see here: https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/library-reference#void-run-uint8-t-9-7
 */
boolean setMotorDir(String message, Adafruit_MotorShield shield){
   String motorID = message.substring(19,20);//make sure this is the right length
   char carr [1];
   motorID.toCharArray(carr, 1);
   uint8_t motorAddr = strtol(carr, NULL, 16);

   String dirIn = message.substring(21,23);//make sure this is the right length
   char dirCarr [2];
   dirIn.toCharArray(dirCarr, 2);
   uint8_t intDir = strtol(dirCarr, NULL, 16);

   shield.getMotor(motorAddr)->run(intDir);
   return true;
}

/*
 * motor shield signals are of the format "MSv2_shield number_then the command_parameters"
 * see the constants at the top for the commands
 * 
 * if the message is meant for a motor shield:
 *   - If the shield doesn't exist, create it and add it to shields
 *     - If there's not a shield connected with the corresponding address, throw an error
 *   - Run the function on the right shield
 *   - return true
 * else: 
 *   - return false
 *   
 *   Remember, you NEED to de-reference toWrite with this: https://stackoverflow.com/questions/2229498/passing-by-reference-in-c

*/
boolean checkMotorShieldMessage(String message, String *toWrite){
  MatchState ms;
  char buf [message.length()];
  message.toCharArray(buf, message.length());
  ms.Target(buf);
  char isForShield = ms.Match(SHIELD_PATTERN_START);//check if the message is for the shield
  // converting to char array: https://www.arduino.cc/reference/en/language/variables/data-types/string/functions/tochararray/
  // regex from: https://github.com/nickgammon/Regexp also see the installed examples
  if(isForShield > 0){
    //parse out which shield, set it as a variable
    Adafruit_MotorShield as = Adafruit_MotorShield();//can't be named asm?
    if(!getMotorShield(message, &as)){
       //set toWrite to an error message saying this isn't a //valid number
       *toWrite = String("MotorShield: That isn't a valid shield address." + message);
       return true;
    }

    if(ms.Match(SPEED_PATTERN) > 0){
      //parse out params
      //set speed on the shield
      setMotorSpeed(message, as);
      return true;
    }else if(ms.Match(DIR_PATTERN) > 0){
      //set direction
      setMotorDir(message, as);
      return true;
    //ADD OTHER STUFF (SET SERVOS...)
      // note, people can put crap between the //SHIELD_PATTERN_START and the parameter patterns, but this isn't //really a problem
    }else{
      *toWrite = String("MotorShield: No matching command found.");
      return false;
    }
  }else{
    return false;
  }
}
\$\endgroup\$
2
\$\begingroup\$

Avoid regular expressions

Some people, when confronted with a problem, think “I know, I'll use regular expressions.” Now they have two problems. -- Jamie Zawinsky

Your problem is simple enough that you don't need any regular expressions to solve it. The input format is very simple: it's a single command with underscores separating the various parameters. The right tool for this job is to simply split the input using '_' as the separator. Since you are using the String class to hold strings, use indexOf() to find the position of the next separator. For example, to loop over all tokens:

String input = ...;
int start = 0;
int pos;

while ((pos = input.indexOf('_', start)) != -1) {
    String token = input.substring(start, pos);
    // do something with this token
    start = pos + 1;
}

Instead of a loop though, you probably want to write a class that will handle finding the next token for you, such that you can write something like:

String input = ...
Tokenizer tokenizer(input);

String type = tokenizer.get_next_token();
if (type != "MSv2")
    return false;

String shield = tokenizer.get_next_token();
...

String command = tokenizer.get_next_token();

if (command == "speed")
    // handle speed command
else if (command == "direction")
    // handle direction command
else ...

This code avoids the overhead of using the regular expression library, and avoids having to Match() multiple regular expressions against the same input string. Your code will be faster and consume less flash memory and RAM.

Avoid casting Strings to char arrays and back

Don't use String if you are going to use functions like strtol() a lot. It is better to use Serial.readBytes() and read directly into a char array than to first read it into a String and then convert it. Or if you really want to use String, use the c_str() member function to get access to the underlying char array, like so: strtol(string.c_str(), NULL, 16).

Don't forget to account for the NUL terminator

C strings must always be terminated by a NUL byte. When you convert a String to a char array, you must ensure the array has room for the NUL byte. So if you have a String of 2 characters, the char array must be at least 3 characters long. But, as mentioned above, just use c_str() and you will avoid this issue.

Avoid global variables

You can move the declaration of toWrite into loop(). Keep variable declarations close to where they are actually used.

Check that parsing of numbers actually succeeded.

You call strtol() but you don't check if the value was correctly parsed. What happens if the wrong command is sent to the Arduino, or if there is a glitch on the serial bus and the message is garbled? What damage might be done if strtol() returns 0? If safety is important, also consider adding a checksum to the message.

\$\endgroup\$
1
  • \$\begingroup\$ In the last paragraph, I used regex to check the input. \$\endgroup\$ Oct 27 '18 at 20:30

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.