public void Evaluate(int SpreadMax)
{
if (In.IsChanged)
{
Values.SliceCount = PortNumber.SliceCount * 8; // 8 bits per port
Port.SliceCount = PortNumber.SliceCount;
foreach (string msg in In)
{
byte[] data = Encoding.GetEncoding(1252).GetBytes(msg);
if (!FirmataUtils.ContainsCommand(data, Command.DIGITALMESSAGE))
{
continue; //
}
int port;
int[] values;
if (FirmataUtils.DecodePortMessage(data, out port, out values))
{
Port[0] = port;
Values.AssignFrom(values);
CachedValues = Values.Clone();
}
else
{
Values = CachedValues.Clone();
}
}
}
}
public void Evaluate(int maxSpread)
{
if (Data.IsChanged)
{
if (Data.SliceCount < 2)
{
return;
}
int _address = FirmataUtils.GetValueFromBytes(Data[1], Data[0]);
if (_address != Address[0] && Data.SliceCount < 4)
{
return;
}
int[] empty = {};
ParsedRegister.AssignFrom(empty);
ParsedData.AssignFrom(empty);
for (int i = 2; i < Data.SliceCount - 4; i += 4)
{
ParsedRegister.Add(FirmataUtils.GetValueFromBytes(Data[i + 1], Data[i]));
ParsedData.Add(FirmataUtils.GetValueFromBytes(Data[i + 3], Data[i + 2]));
}
}
}
void GetSamplerateCommand(int rate)
{
byte lsb, msb;
FirmataUtils.GetBytesFromValue(rate, out msb, out lsb);
CommandBuffer.Enqueue(Command.SYSEX_START);
CommandBuffer.Enqueue(Command.SAMPLING_INTERVAL);
CommandBuffer.Enqueue(lsb);
CommandBuffer.Enqueue(msb);
CommandBuffer.Enqueue(Command.SYSEX_END);
}
///
/// TODO: Make it spreadable with Address spread as size
///
public void Evaluate(int maxSpread)
{
if (Data.IsChanged)
{
FDebug.SliceCount = 0;
if (Data.SliceCount <= 0 || Address.SliceCount < 1)
{
clearOutputs();
return;
}
int NumBytes = (int)Data[0].Length - 4; // substract Address & Register
if (NumBytes <= 0)
{
FDebug.Add(NumBytes > 0 ? "No data bytes to process" : "Not enough bytes at all");
clearOutputs();
return;
}
byte LSB = (byte)Data[0].ReadByte();
byte MSB = (byte)Data[0].ReadByte();
int _Address = FirmataUtils.GetValueFromBytes(MSB, LSB);
if (_Address != Address[0])
{
clearOutputs();
FDebug.Add("Address does not match");
return;
}
LSB = (byte)Data[0].ReadByte();
MSB = (byte)Data[0].ReadByte();
ParsedRegister.SliceCount = 1;
int register = FirmataUtils.GetValueFromBytes(MSB, LSB);
ParsedRegister[0] = register == 255 ? -1 : register; // Handle REGISTER_NOT_SPECIFIED
try {
ParsedData.SliceCount = 1;
Stream data = new MemoryStream(NumBytes / 2); // we deal with 7-bit encoding!
for (int i = 0; i < NumBytes; i += 2)
{
LSB = (byte)Data[0].ReadByte();
MSB = (byte)Data[0].ReadByte();
data.WriteByte((byte)FirmataUtils.GetValueFromBytes(MSB, LSB));
}
ParsedData[0] = data;
}
catch (Exception e) {
FDebug.Add("Could not read data: " + e.ToString());
}
}
}
void SetPinStates(ISpread <double> values)
{
// get the number of output ports
// FIXME: Make MAX_PORTS avaiable through Firmata
int[] digital_out = new int[OUTPUT_PORT_MASKS.Length];
for (int i = 0; i < values.SliceCount; i++)
{
double value = values[i];
PinMode mode = PinModeForPin(i);
switch (mode)
{
case PinMode.ANALOG:
case PinMode.PWM:
case PinMode.SERVO:
byte LSB, MSB;
value *= mode == PinMode.SERVO ? 180 : 255; // servo is in degrees
FirmataUtils.GetBytesFromValue((int)value, out MSB, out LSB);
CommandBuffer.Enqueue((byte)(Command.ANALOGMESSAGE | i));
CommandBuffer.Enqueue(LSB);
CommandBuffer.Enqueue(MSB);
break;
case PinMode.OUTPUT:
case PinMode.INPUT: // fixes PullUp enabling issue, thx to motzi!
int port_index = PortIndexForPin(i);
// Break, if we have no ouputports we can get
if (port_index >= digital_out.Length)
{
break;
}
int shift = i % 8;
int state = value >= 0.5 ? 0x01 : 0x00;
int port_before = digital_out[port_index];
digital_out[port_index] = ((state << shift) | digital_out[port_index]) & OUTPUT_PORT_MASKS[port_index];
break;
}
}
/// Write all the output ports to the command buffer
for (int port_index = 0; port_index < digital_out.Length; port_index++)
{
byte LSB, MSB;
byte atmega_port = (byte)port_index; //Array.GetValues(Port)[port_index];
FirmataUtils.GetBytesFromValue(digital_out[port_index], out MSB, out LSB);
CommandBuffer.Enqueue((byte)(Command.DIGITALMESSAGE | atmega_port));
CommandBuffer.Enqueue(LSB);
CommandBuffer.Enqueue(MSB);
}
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
analogIns.SliceCount = analogInputCount[0];
digitalIns.SliceCount = digitalInputCount[0];
/// Using a Queue and iterate over it (nice to handle and inexpensive)
foreach (byte b in Encoding.GetEncoding(1252).GetBytes(ansiMessage[0]))
{
// we should check for max buffer size and not constantly enque things...
buffer.Enqueue(b);
}
// A cache for sysex data
Queue <byte> cache = new Queue <byte>();
// A flag if parsing sysex data
bool bIsSysex = false;
// PARSE:
while (buffer.Count > 0)
{
byte current = buffer.Dequeue();
switch (current)
{
case Command.SYSEX_START:
case Command.SYSEX_END:
// byte cmd = buffer.Dequeue();
if (current == Command.SYSEX_START)
{
bIsSysex = true;
}
else if (current == Command.SYSEX_END)
{
// Process the Sysexdata:
ProcessSysex(cache);
bIsSysex = false;
}
cache.Clear();
break;
default:
if (bIsSysex)
{
cache.Enqueue(current);
}
else
{
// Treat Ananlog & Digital Messages:
bool hasDigitalMessage = FirmataUtils.VerifiyCommand(current, Command.DIGITALMESSAGE);
bool hasAnalogMessage = FirmataUtils.VerifiyCommand(current, Command.ANALOGMESSAGE);
// We have a data for commands
if (buffer.Count >= 2 && (hasDigitalMessage || hasAnalogMessage))
{
// Reihenfolge matters!
byte[] data = { current, buffer.Dequeue(), buffer.Dequeue() };
// Check for Analog Command
if (hasAnalogMessage)
{
int pinNum, value;
FirmataUtils.DecodeAnalogMessage(data, out pinNum, out value);
if (pinNum < analogInputCount[0])
{
analogIns[pinNum] = value; // assign the found value to the spread
}
}
else if (hasDigitalMessage)
{
int port; int[] vals;
// Decode the values from the bytes:
FirmataUtils.DecodePortMessage(data, out port, out vals);
// Fill the spread with parsed pinstates
for (int i = 0; i < Constants.BitsPerPort; i++)
{
int pinNum = i + Constants.BitsPerPort * port;
if (pinNum < digitalIns.SliceCount)
{
digitalIns[pinNum] = vals[i];
}
}
}
}
}
break;
}
}
}
public void Evaluate(int SpreadMax)
{
SpreadMax = SpreadUtils.SpreadMax(Address, ReadWriteMode);
if (DoSend.IndexOf(true) < 0 || SpreadMax == 0)
{
DataOut[0] = Stream.Null;
return; // return when nothing to send
}
Out.Position = 0;
Out.SetLength(0);
byte lsb, msb;
// I2C config up front
Out.WriteByte(Command.SYSEX_START);
Out.WriteByte(Command.I2C_CONFIG);
FirmataUtils.GetBytesFromValue(I2CDelay[0], out msb, out lsb);
Out.WriteByte(lsb); Out.WriteByte(msb);
Out.WriteByte(Command.SYSEX_END);
for (int i = 0; i < SpreadMax; i++)
{
if ((ReadWriteMode[i] != I2CMode.WRITE && DoSend[i] == false) ||
(ReadWriteMode[i] == I2CMode.WRITE && DataIn[i].Length == 0)
)
{
continue;
}
// handle read continuously readings, keep track of requests
if (ReadWriteMode[i] == I2CMode.READ_CONTINUOUSLY)
{
if (ContinuouslyReadings.Contains(Address[i]))
{
continue;
}
else
{
ContinuouslyReadings.Add(Address[i]);
}
}
else if (ReadWriteMode[i] == I2CMode.STOP_READING && ContinuouslyReadings.Contains(Address[i]))
{
ContinuouslyReadings.Remove(Address[i]);
}
// Write the request header
Out.WriteByte(Firmata.Command.SYSEX_START);
Out.WriteByte(Firmata.Command.I2C_REQUEST);
// Write therequest address and mode
Out.WriteByte((byte)(Address[i] & 0x7F)); // LSB
byte mode = (byte)ReadWriteMode[i];
// Handle 10-bit mode
if (AddressMode[i].Index > 0)
{
mode |= 0x20; // enable
mode |= (byte)((Address[i] >> 8) & 0x03); // add MSB of address
}
Out.WriteByte(mode);
switch (ReadWriteMode[i])
{
case I2CMode.WRITE:
if (Register[i] > 0)
{
FirmataUtils.GetBytesFromValue(Math.Max(Register[i], 0), out msb, out lsb);
Out.WriteByte(lsb); Out.WriteByte(msb);
}
while (DataIn[i].Position < DataIn[i].Length)
{
FirmataUtils.GetBytesFromValue((int)DataIn[i].ReadByte(), out msb, out lsb);
Out.WriteByte(lsb); Out.WriteByte(msb);
}
break;
case I2CMode.READ_ONCE:
case I2CMode.READ_CONTINUOUSLY:
if (Register[i] > 0 || ReadWriteMode[i] == I2CMode.READ_CONTINUOUSLY)
{
FirmataUtils.GetBytesFromValue(Math.Max(Register[i], 0), out msb, out lsb);
Out.WriteByte(lsb); Out.WriteByte(msb);
}
FirmataUtils.GetBytesFromValue(BytesToRead[i], out msb, out lsb);
Out.WriteByte(lsb); Out.WriteByte(msb);
break;
}
// Close SysEx message
Out.WriteByte(Firmata.Command.SYSEX_END);
}
DataOut[0] = Out;
}
void SetPinStates(ISpread <double> values)
{
// get the number of output ports
// FIXME: Get only those ports, whos values have changed
int[] digital_out = new int[NUM_PORTS];
Queue <byte> AnalogCommandBuffer = new Queue <byte>();
int analogOutCount = 0;
int pinCount = Math.Min(Default.MaxDigitalPins, Math.Min(NUM_PINS, values.SliceCount));
for (int pin = 0; pin < pinCount; pin++)
{
double value = values[pin];
PinMode mode = PinModeForPin(pin);
switch (mode)
{
case PinMode.PWM:
case PinMode.SERVO:
byte LSB, MSB;
value *= mode == PinMode.SERVO ? 180 : 255; // servo is in degrees
FirmataUtils.GetBytesFromValue((int)value, out MSB, out LSB);
if (pin <= 0x0F)
{
AnalogCommandBuffer.Enqueue((byte)(Command.ANALOGMESSAGE | pin));
AnalogCommandBuffer.Enqueue(LSB);
AnalogCommandBuffer.Enqueue(MSB);
}
else
{
AnalogCommandBuffer.Enqueue(Command.SYSEX_START);
AnalogCommandBuffer.Enqueue(Command.EXTENDED_ANALOG);
AnalogCommandBuffer.Enqueue((byte)(pin & 0x7F)); // mask 7 Bit
AnalogCommandBuffer.Enqueue(LSB);
AnalogCommandBuffer.Enqueue(MSB);
AnalogCommandBuffer.Enqueue(Command.SYSEX_END);
}
break;
case PinMode.OUTPUT:
case PinMode.INPUT: // fixes PullUp enabling issue, thx to motzi!
int port = PortIndexForPin(pin);
// Break, if we have no outputports we can get
if (port < NUM_PORTS)
{
int state = (value >= 0.5 ? 0x01 : 0x00) << pin % 8;
state |= digital_out[port];
state &= OUTPUT_PORT_MASKS[port];
digital_out[port] = (byte)state;
}
break;
}
}
/// Write all the output ports to the command buffer
for (int port = 0; port < digital_out.Length; port++)
{
byte LSB, MSB;
FirmataUtils.GetBytesFromValue(digital_out[port], out MSB, out LSB);
CommandBuffer.Enqueue((byte)(Command.DIGITALMESSAGE | port));
CommandBuffer.Enqueue(LSB);
CommandBuffer.Enqueue(MSB);
}
/// Append the Commands for the analog messages
if (AnalogCommandBuffer.Count > 0)
{
foreach (byte b in AnalogCommandBuffer)
{
CommandBuffer.Enqueue(b);
}
}
}
void ProcessSysex(Queue <byte> data)
{
if (data.Count == 0)
{
return;
}
switch (data.Dequeue())
{
/// Handle Firmwareversion replies:
case Command.REPORT_FIRMWARE:
if (data.Count < 2)
{
break;
}
int major = data.Dequeue();
int minor = data.Dequeue();
// Read the name, of the Version
string name = FirmataUtils.GetStringFromBytes(data);
string the_name = major + "." + minor;
if (name.Length > 0)
{
the_name += " " + name.ToString();
}
FFirmwareMajorVersion[0] = major;
FFirmwareMinorVersion[0] = minor;
FFirmwareName[0] = name.ToString();
FFirmwareVersion[0] = the_name;
break;
/// Handle String Data
case Command.STRING_DATA:
string message = FirmataUtils.GetStringFromBytes(data);
StringOut.Add(message);
StringOut.Flush(true); // Signal change, regardless of the message
break;
/// Handle I2C replies
case Command.I2C_REPLY:
try {
MemoryStream i2cStream = new MemoryStream(data.ToArray());
FI2CData.Length = 1;
using (var i2cWriter = FI2CData.GetWriter()) {
i2cWriter.Write(i2cStream);
}
} catch (Exception e) {
FI2CData.Length = 0;
}
data.Clear();
break;
case Command.CAPABILITY_RESPONSE:
string report = "";
int pinCount = 0;
int digitalPins = 0;
int analogPins = 0;
int servoPins = 0;
int pwmPins = 0;
int shiftPins = 0;
int i2cPins = 0;
byte[] buffer = data.ToArray();
for (int a = 0; a < buffer.Length; a++)
{
pinCount++;
report += "Pin " + pinCount.ToString() + ":";
if (buffer[a] == 0x7f)
{
report += "\tNot available!";
}
while (buffer[a] != 0x7f)
{
PinMode mode = (PinMode)buffer[a++];
switch (mode)
{
case PinMode.ANALOG:
analogPins++;
break;
case PinMode.INPUT:
case PinMode.OUTPUT:
digitalPins++;
break;
case PinMode.SERVO:
servoPins++;
break;
case PinMode.PWM:
pwmPins++;
break;
case PinMode.I2C:
i2cPins++;
break;
case PinMode.SHIFT:
shiftPins++;
break;
}
int resolution = buffer[a++];
report += "\t" + FirmataUtils.PinModeToString(mode);
report += " (" + resolution.ToString() + " bit) ";
}
report += "\r\n";
}
digitalPins /= 2;
report += "Total number of pins: " + pinCount.ToString() + "\r\n";
report += string.Format("{0} digital, {1} analog, {2} servo, {3} pwm and {4} i2c pins\r\n", digitalPins, analogPins, servoPins, pwmPins, i2cPins);
FCapabilities.SliceCount = 1;
FCapabilities[0] = report;
break;
}
}
private void Decode(byte data)
{
if ((data & 0x80) > 0)
{
byte cmd = FirmataUtils.GetCommandFromByte(data);
lastCommand = cmd;
switch (cmd)
{
case Command.RESET:
buffer.Clear();
break;
case Command.DIGITALMESSAGE:
case Command.ANALOGMESSAGE:
case Command.REPORT_VERSION:
case Command.SETPINMODE:
remaining = 2;
buffer.Clear();
buffer.Enqueue(data);
break;
case Command.TOGGLEDIGITALREPORT:
case Command.TOGGLEANALOGREPORT:
remaining = 1;
buffer.Clear();
buffer.Enqueue(data);
break;
case Command.SYSEX_START:
buffer.Clear();
break;
case Command.SYSEX_END:
// Fire Sysex event
ProcessSysex(buffer);
break;
default:
// unknown command
break;
}
}
else
{
buffer.Enqueue(data);
if (--remaining == 0)
{
// process the message
switch (lastCommand)
{
case Command.ANALOGMESSAGE:
int pin, value;
FirmataUtils.DecodeAnalogMessage(buffer.ToArray(), out pin, out value);
if (pin < FAnalogInputCount[0])
{
FAnalogIns[pin] = value; // assign the found value to the spread
}
break;
case Command.DIGITALMESSAGE:
int port; int[] vals;
// Decode the values from the bytes:
FirmataUtils.DecodePortMessage(buffer.ToArray(), out port, out vals);
// Fill the spread with parsed pinstates
int pinNum;
for (int i = 0; i < Constants.BitsPerPort; i++)
{
pinNum = i + Constants.BitsPerPort * port;
if (pinNum < FDigitalIns.SliceCount)
{
FDigitalIns[pinNum] = vals[i];
}
}
break;
case Command.REPORT_VERSION:
int major = (int)buffer.Dequeue();
int minor = (int)buffer.Dequeue();
FFirmwareMajorVersion[0] = major;
FFirmwareMinorVersion[0] = minor;
break;
default:
// unkown byte...
break;
}
}
}
}
private void HandleStream(Stream InStream)
{
// Check, if the incoming Stream is usable
if (InStream == null || InStream.Length == 0 || !InStream.CanRead)
{
return;
}
// Read the incoming bytes to the internal stream buffer
while (InStream.Position < InStream.Length)
{
Buffer.Enqueue((byte)InStream.ReadByte());
}
// A cache for sysex data
Queue <byte> cache = new Queue <byte>();
// A flag if parsing sysex data
bool bIsSysex = false;
// PARSE:
while (Buffer.Count > 0)
{
byte current = Buffer.Dequeue();
switch (current)
{
case Command.SYSEX_START:
case Command.SYSEX_END:
if (current == Command.SYSEX_START)
{
bIsSysex = true;
}
else if (current == Command.SYSEX_END)
{
// Process the Sysexdata:
ProcessSysex(cache);
bIsSysex = false;
}
cache.Clear();
break;
default:
if (bIsSysex) // Collect bytes for the SysSex message cache
{
cache.Enqueue(current);
}
else
{
// Treat Ananlog & Digital Messages:
bool hasDigitalMessage = FirmataUtils.VerifiyCommand(current, Command.DIGITALMESSAGE);
bool hasAnalogMessage = FirmataUtils.VerifiyCommand(current, Command.ANALOGMESSAGE);
// We have a data for commands
if (Buffer.Count >= 2 && (hasDigitalMessage || hasAnalogMessage))
{
// Reihenfolge matters!
byte[] data = { current, Buffer.Dequeue(), Buffer.Dequeue() };
// Check for Analog Command
if (hasAnalogMessage)
{
int pinNum, value;
FirmataUtils.DecodeAnalogMessage(data, out pinNum, out value);
if (pinNum < FAnalogInputCount[0])
{
FAnalogIns[pinNum] = value; // assign the found value to the spread
}
}
else if (hasDigitalMessage)
{
int port; int[] vals;
// Decode the values from the bytes:
FirmataUtils.DecodePortMessage(data, out port, out vals);
// Fill the spread with parsed pinstates
for (int i = 0; i < Constants.BitsPerPort; i++)
{
int pinNum = i + Constants.BitsPerPort * port;
if (pinNum < FDigitalIns.SliceCount)
{
FDigitalIns[pinNum] = vals[i];
}
}
}
}
}
break;
}
}
}