void IZomBMonitor.UpdateStatus(FRCDSStatus status)
{
if (EnableEvents)
{
EventArgs e = new EventArgs();
if (NewPacketRecieved != null)
NewPacketRecieved(this, e);
if (status.Error.PacketLost && PacketLost != null)
PacketLost(this, e);
//RobotCom
if (status.Status.RobotAttached)
{
if (!stat.RobotAttached && RobotConnected != null)
RobotConnected(this, e);
//Enable
if (status.Status.Enabled && !stat.Enabled)
{
if (Enabled != null)
Enabled(this, e);
//Begin Auto
if (status.Status.Auto)
{
if (AutoBegin != null)
AutoBegin(this, e);
if (status.Status.FMSAttached)
{
matchStart = new DateTime(DateTime.Now.Ticks);
if (MatchStarted != null)
MatchStarted(this, e);
}
}
//Begin Telop
else
{
if (TelopBegin != null)
TelopBegin(this, e);
if (status.Status.FMSAttached)
{
//Give Leeway
if (DateTime.Now.Subtract(matchStart).Seconds >= 14 && DateTime.Now.Subtract(matchStart).Seconds <= 18)
{
}
//uh oh!
else
{
if (MatchAborted != null)
MatchAborted(this, e);
}
}
}
}
//Disable
if (!status.Status.Enabled && stat.Enabled)
{
if (Disabled != null)
Disabled(this, e);
//end Auto
if (status.Status.Auto)
{
if (AutoEnd != null)
AutoEnd(this, e);
if (status.Status.FMSAttached)
{
//Give Leeway
if (DateTime.Now.Subtract(matchStart).Seconds >= 14 && DateTime.Now.Subtract(matchStart).Seconds <= 18)
{
}
//uh oh!
else
{
if (MatchAborted != null)
MatchAborted(this, e);
}
}
}
//end Telop
else
{
if (TelopEnd != null)
TelopEnd(this, e);
if (status.Status.FMSAttached)
{
//Give Leeway
if (DateTime.Now.Subtract(matchStart).Seconds >= 14 && DateTime.Now.Subtract(matchStart).Seconds <= 18
&& DateTime.Now.Subtract(matchStart).Minutes == 2)
{
if (MatchEnded != null)
MatchEnded(this, e);
}
//uh oh!
else
{
if (MatchAborted != null)
MatchAborted(this, e);
}
}
}
}
if (status.Status.EmergencyStopped && !stat.EmergencyStopped && EStopped != null)
EStopped(this, e);
if (status.Status.FMSAttached && !stat.FMSAttached && FMSAttached != null)
FMSAttached(this, e);
}
}
stat.Byte = status.Status.Byte;
ebf.Byte = status.Error.Byte;
indio.Byte = status.DigitalIn.Byte;
outdio.Byte = status.DigitalOut.Byte;
team = status.Team;
battery = status.Battery;
}
private void worker()
{
char c;
ushort us;
int iv;
while (Running)
{
try
{
c = (char)redbit.ReadByte();
switch (c)
{
case 'T':
iv = (((int)redbit.ReadByte()) << 8) + redbit.ReadByte();
while (Running && iv > 1)//takes some time to read the next one
{
Thread.Sleep(2);
iv -= 2;
}
break;
case 'S':
sts = new FRCDSStatus();
sts.Status = new StatusBitField(redbit.ReadByte());
sts.Error = new ErrorBitField(redbit.ReadByte());
sts.DigitalIn = new DIOBitField(redbit.ReadByte());
sts.DigitalOut = new DIOBitField(redbit.ReadByte());
sts.PacketNumber = (ushort)((((int)redbit.ReadByte()) << 8) + redbit.ReadByte());
sts.Battery = redbit.ReadByte() + (((int)redbit.ReadByte()) / 100);
if (NewStatusRecieved != null)
NewStatusRecieved(this, new NewStatusRecievedEventArgs(sts));
break;
case 'D':
iv = (((int)redbit.ReadByte()) << 8) + redbit.ReadByte();
byte[] buffer = new byte[iv];
us = 0;
us = (ushort)redbit.Read(buffer, 0, iv);
while (us != iv)
{
us += (ushort)redbit.Read(buffer, us, iv - us);
}
vls = SplitParams(UTF8Encoding.UTF8.GetString(buffer));
if (DataRecieved != null)
DataRecieved(this, new EventArgs());
if (NewDataRecieved != null)
NewDataRecieved(this, new NewDataRecievedEventArgs(vls));
break;
case 'Q':
return;
default:
if (OnError != null)
OnError(this, new ErrorEventArgs(new Exception("missed command statements")));
break;
}
}
catch (Exception ex)
{
if (OnError != null)
OnError(this, new ErrorEventArgs(ex));
}
}
}
/// <summary>
/// IZomBMonitor: Updates the status
/// </summary>
/// <param name="status">The new status</param>
public void UpdateStatus(FRCDSStatus status)
{
//ignore status
}
/// <summary>
/// The background worker. Will exit after 10 consectutive errors
/// </summary>
private void DoWork()
{
//number of errors
int nume = 0;
while (isrunning)
{
try
{
IPEndPoint RIPend = null;
//Recieve the data
while (cRIOConnection.Available < 1018 && isrunning) { Thread.Sleep(2); }
if (!isrunning)
{
cRIOConnection.Close();
return;
}
byte[] buffer = cRIOConnection.Receive(ref RIPend);
//Check first
if (!VerifyPacket(buffer))
{
if (InvalidPacketRecieved != null && !peeking)
{
//Create our e
InvalidPacketRecievedEventArgs e = new InvalidPacketRecievedEventArgs(buffer, ddh.InvalidPacketAction == InvalidPacketActions.AlwaysContinue || ddh.InvalidPacketAction == InvalidPacketActions.Continue);
InvalidPacketRecieved(this, e);
if ((int)ddh.InvalidPacketAction < 3)//1-4
{
if (!e.ContinueAnyway)
break;
}
else if (ddh.InvalidPacketAction == InvalidPacketActions.AlwaysIgnore)
break;
}
}
//DS Status
cStat = ParseDSBytes(buffer);
//Get a stream
MemoryStream Output = new MemoryStream(buffer);
Output.Seek(27, SeekOrigin.Begin);
EBinaryReader binladen = new EBinaryReader(Output);
//get length
int totallength = (binladen.ReadUInt16() << 16);
totallength += binladen.ReadUInt16();
//clear last loop's controls
kys = new ZomBDataLookup();
//loop all controls
while (totallength > (Output.Position - 27))
{
byte b = binladen.ReadByte();
switch (b)
{
case 0://Announce
{
int id = binladen.ReadByte();
SmartDataTypes type = (SmartDataTypes)binladen.ReadByte();
int len = binladen.ReadUInt16();
byte[] name = new byte[len];
binladen.Read(name, 0, len);
if (peeking)
{
dp.Invoke(cb, Encoding.UTF8.GetString(name));
nametable[id] = new SmartInfo { Name = Encoding.UTF8.GetString(name), Type = type };
}
else
{
nametable[id] = new SmartInfo { Name = Encoding.UTF8.GetString(name), Type = type };
}
break;
}
case 1://Update
{
int id = binladen.ReadByte();
if (nametable.ContainsKey(id))
{
var info = nametable[id];
string value = info.Parse(binladen);
kys[info.Name] = new ZomBDataObject(value, info.ZomBTypeHint);
}
else
{
b = binladen.ReadByte();//get length
binladen.Read(new byte[b], 0, b);//skip it
}
break;
}
case 2://GUI Announce, not impl
{
System.Windows.Forms.MessageBox.Show("Dashboard selection via Protocol has not been implemented in ZomB, try WPF tabs instead");
break;
}
default:
throw new Exception("Bin ("+b+") not a 1 or zero, exiting");
}
}
if (kys.Count > 0)//Empty keys in empty packet, "Smart" Dashboard can be dumb
//All in favor of chaging the name to DumbDashboard say "aye!" "sqrt(-1)!"
{
//Fire events
if (DataRecieved != null)
DataRecieved(this, new EventArgs());
if (NewStatusRecieved != null)
NewStatusRecieved(this, new NewStatusRecievedEventArgs(cStat));
if (NewDataRecieved != null)
NewDataRecieved(this, new NewDataRecievedEventArgs(kys));
}
if (nume > 0)
nume--;
}
catch (ThreadAbortException)
{
isrunning = false;
cRIOConnection.Close();
return;
}
catch (Exception ex)
{
nume++;
DoError(ex);
if (nume > 10)
{
isrunning = false;
DoError(new Exception("10 consecutive errors were encountered, stopping Smart"));
isrunning = false;
try
{
cRIOConnection.Close();
}
catch { }
return;
}
}
}
}
/// <summary>
/// Convert the DS Bytes to a FRCDSStatus
/// </summary>
/// <param name="buffer">The bytes from the Robot packet</param>
/// <returns>A FRCDSStatus containg the robot status</returns>
protected static FRCDSStatus ParseDSBytes(byte[] buffer)
{
//TODO: Find and Fix errors here
FRCDSStatus ret = new FRCDSStatus();
ret.PacketNumber = buffer[0];
ret.PacketNumber += (ushort)(buffer[1] << 8);
ret.DigitalIn = new DIOBitField(buffer[2]);
ret.DigitalOut = new DIOBitField(buffer[3]);
ret.Battery = float.Parse(buffer[4].ToString("x") + "." + buffer[5].ToString("x"));
ret.Status = new StatusBitField(buffer[6]);
ret.Error = new ErrorBitField(buffer[7]);
ret.Team = (int)(buffer[8] * 100) + (int)(buffer[9]);
//there's got to be a better way to do this
//TODO: This does not work DS Bytes
//int Month = int.Parse(new string(new char[] { (char)buffer[10], (char)buffer[11] }));
//int Day = int.Parse(new string(new char[] { (char)buffer[12], (char)buffer[13] }));
//int year = 2000 + int.Parse(new string(new char[] { (char)buffer[14], (char)buffer[15] }));
//ret.Version = new DateTime(year, Month, Day);
//ret.Revision = (ushort)((ushort)(buffer[16]) + ((ushort)(buffer[17] >> 8)));
return ret;
}
public void UpdateStatus(FRCDSStatus status)
{
}
/// <summary>
/// IZomBMonitor: Updates the status
/// </summary>
/// <param name="status">The new status</param>
public void UpdateStatus(FRCDSStatus status)
{
if (saving)
{
byte[] bit = new byte[9];
bit[0] = 83;//"S"
bit[1] = status.Status.Byte;
bit[2] = status.Error.Byte;
bit[3] = status.DigitalIn.Byte;
bit[4] = status.DigitalOut.Byte;
bit[5] = (byte)(status.PacketNumber >> 8);
bit[6] = (byte)status.PacketNumber;
bit[7] = (byte)((int)status.Battery);
bit[8] = (byte)((status.Battery - ((int)status.Battery)) * 100);
AddValue(bit);
}
}
/// <summary>
/// The background worker. Will exit after 10 consectutive errors
/// </summary>
private void DoWork()
{
//number of errors
int nume = 0;
while (isrunning)
{
try
{
IPEndPoint RIPend = null;
//Recieve the data
while (cRIOConnection.Available < 1018 && isrunning) { Thread.Sleep(2); }
if (!isrunning)
{
cRIOConnection.Close();
return;
}
byte[] buffer = cRIOConnection.Receive(ref RIPend);
string Output;
//Convert
Output = UTF8Encoding.UTF8.GetString(buffer);
//Find segment of data
if (Output.Contains("@@ZomB:|") && Output.Contains("|:ZomB@@") && !Output.Contains("@@ZomB:|:ZomB@@"))
{
Output = Output.Substring(Output.IndexOf("@@ZomB:|") + 8, (Output.IndexOf("|:ZomB@@") - (Output.IndexOf("@@ZomB:|") + 8)));
if (Output != "")
{
//Check first
if (!VerifyPacket(buffer))
{
if (InvalidPacketRecieved != null && !peeking)
{
//Create our e
InvalidPacketRecievedEventArgs e = new InvalidPacketRecievedEventArgs(buffer, ddh.InvalidPacketAction == InvalidPacketActions.AlwaysContinue || ddh.InvalidPacketAction == InvalidPacketActions.Continue);
InvalidPacketRecieved(this, e);
if ((int)ddh.InvalidPacketAction < 3)//1-4
{
if (!e.ContinueAnyway)
break;
}
else if (ddh.InvalidPacketAction == InvalidPacketActions.AlwaysIgnore)
break;
}
}
//Get the items in a dictionary
ZomBDataLookup vals = SplitParams(Output);
if (peeking)
{
foreach (var keys in vals)
{
dp.Invoke(cb, keys.Key);
}
}
else
{
FRCDSStatus status = ParseDSBytes(buffer);
cStat = status;
kys = vals;
//Fire events
if (DataRecieved != null)
DataRecieved(this, new EventArgs());
if (NewStatusRecieved != null)
NewStatusRecieved(this, new NewStatusRecievedEventArgs(status));
if (NewDataRecieved != null)
NewDataRecieved(this, new NewDataRecievedEventArgs(vals));
}
}
}
if (nume > 0)
nume--;
}
catch (ThreadAbortException)
{
isrunning = false;
cRIOConnection.Close();
return;
}
catch (Exception ex)
{
nume++;
DoError(ex);
if (nume > 10)
{
isrunning = false;
DoError(new Exception("10 consecutive errors were encountered, stopping DashboardDataHub"));
isrunning = false;
try
{
cRIOConnection.Close();
}
catch { }
return;
}
}
}
}
