public MoveCommand(Vector3i location, UInt16 time_milliseconds)
 {
     this.x = (Int32)location.x;
     this.y = (Int32)location.y;
     this.z = (Int32)location.z;
     this.time_milliseconds = time_milliseconds;
     data_valid = false;
 }
        public override void ProcessResponse(byte[] data)
        {
            data_valid = (data[0] == commandCode);
            if (data.Length <= 14)
            {
                data_valid = false;
                return;
            }

            Int32 x = 0;
            Int32 y = 0;
            Int32 z = 0;
            for (int i = 4; i >= 1; i--)
            {
                x = (x << 8) | data[i];
                y = (y << 8) | data[i + 4];
                z = (z << 8) | data[i + 8];
            }

            byte status_bits = data[13];
            bool x_moving = (status_bits & 0x01) > 0;
            bool y_moving = (status_bits & 0x02) > 0;
            bool z_moving = (status_bits & 0x04) > 0;

            is_moving = x_moving | y_moving | z_moving;
            locations = data[14];
            currentPosition = new Vector3i(x, y, z);

            //Console.WriteLine("Location = ({0}, {1}, {2}), Moving = ({3}, {4}, {5})", x, y, z, x_moving, y_moving, z_moving);
        }
Exemple #3
0
        /// <summary>
        /// Run the router from the current position to the given position
        /// </summary>
        /// <param name="p">Destination location in inches</param>
        /// <param name="inches_per_minute">Tool speed in inches per second</param>
        public override void GoTo(Vector3 p, float inches_per_minute)
        {
            lock (thisLock)
            {
                Vector3 delta = lastPosition - p;

                // Z axis has a slower maximum speed.
                // TODO: implement clamping to a maximum speed for each axis.
                delta.Z = delta.Z * 10;

                float inches = delta.Length;

                Vector3i pointInt = new Vector3i(Vector3.Multiply(p, ScaleFactors));

                UInt16 time_milliseconds = (UInt16)(1000 * 60 * inches / inches_per_minute);

                if (time_milliseconds > 0)
                {

                    Console.WriteLine("Moving ({0}, {1}, {2}) to ({3}, {4}, {5}) in {6} milliseconds",
                        lastPosition.X,
                        lastPosition.Y,
                        lastPosition.Z,
                        pointInt.x,
                        pointInt.y,
                        pointInt.z,
                        time_milliseconds);

                    lastPosition = p;

                    currentCommand = new MoveCommand(pointInt, time_milliseconds);
                }
                else
                {
                    Console.WriteLine("Ignoring command with time of 0");
                }
            }
        }