public static void ParseRailTargetPacket(Packet Packet)
{
if (CheckPacket(Packet, 5, "Rail Target"))
{
float TargetDist = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 1, 4));
switch (Packet.Data.Payload[0])
{
case 0x00:
RoverMain.IOHandler.RailController.GotoTop();
break;
case 0x01:
RoverMain.IOHandler.RailController.GotoDrillGround();
break;
case 0x02:
RoverMain.IOHandler.RailController.TargetLocation = TargetDist;
RoverMain.IOHandler.RailController.TargetLocationRefIsTop = true;
break;
case 0x03:
RoverMain.IOHandler.RailController.TargetLocation = TargetDist;
RoverMain.IOHandler.RailController.TargetLocationRefIsTop = false;
break;
}
}
}
public static void PacketGroundSensor(Packet Packet)
{
if (Packet == null || Packet.Data == null || Packet.Data.Payload == null || Packet.Data.Payload.Length != 40)
{
Log.Output(Log.Severity.WARNING, Log.Source.NETWORK, "Ground sensor packet invalid. Discarding. Length: " + Packet?.Data?.Payload?.Length);
return;
}
DateTime Timestamp = DateTime.Now;
int UVLight = UtilData.ToInt(UtilMain.SubArray(Packet.Data.Payload, 0, 4));
float AirQuality = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 4, 4));
float SoilMoist = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 8, 4));
uint ThermocoupleData = UtilData.ToUInt(UtilMain.SubArray(Packet.Data.Payload, 12, 4));
double AtmoTemp = UtilData.ToDouble(UtilMain.SubArray(Packet.Data.Payload, 16, 8));
double AtmoPres = UtilData.ToDouble(UtilMain.SubArray(Packet.Data.Payload, 24, 8));
double AtmoHumid = UtilData.ToDouble(UtilMain.SubArray(Packet.Data.Payload, 32, 8));
UV.Data.Add(new Datum <int>(Timestamp, UVLight));
AirPollution.Data.Add(new Datum <float>(Timestamp, AirQuality));
SoilMoisture.Data.Add(new Datum <float>(Timestamp, SoilMoist));
ThermoExt.Data.Add(new Datum <float>(Timestamp, MAX31855.ConvertExternalFromRaw(ThermocoupleData)));
ThermoInt.Data.Add(new Datum <float>(Timestamp, MAX31855.ConvertInternalFromRaw(ThermocoupleData)));
AirTemp.Data.Add(new Datum <double>(Timestamp, AtmoTemp));
AirPressure.Data.Add(new Datum <double>(Timestamp, AtmoPres));
AirHumidity.Data.Add(new Datum <double>(Timestamp, AtmoHumid));
}
public static void ProcessBasePackets()
{
for (int i = 0; !DrivePackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = DrivePackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
case PacketID.RPMAllDriveMotors:
MotorControl.SetAllRPM((sbyte)p.Data.Payload[0]);
break;
case PacketID.RPMFrontRight:
case PacketID.RPMFrontLeft:
case PacketID.RPMBackRight:
case PacketID.RPMBackLeft:
int MotorID = p.Data.ID - (byte)PacketID.RPMFrontRight;
MotorControl.SetRPM(MotorID, (sbyte)p.Data.Payload[1]);
break;
case PacketID.RPMSteeringMotor:
float SteerSpeed = UtilData.ToFloat(p.Data.Payload);
//MotorControl.SetSteerSpeed(SteerSpeed);
break;
case PacketID.SteerPosition:
float Position = UtilData.ToFloat(p.Data.Payload);
//MotorControl.SetRackAndPinionPosition(Position);
break;
case PacketID.SpeedAllDriveMotors:
float Speed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SetAllSpeed(Speed);
break;
case PacketID.BaseSpeed:
case PacketID.ShoulderSpeed:
case PacketID.ElbowSpeed:
case PacketID.WristSpeed:
case PacketID.DifferentialVert:
case PacketID.DifferentialRotate:
case PacketID.HandGrip:
byte address = (byte)(p.Data.ID - 0x8A);
byte direction = 0x00;
if (p.Data.Payload[0] > 0)
{
direction = 0x01;
}
UtilCan.SpeedDir(CANBBB.CANBus0, false, 0x02, address, p.Data.Payload[1], direction);
break;
}
}
}
public static void PacketRailStatus(Packet Packet)
{
if (Packet == null || Packet.Data == null || Packet.Data.Payload == null || Packet.Data.Payload.Length != 17)
{
Log.Output(Log.Severity.WARNING, Log.Source.NETWORK, "Rail status packet invalid. Discarding. Length: " + Packet?.Data?.Payload?.Length);
return;
}
byte Status = Packet.Data.Payload[0];
float RailSpeed = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 1, 4));
float DepthTop = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 5, 4));
float HeightGround = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 9, 4));
float Target = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 13, 4));
BaseMain.Window.UpdateRail(RailSpeed, DepthTop, HeightGround, Target, ((Status & 0b100) == 0b100), ((Status & 0b10) == 0b10), ((Status & 0b1) == 0b1));
}
public static void ProcessPathPackets()
{
for (int i = 0; !PathPackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = PathPackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
// TODO Maybe: Combine pathing speed and turn in same packet???
case PacketID.PathingSpeed:
PathSpeed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SkidSteerDriveSpeed(PathSpeed, PathAngle);
break;
case PacketID.PathingTurnAngle:
PathAngle = UtilData.ToFloat(p.Data.Payload);
MotorControl.SkidSteerDriveSpeed(PathSpeed, PathAngle);
break;
}
}
}
static void SetupClient()
{
Client.Start(SERVER_IP, 1025, 1026, "ArmMaster");
Parse.SetParseHandler(0x80, (Pack) =>
{
MotorControl.SetAllMotorSpeed(0.0f);
Slave.Write(new byte[] { 0x80 });
});
for (byte i = 0x9A; i <= 0x9C; i++)
{
Parse.SetParseHandler(i, (Packet) =>
{
MotorControl.SetMotorSpeed(Packet.Data.ID - 0x9A, UtilData.ToFloat(Packet.Data.Payload));
});
}
for (byte i = 0x9D; i <= 0xA0; i++)
{
Parse.SetParseHandler(i, (Pack) =>
{
Slave.Write(new byte[] { Pack.Data.ID });
Slave.Write(Pack.Data.Payload);
});
}
}
public static void ProcessBasePackets()
{
for (int i = 0; !DrivePackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Console.WriteLine("Processing Base Packets");
Packet p = DrivePackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
//case PacketID.RPMAllDriveMotors:
// MotorControl.SetAllRPM((sbyte)p.Data.Payload[0]);
// break;
case PacketID.RPMFrontRight:
case PacketID.RPMFrontLeft:
case PacketID.RPMBackRight:
case PacketID.RPMBackLeft:
int MotorID = p.Data.ID - (byte)PacketID.RPMFrontRight;
MotorControl.SetRPM(MotorID, (sbyte)p.Data.Payload[1]);
break;
case PacketID.SpeedAllDriveMotors:
float Speed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SetAllSpeed(Speed);
break;
case PacketID.BaseSpeed:
case PacketID.ShoulderSpeed:
case PacketID.ElbowSpeed:
case PacketID.WristSpeed:
case PacketID.DifferentialVert:
case PacketID.DifferentialRotate:
case PacketID.HandGrip:
byte address = (byte)(p.Data.ID - 0x8A);
byte direction = 0x00;
if (p.Data.Payload[0] > 0)
{
direction = 0x01;
//UtilCan.SpeedDir(CANBBB.CANBus0, false, 2, address, (byte)(-p.Data.Payload[1]), direction);
Console.WriteLine("ADDRESS :" + address + "DIR :" + direction + "PAY :" + (byte)(-p.Data.Payload[1]));
}
else
{
direction = 0x00;
//UtilCan.SpeedDir(CANBBB.CANBus0, false, 2, address, p.Data.Payload[1], direction);
Console.WriteLine("ADDRESS :" + address + "DIR :" + direction + "PAY :" + p.Data.Payload[1]);
}
break;
case PacketID.CameraRotation:
if ((sbyte)p.Data.Payload[1] > 0)
{
ServoSpinner -= 0.005f;
OutB.SetOutput(ServoSpinner);
}
else if ((sbyte)p.Data.Payload[1] < 0)
{
ServoSpinner += 0.005f;
OutB.SetOutput(ServoSpinner);
}
OutB.Dispose();
break;
}
}
}
/// <summary> Interperate next data block as float. </summary>
/// <returns> Next data. </returns>
public float NextFloat()
{
return(UtilData.ToFloat(NextData(sizeof(float))));
}
