Example #1
0
        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;
                }
            }
        }
Example #2
0
        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));
        }
Example #3
0
        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;
                }
            }
        }
Example #4
0
        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));
        }
Example #5
0
        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;
                }
            }
        }
Example #6
0
 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);
         });
     }
 }
Example #7
0
        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;
                }
            }
        }
Example #8
0
 /// <summary> Interperate next data block as float. </summary>
 /// <returns> Next data. </returns>
 public float  NextFloat()
 {
     return(UtilData.ToFloat(NextData(sizeof(float))));
 }