private void ParsePortMessage(byte [] data)
{
try
{
// A port 0 and a BoostMove? It's the latest firmware. Fix ports
if (Type == Types.BOOST_MOVE_HUB && data[3] == 0)
{
UpdateBoostMovePortToLatestFirmware();
}
Port port = GetPortFromPortNumber(data[3]);
if (port == null)
{
return;
}
port.Connected = (data[4] == 1 || data[4] == 2) ? true : false;
RegisterDeviceAttachement(port, (data.Length < 6) ? Port.Devices.UNKNOWN : (Port.Devices)data[5]);
}
catch (Exception ex)
{
MainBoard.WriteLine("ERROR in Port Message: " + ex.Message, Color.Red);
}
}
/// <summary>
/// Send the message to the proper device and characteristic
/// </summary>
/// <param name="message"></param>
protected virtual async void WriteMessage(byte[] message, bool addLength)
{
try
{
if (Characteristic != null)
{
using (DataWriter writer = new DataWriter())
{
if (addLength)
{
writer.WriteBytes(new byte[] { (byte)(message.Length + 2), 0x00 });
}
writer.WriteBytes(message);
await Characteristic.WriteValueAsync(writer.DetachBuffer(), GattWriteOption.WriteWithoutResponse);
}
}
}
catch
{
MainBoard.WriteLine($"{Name} lost bluetooth connection", Color.Red);
Dispose();
OnDataUpdated();
}
}
public async Task ClearAutomatedPathAndStop(bool clearPath)
{
AbortReserve = true;
// If the train is waiting at a section we wait for its status to be cleared
while (IsWaitingSection)
{
MainBoard.WriteLine($"{Name} - Attempting to clear path and stop train, but waiting for lock to finish", Color.Red);
await Task.Delay(1000);
}
AbortReserve = false;
if (clearPath)
{
IsPathProgramRunning = false;
CurrentPathPositionIndex = -1;
CurrentPath = null;
}
// Stop the train!
Stop();
MainBoard.WriteLine($"{Name} path has been cleared and train is stopped", Color.Red);
}
private void ParseDeviceInfo(byte [] data)
{
//MainBoard.WriteLine("Device Info Message received: " + BitConverter.ToString(data));
if (data[3] == 2)
{
if (data[5] == 1)
{
MainBoard.WriteLine("Power Button was pressed");
OnRemoteTriggered(this, null, RemoteButtons.BUTTON_POWER);
return;
}
}
// Firmware version
else if (data[3] == 3)
{
int fwVersion10000224 = BitConverter.ToInt32(new byte[] { 0x24, 0x02, 0x00, 0x10 }, 0);
int currentVersion = BitConverter.ToInt32(data, 5);
if (currentVersion > fwVersion10000224 && Type == Types.BOOST_MOVE_HUB)
{
UpdateBoostMovePortToLatestFirmware();
OnPortTypeUpdate();
OnDataUpdated();
}
}
else if (data[3] == 0x06)
{
BatteryLevel = data[5];
OnDataUpdated();
}
}
private async void RefreshFileDir()
{
try
{
PFxAction action = new PFxAction();
byte[] rx = await SendData(action.GetCmdNumFiles());
fileId.Clear();
fileNames.Clear();
int file_count = rx[4];
for (int i = 0; i < file_count; i++)
{
byte[] rxDir = await SendData(action.GetCmdGetDirEntry((byte)(i + 1)));
int id = rxDir[3];
fileId.Add(id);
string name = "";
for (int j = 0; j < 32 && rxDir[24 + j] != 0; j++)
{
name += (char)rxDir[24 + j];
}
fileNames.Add(name);
MainBoard.WriteLine("PFx: new file found - " + name + " (id " + id + ")");
}
MainBoard.WriteLine(fileId.Count + " files found on PFx");
}
catch (Exception ex)
{
MainBoard.WriteLine("Exception while parsing files: " + ex.Message, Color.Red);
}
}
internal override async Task RenewCharacteristic()
{
if (Device != null)
{
Device = await BluetoothLEDevice.FromBluetoothAddressAsync(Device.BluetoothAddress);
Gatt = await Device.GetGattServicesAsync(BluetoothCacheMode.Uncached);
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("4dc591b0-857c-41de-b5f1-15abda665b0c")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
Characteristic = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse("489a6ae0-c1ab-4c9c-bdb2-11d373c1b7fb"));
CharacteristicCommands = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse("02b8cbcc-0e25-4bda-8790-a15f53e6010f"));
MainBoard.WriteLine("New Hub Found of type " + Enum.GetName(typeof(Hub.Types), Type), Color.Green);
}
// If we reconnect, let's recalibrate sensors
CalibrationIsDown = false;
LastCalibrationTick = Environment.TickCount;
// If we come back, we stop and start over.
if (pingTimer != null)
{
pingTimer.Stop();
pingTimer.Elapsed -= PingTimer_Elapsed;
}
else
{
pingTimer = new Timer();
}
pingTimer.Interval = 150;
pingTimer.Elapsed += PingTimer_Elapsed;
pingTimer.Start();
}
internal override async Task RenewCharacteristic()
{
if (Device != null)
{
Device = await BluetoothLEDevice.FromBluetoothAddressAsync(Device.BluetoothAddress);
Gatt = await Device.GetGattServicesAsync(BluetoothCacheMode.Uncached);
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("00001523-1212-efde-1523-785feabcd123")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
CharacteristicPort = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_PORT_TYPE));
CharacteristicButton = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_BUTTON));
CharacteristicDisconnect = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_DISCONNECT));
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("00004f0e-1212-EFDE-1523-785FEABCD123")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
CharacteristicSensor = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_SENSOR_VALUE));
CharacteristicPortWrite = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_PORT_TYPE_WRITE));
Characteristic = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_MOTOR_VALUE_WRITE));
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("0000180F-0000-1000-8000-00805F9B34FB")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
CharacteristicBattery = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse(WEDO2_BATTERY));
MainBoard.WriteLine("New Hub Found of type " + Enum.GetName(typeof(Hub.Types), Type), Color.Green);
}
}
public void TryToConnect()
{
MainBoard.WriteLine("Connecting to EV3 on port " + DeviceId);
var conType = CreateConnection();
Dispatcher.CurrentDispatcher.Invoke(new Action(async() =>
{
if (conType != null)
{
brick = new Brick(conType, true);
brick.BrickChanged += Brick_BrickChanged;
try
{
await brick.ConnectAsync();
}
catch (Exception)
{
MessageBox.Show("Could not connect", "Error", MessageBoxButtons.OK);
}
}
else
{
MessageBox.Show("Invalid connection type for this device", "Error", MessageBoxButtons.OK);
}
}));
}
public override void SetLightBrightness(string port, int brightness)
{
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Light Brightness for " + Name + " because no default port are setup", Color.Red);
return;
}
portObj.Speed = brightness;
OnDataUpdated();
byte[] data = new byte[6];
data[0] = 0x10;
foreach (Port p in RegistredPorts)
{
data[RegistredPorts.IndexOf(p) + 1] = (byte)p.Speed;
}
data[5] = 0;
WriteMessage(data, false);
}
/// <summary>
/// Click on the "Open All"
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public static TrainProject Load(string path)
{
try
{
IFormatter formatter = new BinaryFormatter();
using (Stream stream = new FileStream(path, FileMode.Open, FileAccess.Read))
{
TrainProject project = (TrainProject)formatter.Deserialize(stream);
project.Path = path;
stream.Close();
// Make sure we clear any event serialized
foreach (Hub train in project.RegisteredTrains)
{
train.CleanAllEvents();
}
return(project);
}
}
catch (Exception ex)
{
MainBoard.WriteLine("ERROR - Could not open file: " + ex.Message, System.Drawing.Color.Red);
return(null);
}
}
private void DistanceTriggeredHandler(Hub train, Port port, int distance)
{
if (hubs == null || hubs.Count == 0)
{
MainBoard.WriteLine("Distance event received, but program is not started. Start program first");
return;
}
foreach (TrainProgramEvent e in Events)
{
if (e.TrainDeviceID == train.DeviceId && (e.TrainPort == null || e.TrainPort == port.Id))
{
if ((e.Trigger == TrainProgramEvent.TriggerType2.Distance_is_above || e.Trigger == TrainProgramEvent.TriggerType2.Raw_Value_is_above) && distance > e.TriggerDistanceParam)
{
port.LastDistanceTick = Environment.TickCount;
MainBoard.WriteLine("Event #" + (Events.IndexOf(e) + 1) + " triggered - " + train.Name + " on port " + port.Id + ((train.Type == Hub.Types.EV3) ? " has a raw value " : " distance ") + "above " + distance, Color.Purple);
ActivateAction(e);
}
else if ((e.Trigger == TrainProgramEvent.TriggerType2.Distance_is_below || e.Trigger == TrainProgramEvent.TriggerType2.Raw_Value_is_below) && distance < e.TriggerDistanceParam)
{
port.LastDistanceTick = Environment.TickCount;
MainBoard.WriteLine("Event #" + (Events.IndexOf(e) + 1) + " triggered - " + train.Name + " on port " + port.Id + ((train.Type == Hub.Types.EV3) ? " has raw value " : " has a distance ") + "below " + distance, Color.Purple);
ActivateAction(e);
}
}
}
}
public bool Start(TrainProject project)
{
if (IsRunning)
{
return(true);
}
// We first initialize the program
if (!Init(project))
{
return(false);
}
// Then we plug all event handlers
foreach (Hub t in hubs)
{
t.ColorTriggered += ColorTriggeredHandler;
t.DistanceTriggered += DistanceTriggeredHandler;
t.RemoteTriggered += RemoteTriggeredHandler;
}
MainBoard.WriteLine($"All Sensor Events for {Name} are active.", Color.DarkMagenta);
IsRunning = true;
return(true);
}
/// <summary>
/// Treat Incoming Data from the train
/// </summary>
/// <param name="sender"></param>
/// <param name="args"></param>
internal virtual void Characteristic_ValueChanged(GattCharacteristic sender, GattValueChangedEventArgs args)
{
try
{
int numberOfMessage = 0;
// An Indicate or Notify reported that the value has changed.
var reader = DataReader.FromBuffer(args.CharacteristicValue);
while (reader.UnconsumedBufferLength > 0)
{
numberOfMessage++;
// We read the length and the rest of the body
byte len = reader.ReadByte();
byte[] toread = new byte[len - 1];
reader.ReadBytes(toread);
// Ajusting so that the message is properly sized
byte[] message = new byte[len];
System.Buffer.BlockCopy(toread, 0, message, 1, toread.Length);
switch (message[2])
{
case 0x01:
{
this.ParseDeviceInfo(message);
break;
}
case 0x04:
{
this.ParsePortMessage(message);
break;
}
case 0x45:
{
this.ParseSensorMessage(message);
break;
}
case 0x82:
{
this.ParsePortAction(message);
break;
}
default:
{
break;
}
}
}
}
catch (Exception ex)
{
MainBoard.WriteLine("FATAL: Something went wrong while reading messages!" + Environment.NewLine + ex.Message, Color.DarkRed);
}
}
private static void ParseStatus(byte[] message)
{
switch (message[1])
{
case 0x00:
{
//MainBoard.WriteLine("SBRICK - ACK - ");
break;
}
case 0x01:
{
MainBoard.WriteLine("SBRICK - Invalid Data Length - ");
break;
}
case 0x02:
{
MainBoard.WriteLine("SBRICK - Invalid Parameter - ");
break;
}
case 0x03:
{
MainBoard.WriteLine("SBRICK - No Such Command - ");
break;
}
case 0x04:
{
MainBoard.WriteLine("SBRICK - No authentication needed - ");
break;
}
case 0x05:
{
MainBoard.WriteLine("SBRICK - Authentication error - ");
break;
}
case 0x06:
{
MainBoard.WriteLine("SBRICK - Authentication needed - ");
break;
}
case 0x08:
{
MainBoard.WriteLine("SBRICK - Thermal protection is active - ");
break;
}
case 0x09:
{
MainBoard.WriteLine("SBRICK - The system is in a state where the command does not make sense - ");
break;
}
}
}
/// <summary>
/// Enable Notifications from Train Characteristics
/// </summary>
public async override void StartListening(BluetoothLEDevice device)
{
try
{
// Assign the device to this hub
Device = device;
// Save the bluetooth address for reconnection purpose
BluetoothAddress = device.BluetoothAddress;
// Obtain a fresh Characteristics
await RenewCharacteristic();
// If it succeeded
if (Characteristic != null)
{
// Immediately attach to get all data from the Hub
CharacteristicPort.ValueChanged += CharacteristicPort_ValueChanged;
CharacteristicSensor.ValueChanged += CharacteristicSensor_ValueChanged;
CharacteristicButton.ValueChanged += CharacteristicButton_ValueChanged;
CharacteristicBattery.ValueChanged += CharacteristicBattery_ValueChanged;
// Ask for notifications
GattCommunicationStatus statusPort = await CharacteristicPort.WriteClientCharacteristicConfigurationDescriptorAsync(GattClientCharacteristicConfigurationDescriptorValue.Notify);
GattCommunicationStatus statusSensor = await CharacteristicSensor.WriteClientCharacteristicConfigurationDescriptorAsync(GattClientCharacteristicConfigurationDescriptorValue.Notify);
GattCommunicationStatus statusButton = await CharacteristicButton.WriteClientCharacteristicConfigurationDescriptorAsync(GattClientCharacteristicConfigurationDescriptorValue.Notify);
GattCommunicationStatus statusBattery = await CharacteristicBattery.WriteClientCharacteristicConfigurationDescriptorAsync(GattClientCharacteristicConfigurationDescriptorValue.Notify);
if (statusPort == GattCommunicationStatus.Success &&
statusSensor == GattCommunicationStatus.Success &&
statusButton == GattCommunicationStatus.Success &&
statusBattery == GattCommunicationStatus.Success)
{
Thread.Sleep(1000);
InitializeNotifications();
IsConnected = true;
MainBoard.WriteLine($"Hub {Name} is connected!", Color.Green);
RestoreLEDColor();
}
else
{
MainBoard.WriteLine("Characteristic Unreachable!", Color.Red);
}
}
}
catch (Exception ex)
{
MainBoard.WriteLine("FATAL ERROR while trying to connect to a train", Color.Red);
MainBoard.WriteLine("Exception: " + ex.Message, Color.Red);
Device = null;
}
}
//////////////////////////////////////////
////
////
////
//// All Public Functions functions
////
////
////
////
//////////////////////////////////////////
public void SetMotorSpeed(int speed)
{
if (TrainMotorPort != null)
{
SetMotorSpeed(TrainMotorPort, speed);
}
else
{
MainBoard.WriteLine("Could not start engine as no TRAIN_MOTOR is defined");
}
}
public override void SetLightBrightness(string port, int brightness)
{
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Light Brightness for " + Name + " because no default port are setup", Color.Red);
return;
}
portObj.Speed = brightness;
WriteMessage(new byte[] { 0x01, (byte)portObj.Value, 1, (byte)(portObj.Speed * 2.5) }, CharacteristicCommands);
}
internal void StartSequence(TrainProgramEvent trainEvent)
{
if (hubs == null)
{
MainBoard.WriteLine($"You need at least 1 registered hub to start a program. '{Name}' cannot be started.", Color.Red);
return;
}
// Make sure to execute code and nothing else
trainEvent.Action = TrainProgramEvent.ActionType.Execute_Code;
// Let's roll!
ActivateAction(trainEvent);
}
public virtual void SetLightBrightness(string port, int brightness)
{
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Light Brightness for " + Name + " because no default port are setup", Color.Red);
return;
}
byte[] message = new byte[] { 0x81, (byte)portObj.Value, 0x11, 0x51, 0x00, (byte)brightness };
WriteMessage(message);
}
/// <summary>
/// Set Motor Speed for this train
/// </summary>
/// <param name="port"></param>
/// <param name="speed"></param>
public virtual void SetMotorSpeed(string port, int speed)
{
byte[] message;
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Motor Speed to " + speed + " for " + Name + " because no default port are setup", Color.Red);
return;
}
// BOOST HUB
if (Type == Types.BOOST_MOVE_HUB)
{
portObj.Busy = true;
message = null;
if (portObj.Id == "AB")
{
message = new byte[] { 0x81, (byte)portObj.Value, 0x11, 0x02, (byte)speed, (byte)speed, 0x64, 0x7f, 0x03 };
}
else
{
message = new byte[] { 0x81, (byte)portObj.Value, 0x11, 0x01, (byte)speed, 0x64, 0x7f, 0x03 };
}
}
// ELSE TRAIN HUB
else
{
if (port == "AB")
{
byte p = 57;
byte s = (byte)speed;
message = new byte[] { 0x81, p, 0x11, 0x02, (byte)speed, (byte)speed };
}
else
{
byte s = (byte)speed;
byte p = (port == "A") ? (byte)0x00 : (byte)0x01;
message = new byte[] { 0x81, (byte)portObj.Value, 0x11, 0x51, 0x00, (byte)speed };
//message = new byte[] { 0x81, p, 0x11, 0x60, 0x00, (byte)speed, 0x00, 0x00 };
}
}
portObj.Speed = speed;
IsBusy = speed != 0 && speed != 127;
OnDataUpdated();
WriteMessage(message);
}
internal override async Task RenewCharacteristic()
{
if (Device != null)
{
Device = await BluetoothLEDevice.FromBluetoothAddressAsync(Device.BluetoothAddress);
Gatt = await Device.GetGattServicesAsync(BluetoothCacheMode.Uncached);
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("4E050000-74FB-4481-88B3-9919B1676E93")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
Characteristic = AllCharacteristic.Characteristics.Single(s => s.Uuid == Guid.Parse("000092d1-0000-1000-8000-00805f9b34fb"));
MainBoard.WriteLine("New Hub Found of type " + Enum.GetName(typeof(Hub.Types), Type), Color.Green);
}
}
public override void SetLightBrightness(string port, int brightness)
{
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set light brightness for " + Name, Color.Red);
return;
}
portObj.Speed = brightness;
OnDataUpdated();
//brick.DirectCommand.(OutputPort.A, speed);
}
//added by Tom Cook to rename hub and send via bluetooth
private void btnRename_Click(object sender, EventArgs e)
{
string name = textBoxName.Text;
//if (CurrentHub.Name != name)
{
if (!CurrentHub.Rename(name))
{
MessageBox.Show(this, "Name must be 14 characters or less.", "Name is invalid", MessageBoxButtons.OK);
}
else
{
MainBoard.WriteLine(string.Format("Hub with MAC Address {0:X} name was changed to '{1:X}'.", CurrentHub.BluetoothAddress, name), System.Drawing.Color.Blue);
}
}
}
/// <summary>
/// Obtain a new Characteristic
/// </summary>
/// <param name="device"></param>
/// <returns></returns>
internal virtual async System.Threading.Tasks.Task RenewCharacteristic()
{
if (Device != null)
{
Device = await BluetoothLEDevice.FromBluetoothAddressAsync(Device.BluetoothAddress);
Gatt = await Device.GetGattServicesAsync(BluetoothCacheMode.Uncached);
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("00001623-1212-efde-1623-785feabcd123")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
Characteristic = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse("00001624-1212-efde-1623-785feabcd123"));
MainBoard.WriteLine("New Hub Found of type " + Enum.GetName(typeof(Hub.Types), Type), Color.Green);
}
}
internal override async Task RenewCharacteristic()
{
if (Device != null)
{
Device = await BluetoothLEDevice.FromBluetoothAddressAsync(Device.BluetoothAddress);
Gatt = await Device.GetGattServicesAsync(BluetoothCacheMode.Uncached);
AllCharacteristic = await Gatt.Services.Single(s => s.Uuid == Guid.Parse("49535343-FE7D-4AE5-8FA9-9FAFD205E455")).GetCharacteristicsAsync(BluetoothCacheMode.Uncached);
Characteristic = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse("49535343-1E4D-4BD9-BA61-23C647249616"));
TxCharacteristic = AllCharacteristic.Characteristics.Single(c => c.Uuid == Guid.Parse("49535343-8841-43F4-A8D4-ECBE34729BB3"));
MainBoard.WriteLine("New Hub Found of type " + Enum.GetName(typeof(Hub.Types), Type), Color.Green);
}
}
public override void SetLightBrightness(string port, int brightness)
{
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Light Brightness for " + Name + " because no default port are setup", Color.Red);
return;
}
portObj.Speed = brightness;
byte[] data = new byte[] { (byte)portObj.Value, 0x01, 0x02, (byte)brightness };
WriteMessage(data);
}
public void SaveAs(string path)
{
try
{
IFormatter formatter = new BinaryFormatter();
using (Stream stream = new FileStream(path, FileMode.Create, FileAccess.Write))
{
formatter.Serialize(stream, this);
stream.Close();
Path = path;
}
}
catch (Exception ex)
{
MainBoard.WriteLine("ERROR - Could not save file: " + ex.Message, System.Drawing.Color.Red);
}
}
public override void SetMotorSpeed(string port, int speed)
{
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Motor Speed to " + speed + " for " + Name + " because no default port are setup", Color.Red);
return;
}
portObj.Speed = speed;
IsBusy = (speed != 0);
OnDataUpdated();
WriteMessage(new byte[] { 0x01, (byte)portObj.Value, (byte)((speed > 0) ? 0 : 1), (byte)Math.Abs(portObj.Speed * 2.5) }, CharacteristicCommands);
}
public async override void SetMotorSpeed(string port, int speed)
{
PFxAction action = new PFxAction();
Port portObj = GetPortFromPortId(port);
// If we can't find the port, we can't do anything!
if (portObj == null)
{
MainBoard.WriteLine("Could not set Motor Speed to " + speed + " for " + Name + " because no default port are setup", Color.Red);
return;
}
double sf = speed;
if (sf > 100.0)
{
sf = 100.0;
}
if (sf < -100.0)
{
sf = -100.0;
}
sf = (sf / 100.0) * 63.0;
portObj.Speed = speed;
int si = (int)(Math.Abs(sf)) & EVT_MOTOR_SPEED_HIRES_MASK;
si |= EVT_MOTOR_SPEED_HIRES;
if (sf < 0.0)
{
si |= EVT_MOTOR_SPEED_HIRES_REV;
}
action.motorParam1 = (byte)si;
int m = PortToMask(port) & EVT_MOTOR_OUTPUT_MASK;
m |= EVT_MOTOR_SET_SPD;
action.motorActionId = (byte)m;
OnDataUpdated();
await SendData(action.GetTestActionData());
}
protected async void WriteMessage(byte[] message, GattCharacteristic characteristic)
{
try
{
if (characteristic != null)
{
using (DataWriter writer = new DataWriter())
{
writer.WriteBytes(message);
await characteristic.WriteValueAsync(writer.DetachBuffer(), GattWriteOption.WriteWithoutResponse);
}
}
}
catch
{
MainBoard.WriteLine($"{Name} just lost bluetooth connection. Attempting to reconnect.", Color.Red);
Dispose();
}
}