static void Main(string[] args)
{
Log.SetGlobalOutputLevel(Log.Severity.INFO);
Log.SetSingleOutputLevel(Log.Source.NETWORK, Log.Severity.DEBUG);
Log.ErrorCodes = ScienceErrors.ERROR_CODES;
Log.SystemNames = ScienceErrors.SYSTEMS;
Log.Begin();
Log.ForceOutput(Log.Severity.INFO, Log.Source.OTHER, "Science Station - Base Side");
byte[] StrB = UtilData.ToBytes("Erza-{}'");
byte[] Fail = new byte[] { 0x8C };
bool Success = UtilData.TryToString(Fail, out string Str);
if (Success)
{
Console.WriteLine(Str);
}
else
{
Console.WriteLine(UtilMain.BytesToNiceString(StrB, true));
}
MainWindow Main = new MainWindow();
Application.EnableVisualStyles();
Server.ClientConnectionChange += Main.UpdateClientList;
Application.Run(Main);
}
/// <summary>
/// Sends an error packet to the given
/// endpoint via TCP.
/// Adds Error packet to packet send queue
/// </summary>
/// <param name="ErrorPacketID">ID for the Error Packet</param>
/// <param name="Endpoint">Endpoint to send the packet</param>
/// <param name="ErrorCode">Error code for the indicated error</param>
public static void SendError(byte ErrorPacketID, string Endpoint, int ErrorCode)
{
Packet ErrorPacket = new Packet(ErrorPacketID, false, Endpoint);
ErrorPacket.AppendData(UtilData.ToBytes(ErrorCode));
Send(ErrorPacket);
}
/// <summary> Gets the current time as a byte array for use in packets. </summary>
public static byte[] GetCurrentTime()
{
// We can't use this because it requires .NET 4.6, which isn't present on the version of Mono on the BeagleBone.
// int UnixTime = (int)DateTimeOffset.Now.ToUnixTimeSeconds();
int UnixTime = (int)(DateTime.UtcNow.Subtract(new DateTime(1970, 1, 1))).TotalSeconds;
byte[] TimeArray = UtilData.ToBytes(UnixTime);
return(TimeArray);
}
public static void SampleDoorChange(bool NewValue)
{
Packet Packet = new Packet(new Message(ScienceConstants.Packets.SERVO_SET, new byte[]
{
0x00,
}.Concat(UtilData.ToBytes((int)(NewValue ? 0x01 : 0x00))).ToArray()), false, ScienceConstants.CLIENT_NAME);
Server.Send(Packet);
}
public void UpdateState()
{
if (this.TakeReadings)
{
DateTime Sample = DateTime.Now;
this.SystemSensor.UpdateState();
this.DrillSensor.UpdateState();
this.RailSensor.UpdateState();
double RailA = this.RailSensor.GetShuntVoltage();
double Drill = this.DrillSensor.GetCurrent();
double SysA = this.SystemSensor.GetCurrent();
double SysV = this.SystemSensor.GetBusVoltage();
double SysSV = this.SystemSensor.GetShuntVoltage();
double DrlSV = this.DrillSensor.GetShuntVoltage();
if (this.TraceLogging)
{
Log.Trace(this, string.Format("Sys: {0:N5}A, {1:N5}V (Shunt {2:N5}V). Drill: {3:N5}A. Rail: {4:N5}A. Test: {5}", SysA, SysV, SysSV, Drill, RailA, this.SystemSensor.Test()));
}
byte[] Data = UtilData.ToBytes(SysSV / 0.150).Concat(UtilData.ToBytes(DrlSV / 0.010)).Concat(UtilData.ToBytes(RailA / 0.002)).Concat(UtilData.ToBytes(SysV)).Concat(UtilData.ToBytes(Sample.Ticks)).ToArray(); // TODO: Decide if we want to calculate or use device
Packet Packet = new Packet(new Message(ScienceConstants.Packets.SYS_SENSOR, Data), false);
Client.Send(Packet);
uint SysVoltColour;
if (SysV <= 25)
{
SysVoltColour = RGBLED.RedGreenGradient(SysV, 22, 26);
}
else if (SysV <= 28)
{
SysVoltColour = 0x00FF00;
}
else
{
SysVoltColour = RGBLED.RedGreenGradient(SysV, 30, 28);
}
RoverMain.IOHandler.LEDController.SystemVoltage.SetOutput(SysVoltColour);
uint SysCurrentColour;
if (SysA <= 2)
{
SysCurrentColour = 0x00FF00;
}
else if (SysA >= 5)
{
SysCurrentColour = 0xFF0000;
}
else
{
SysCurrentColour = RGBLED.RedGreenGradient(SysA, 5, 2);
}
RoverMain.IOHandler.LEDController.SystemCurrent.SetOutput(SysCurrentColour);
}
}
private void EmergencyStopClick(object sender, EventArgs e)
{
try
{
Packet EmergencyStopPacket = new Packet(PacketType.EMERGENCY_STOP, false, Scarlet.Science.Constants.CLIENT_NAME);
EmergencyStopPacket.AppendData(UtilData.ToBytes("Homura"));
Server.SendNow(EmergencyStopPacket);
}
catch (Exception Exc)
{
Log.Output(Log.Severity.FATAL, Log.Source.GUI, "FAILED TO SEND EMERGENCY STOP!");
Log.Exception(Log.Source.GUI, Exc);
DarkMessageBox.ShowError("Failed to send emergency stop!\n\n" + Exc.ToString(), "Science");
}
}
/// <summary> Sends the speed between -1.0 and 1.0 to the motor controller </summary>
/// <param name="Speed"> Speed from -1.0 to 1.0 </param>
private void SendSpeed(float Speed)
{
byte[] SpeedArray = UtilData.ToBytes((int)(Speed * 100000.0f));
if (this.IsCAN)
{
this.CANBus.Write(this.CANID, SpeedArray);
}
else
{
List <byte> payload = new List <byte>();
payload.Add((byte)UARTPacketID.SET_DUTY);
payload.AddRange(SpeedArray);
// Duty Cycle (100000.0 mysterious magic number from https://github.com/VTAstrobotics/VESC_BBB_UART/blob/master/bldc_interface.c)
this.UARTBus.Write(ConstructPacket(payload));
}
}
/// <summary> Sends the speed to the motor controller </summary>
/// <param name="RPM"> RPM for the motor to spin at. RPM is capped at 36500 </param>
private void SendRPM(int RPM)
{
RPM = Math.Min(RPM, MOTOR_MAX_RPM) * ERPM_PER_RPM;
byte[] SpeedArray = UtilData.ToBytes(RPM);
if (this.IsCAN)
{
this.CANBus.Write(((byte)CANPacketID.CAN_PACKET_SET_RPM << 8) | this.CANID, SpeedArray);
}
else
{
List <byte> payload = new List <byte>();
payload.Add((byte)UARTPacketID.SET_RPM);
payload.AddRange(SpeedArray);
this.UARTBus.Write(ConstructPacket(payload));
}
}
public static void SendSensorData(int count)
{
foreach (ISensor Sensor in Sensors)
{
if (Sensor is MTK3339)
{
var Tup = ((MTK3339)Sensor).GetCoords();
float Lat = Tup.Item1;
float Long = Tup.Item2;
Packet Pack = new Packet((byte)PacketID.DataGPS, true);
Pack.AppendData(UtilData.ToBytes(Lat));
Pack.AppendData(UtilData.ToBytes(Long));
Client.SendNow(Pack);
if (count == 100)
{
Packet HeadingFromGPSPack = new Packet((byte)PacketID.HeadingFromGPS, true);
//Math between two coords given from Tup and previousCoords
float latDiff = Lat - previousCoords.Item1;
float longDiff = Long - previousCoords.Item1;
float theta = (float)Math.Atan2(latDiff, longDiff);
if (longDiff > 0)
{
theta = 90 - theta;
}
else if (longDiff < 0)
{
theta = 270 - theta;
}
HeadingFromGPSPack.AppendData(UtilData.ToBytes(theta));
Client.SendNow(HeadingFromGPSPack);
previousCoords = Tup;
}
}
if (Sensor is BNO055)
{
var Tup = ((BNO055)Sensor).GetVector(BNO055.VectorType.VECTOR_MAGNETOMETER);
float X = Tup.Item1;
float Y = Tup.Item2;
float Z = Tup.Item3;
Packet Pack = new Packet((byte)PacketID.DataMagnetometer, true);
Pack.AppendData(UtilData.ToBytes(X));
Pack.AppendData(UtilData.ToBytes(Y));
Pack.AppendData(UtilData.ToBytes(Z));
Client.SendNow(Pack);
}
}
}
private byte[] InterpretInput(char[] Input)
{
if (!Input.Contains('"'))
{
return(UtilMain.StringToBytes(new string(Input)).Reverse().ToArray());
}
int LocOfStart = Array.IndexOf(Input, '"');
int StrLen = Array.IndexOf(Input.Skip(LocOfStart + 1).ToArray(), '"');
char[] BeforeChars = Input.Take(LocOfStart).ToArray();
List <byte> Output = new List <byte>();
Output.AddRange(UtilMain.StringToBytes(new string(BeforeChars)).Reverse().ToArray());
string Str = new string(Input.Skip(LocOfStart + 1).Take(StrLen).ToArray());
Output.AddRange(UtilData.ToBytes(Str));
Output.AddRange(InterpretInput(Input.Skip(LocOfStart + StrLen + 2).ToArray()));
return(Output.ToArray());
}
public void UpdateState()
{
if (this.TakeReadings)
{
DateTime Sample = DateTime.Now;
this.Thermocouple.UpdateState();
if (this.Thermocouple.GetFaults() != MAX31855.Fault.NONE)
{
Log.Output(Log.Severity.WARNING, Log.Source.SENSORS, "Thermocouple has faults: " + this.Thermocouple.GetFaults());
}
this.UVLight.UpdateState();
this.Atmospheric.UpdateState();
this.AirQuality.UpdateState();
this.SoilMoisture.UpdateState();
if (this.TraceLogging)
{
Log.Trace(this, "Thermocouple: int " + this.Thermocouple.GetInternalTemp() + " ext " + this.Thermocouple.GetExternalTemp() + " faults " + this.Thermocouple.GetFaults());
}
//Log.Output(Log.Severity.INFO, Log.Source.SENSORS, "Temp: " + this.Atmospheric.Temperature + ", press: " + this.Atmospheric.Pressure + ", humid: " + this.Atmospheric.Humidity + ", on: " + this.Atmospheric.Test());
byte[] Data = UtilData.ToBytes(this.UVLight.GetReading())
.Concat(UtilData.ToBytes(this.AirQuality.GetReadingUncalibrated()))
.Concat(UtilData.ToBytes(this.SoilMoisture.GetReading()))
.Concat(UtilData.ToBytes(this.Thermocouple.GetExternalTemp()))
.Concat(UtilData.ToBytes(this.Atmospheric.Temperature))
.Concat(UtilData.ToBytes(this.Atmospheric.Pressure))
.Concat(UtilData.ToBytes(this.Atmospheric.Humidity))
.ToArray();
if (this.TraceLogging)
{
Log.Trace(this, "UV: " + this.UVLight.GetReading() + ", AirQ: " + this.AirQuality.GetReadingUncalibrated() + ", Soil: " + this.SoilMoisture.GetReading() + ", AirTemp: " + this.Atmospheric.Temperature);
}
Packet Packet = new Packet(new Message(ScienceConstants.Packets.GND_SENSOR, Data), false);
Client.Send(Packet);
}
}
public void BasicTestQueueBuffer()
{
String TestText1 = "Hello, World!";
String TestText2 = "hello, world";
Byte ChannelID1 = 0xcd;
Byte ChannelID2 = 0xcf;
InitializeConnectionWithQueueBuffer();
Parse.SetParseHandler(ChannelID1, MessageHandler);
Parse.SetParseHandler(ChannelID2, MessageHandler2);
Packet MyPack = new Packet(new Message(ChannelID1, UtilData.ToBytes(TestText1)), false);
Client.Send(MyPack);
System.Threading.Thread.Sleep(200);
Assert.AreEqual(TestText1, ReceivedMessage);
Packet MyPack2 = new Packet(new Message(ChannelID2, UtilData.ToBytes(TestText2)), false, "TestClient");
Server.Send(MyPack2);
System.Threading.Thread.Sleep(200);
Assert.AreEqual(TestText2, ReceivedMessage2);
}
/// <summary> Generates the packet for the motor controller: </summary>
/// <remarks>
/// One Start byte (value 2 for short packets and 3 for long packets)
/// One or two bytes specifying the packet length
/// The payload of the packet
/// Two bytes with a CRC checksum on the payload
/// One stop byte (value 3)
/// </remarks>
/// <param name="Speed"> Speed from -1.0 to 1.0 </param>
private byte[] ConstructPacket(List <byte> Payload)
{
List <byte> Packet = new List <byte>();
Packet.Add(2); // Start byte (short packet - payload <= 256 bytes)
if (this.CANForwardID >= 0)
{
Payload.Add((byte)UARTPacketID.FORWARD_CAN);
Payload.Add((byte)CANForwardID);
}
Packet.Add((byte)Payload.Count); // Length of payload
Packet.AddRange(Payload); // Payload
ushort Checksum = UtilData.CRC16(Payload.ToArray());
Packet.AddRange(UtilData.ToBytes(Checksum)); // Checksum
Packet.Add(3); // Stop byte
return(Packet.ToArray());
}
/// <summary> Constructs a message given data that is already split. </summary>
/// <param name="ID"> The packet ID, used to determine how it is handled at the recipient. </param>
/// <param name="Payload"> The packet's data content. Will be converted to bytes for you. </param>
/// <param name="Timestamp"> The timestamp of the packet. If null or invalid, the current time gets set. </param>
public Message(byte ID, string Payload = null, byte[] Timestamp = null) : this(ID, UtilData.ToBytes(Payload), Timestamp)
{
}
/// <summary>
/// Assumes TCP and UCP clients are connected.
/// Sends name to initialize a connection with server.
/// </summary>
private static void SendNames()
{
byte[] SendData = UtilData.ToBytes(Name);
ServerUDP.Client.Send(SendData);
ServerTCP.Client.Send(SendData);
}
public PacketWriter Put(string data)
{
this.Packet.Data.AppendData(UtilData.ToBytes(data)); return(this);
}
public static void RailTargetChange(bool FromTop, float TargetDist)
{
byte Command;
if (FromTop && float.IsNaN(TargetDist))
{
Command = 0x00;
}
else if (!FromTop && float.IsNaN(TargetDist))
{
Command = 0x01;
}
else
{
Command = (byte)(FromTop ? 0x02 : 0x03);
}
Packet Packet = new Packet(new Message(ScienceConstants.Packets.RAIL_TARGET_SET, new byte[] { Command }.Concat(UtilData.ToBytes(TargetDist)).ToArray()), false, ScienceConstants.CLIENT_NAME);
Server.Send(Packet);
}
