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);
}
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);
}
}
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);
}
}
}