Extend EZB Protocol Custom Command
The EZB protocol contains built-in commands for setting servo positions, reading adc/digital data, etc. However, you can add custom commands by extending the protocol. This is done using the EZB—SendCommand () function in your C# robot skill. The function will send some data and optionally return data in an array. Your firmware needs to wait, listen for the command, and return the specified number of bytes. In this example, the EZB firmware will wait for a custom command and return some example data.
*Note: This ability is more detailed in the EZB Protocol definition section HERE.
1. EZB Arduino Firmware
First, we must program the Arduino with firmware with a custom command and return some data.
#include
#include "SendOnlySoftwareSerial.h"
// The first digital port that is usable on this controller
#define _PortStart 4
// The last digital port on this controller that is usable
#define _PortCnt 14
// The number of analog ports
#define _AnalogPorts 6
// The firmware version that is reported to EZ-Builder to notify of capabilities
#define _FIRMWARE_ID 0x0000000B
// The communication baud rate
#define _BAUD_RATE 57600
// The primary communication interface between EZ-Builder and this controller
#define COMMUNICATION_PORT Serial
// The amount of RX buffer on the communication interface for EZ-Builder
#define _BUFFER_SIZE 1024
// --------------------------------------------------------------------------
byte _INPUT_BUFFER[_BUFFER_SIZE];
unsigned int _WRITE_POSITION = 0;
unsigned int _READ_POSITION = 0;
int _BAUD_RATES [] = {
4800,
9600,
19200,
38400,
57600,
115200,
115200
};
Servo Servos[_PortCnt];
#define CmdOurCustomCmds 0
#define CmdReleaseAllServos 1
#define CmdGetUniqueID 2
#define CmdEZBv3 3
#define CmdEZBv4 4
#define CmdSoundBeep 5
#define CmdEZServo 6
#define CmdI2CWrite 10
#define CmdI2CRead 11
#define CmdBootLoader 14
#define CmdSetPWMSpeed 15
#define CmdSetServoSpeed 39
#define CmdPing 0x55
#define CmdSetDigitalPortOn 100
#define CmdSetDigitalPortOff 124
#define CmdGetDigitalPort 148
#define CmdSetServoPosition 172
#define CmdGetADCValue 196
#define CmdSendSerial 204
#define CmdHC_SR04 228
#define CmdGetFirwareID 253
#define CmdSoundStreamCmd 254
// CmdEZBv4 Commands
// ----------------------------------------------------------------------------------
#define CmdV4SetLipoBatteryProtectionState 0
#define CmdV4SetBatteryMonitorVoltage 1
#define CmdV4GetBatteryVoltage 2
#define CmdV4GetCPUTemp 3
#define CmdV4UARTExpansion0Init 5
#define CmdV4UARTExpansion0Write 6
#define CmdV4UARTExpansion0AvailableBytes 7
#define CmdV4UARTExpansion0Read 8
#define CmdV4UARTExpansion1Init 9
#define CmdV4UARTExpansion1Write 10
#define CmdV4UARTExpansion1AvailableBytes 11
#define CmdV4UARTExpansion1Read 12
#define CmdV4UARTExpansion2Init 13
#define CmdV4UARTExpansion2Write 14
#define CmdV4UARTExpansion2AvailableBytes 15
#define CmdV4UARTExpansion2Read 16
#define CmdV4I2CClockSpeed 17
#define CmdV4UARTClockSpeed 18
#define CmdV4ResetToDefaults 19
// CmdSoundStreamCmd Commands
// ----------------------------------------------------------------------------------
#define CmdSoundInitStop 0
#define CmdSoundLoad 1
#define CmdSoundPlay 2
bool IsAvail() {
return _WRITE_POSITION != _READ_POSITION || COMMUNICATION_PORT.available();
}
byte ReadByte() {
while (_WRITE_POSITION == _READ_POSITION && COMMUNICATION_PORT.available() == 0);
while (COMMUNICATION_PORT.available()) {
_WRITE_POSITION++;
_INPUT_BUFFER[_WRITE_POSITION % _BUFFER_SIZE] = COMMUNICATION_PORT.read();
}
_READ_POSITION++;
return _INPUT_BUFFER[_READ_POSITION % _BUFFER_SIZE];
}
void setup() {
COMMUNICATION_PORT.begin(_BAUD_RATE);
}
void loop() {
doEZProtocol();
}
void Write32(long val) {
COMMUNICATION_PORT.write((byte)(val & 0xff));
COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
COMMUNICATION_PORT.write((byte)((val >> 16) & 0xff));
COMMUNICATION_PORT.write((byte)((val >> 24) & 0xff));
}
void Write16(int val) {
COMMUNICATION_PORT.write((byte)(val & 0xff));
COMMUNICATION_PORT.write((byte)((val >> 8) & 0xff));
}
#define UNKNOWN_PIN 0xFF
uint8_t getPinMode(uint8_t pin) {
uint8_t bit = digitalPinToBitMask(pin);
uint8_t port = digitalPinToPort(pin);
// Megan't see an option for mega to return this, but whatever...
if (NOT_A_PIN == port)
return UNKNOWN_PIN;
// Is there a bit we can check?
if (0 == bit)
return UNKNsingle-bit // Is there only a single bit set?
if (bit & bit - 1)
return UNKNOWN_PIN;
volatile uint8_t *reg, *out;
reg = portModeRegister(port);
out = portOutputRegister(port);
if (*reg & bit)
return OUTPUT;
else if (*out & bit)
return INPUT_PULLUP;
else
return INPUT;
}
void doEZProtocol() {
if (IsAvail()) {
byte cmd = ReadByte();
if (cmd == CmdPing) {
// return as a "Capability Controller"
COMMUNICATION_PORT.write(222);
} else if (cmd == CmdGetFirwareID) {
Write32(_FIRMWARE_ID);
} else if (cmd == CmdReleaseAllServos) {
for (int port = _PortStart; port < _PortCnt; port++)
if (Servos[port].attached())
Servos[port].detach();
} else if (cmd >= CmdSetServoPosition && cmd <= CmdSetServoPosition + 23) {
byte port = cmd - CmdSetServoPosition;
byte pos = ReadByte();
if (port >= _PortStart && port <= _PortCnt) {
if (pos == 0 && Servos[port].attached()) {
Servos[port].detach();
} else {
if (!Servos[port].attached())
Servos[port].attach(port);
Servos[port].write(pos);
}
}
} else if (cmd >= CmdSetPWMSpeed && cmd <= CmdSetPWMSpeed + 23) {
byte port = cmd - CmdSetPWMSpeed;
byte pos = ReadByte();
if (port >= _PortStart && port <= _PortCnt) {
if (Servos[port].attached())
Servos[port].detach();
if (getPinMode(port) != OUTPUT)
pinMode(port, OUTPUT);
analogWrite(port, map(pos, 0, 100, 0, 255));
}
} else if (cmd >= CmdSetDigitalPortOn && cmd <= CmdSetDigitalPortOn + 23) {
byte port = cmd - CmdSetDigitalPortOn;
if (port >= _PortStart && port <= _PortCnt) {
if (Servos[port].attached())
Servos[port].detach();
if (getPinMode(port) != OUTPUT)
pinMode(port, OUTPUT);
digitalWrite(port, HIGH);
}
} else if (cmd >= CmdSetDigitalPortOff && cmd <= CmdSetDigitalPortOff + 23) {
byte port = cmd - CmdSetDigitalPortOff;
if (port >= _PortStart && port <= _PortCnt) {
if (Servos[port].attached())
Servos[port].detach();
if (getPinMode(port) != OUTPUT)
pinMode(port, OUTPUT);
digitalWrite(port, LOW);
}
} else if (cmd >= CmdGetDigitalPort && cmd <= CmdGetDigitalPort + 23) {
byte port = cmd - CmdGetDigitalPort;
if (port >= _PortStart && port <= _PortCnt) {
if (Servos[port].attached())
Servos[port].detach();
if (getPinMode(port) != INPUT)
pinMode(port, INPUT);
COMMUNICATION_PORT.write(digitalRead(port));
} else {
COMMUNICATION_PORT.write(0);
}
} else if (cmd >= CmdGetADCValue && cmd <= CmdGetADCValue + 7) {
byte port = cmd - CmdGetADCValue;
if (port >= 0 && port <= _AnalogPorts)
Write16(analogRead(port));
else
Write16(0);
} else if (cmd >= CmdSendSerial && cmd <= CmdSendSerial + 23) {
// Send Serial
uint8_t port = cmd - CmdSendSerial;
uint8_t baud = ReadByte(); // Baud rate
uint8_t size = ReadByte(); // Size
if (port >= _PortStart && port <= _PortCnt) {
if (Servos[port].attached())
Servos[port].detach();
SendOnlySoftwareSerial tmpSerial(port);
tmpSerial.begin(_BAUD_RATES[baud]);
for (int x = 0; x < size; x++)
tmpSerial.write(ReadByte());
tmpSerial.end();
}
} else if (cmd == CmdOurCustomCmds) {
byte cmd2 = ReadByte();
if (cmd2 == 0x00) {
// Our first custom command sends two 16 bit ints (4 bytes total)
Write16(1234);
Write16(4321);
} else if (cmd2 == 0x01) {
// Our second custom command sends two different 16 bit ints (4 bytes total)
Write16(5678);
Write16(8765);
}
}
}
}
2. C# Robot Skill
In the C# robot skill, we need to send the command and receive the data in an array. Here, we provide examples for both custom commands we created in the Arduino EZB firmware example above.
*Note: In this example, the functions use the EZBManager.PrimaryEZB is the 0 (first) index of the EZB in the connection control. Alternatively, you can use the EZBManager.EZBs[0].SendCommandData() to specify the EZB index manually or programmatically. You can have an option in the configuration menu of the robot skill to specify which EZB connection index is hosting your custom command firmware.
void readValues1() {
try {
if (!EZBManager.PrimaryEZB.IsConnected)
return;
// We'll get the data from the first custom command (should return four bytes, which will be 1234 and 4321)
var response = EZBManager.PrimaryEZB.SendCommandData(4, 0x00, 0x00);
// Decode the data returned
UInt16 val1 = BitConverter.ToUInt16(response, 0);
UInt16 val2 = BitConverter.ToUInt16(response, 2);
// Set the data into global ARC variables so all robot skills have access to it
ARC.Scripting.VariableManager.SetVariable("$val1", val1);
ARC.Scripting.VariableManager.SetVariable("$val2", val2);
// Display the values to the textbox on this robot skill
Invokers.SetAppendText(textBox1, true, "val1: {0}, val2: {1}", val1, val2);
} catch (Exception ex) {
Invokers.SetAppendText(textBox1, true, ex.ToString());
}
}
void readValues2() {
try {
if (!EZBManager.PrimaryEZB.IsConnected)
return;
// We'll get the data from the second custom command (should return four bytes, which will be 5678 and 8765)
var response = EZBManager.PrimaryEZB.SendCommandData(4, 0x00, 0x01);
// Decode the data returned
UInt16 val1 = BitConverter.ToUInt16(response, 0);
UInt16 val2 = BitConverter.ToUInt16(response, 2);
// Set the data into global ARC variables so all robot skills have access to it
ARC.Scripting.VariableManager.SetVariable("$val3", val1);
ARC.Scripting.VariableManager.SetVariable("$val4", val2);
// Display the values to the textbox on this robot skill
Invokers.SetAppendText(textBox1, true, "val3: {0}, val4: {1}", val1, val2);
} catch (Exception ex) {
Invokers.SetAppendText(textBox1, true, ex.ToString());
}
}