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