Ejemplo n.º 1
0
        // Starts the specified servo move using the specified speed.
        private void StartSpeedMove(ServoPosition servoPosition, long servoSpeed)
        {
            Servo servo = servoList.Find(x => x.index == servoPosition.index);

            SetServoSpeed(servo, servoSpeed);
            SetServoPosition(servo, servoPosition.position);
        }
Ejemplo n.º 2
0
        // Sets the speed of the specified servo, in the servo controller hardware. This speed
        // value is first bounded within the servo's min/max speed limits, and a warning is
        // logged if a speed outside of these limits was specified.
        private void SetServoSpeed(Servo servo, long speed)
        {
            if (speed < servo.speedLimitMin)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to minimum limit " + servo.speedLimitMin.ToString());

                // Bound to this limit.
                speed = servo.speedLimitMin;
            }

            if (speed > servo.speedLimitMax)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to maximum limit " + servo.speedLimitMax.ToString());

                // Bound to this limit.
                speed = servo.speedLimitMax;
            }

            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " speed to " + speed.ToString());

            try
            {
                // Send this value to the hardware.
                uscDevice.setSpeed((byte)servo.index, (ushort)speed);
            }
            catch (System.Exception ex)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoSpeed(): " + ex.Message);
            }
        }
Ejemplo n.º 3
0
        // Starts the specified servo move using the specified speed. StartSpeedMove is used by PersonTracking
        // and RobotSpeech, which are lower priority moves than those issued through Sequences. StartSpeedMove
        // motion can be interrupted by Sequence moves, and new StartSpeedMove moves are ignored if a Sequence
        // is already moving this servo.
        public void StartSpeedMove(ServoPosition servoPosition, long servoSpeed)
        {
            if (!IsEnabled())
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove failed, servos are disabled.");
                return;
            }

            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove moving servo " + servoPosition.index.ToString() + " to position " + servoPosition.position.ToString() + " at speed " + servoSpeed.ToString() + ".");

            Servo servo = servoList.Find(x => x.index == servoPosition.index);

            lock (servo.movePriorityLock)
            {
                if (servo.isRunningHighPriorityMove)
                {
                    // Ignore this command if a higher priority (i.e. Sequence) move is already running.

                    ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "StartSpeedMove, isRunningHighPriorityMove is set for servo " + servo.index.ToString() + ". Aborting move.");
                    return;
                }

                if (!IsConnected())
                {
                    return;
                }

                SetServoSpeed(servo, servoSpeed);
                SetServoPosition(servo, servoPosition.position);
            }
        }
Ejemplo n.º 4
0
        // Sets the target position of the specified servo, causing this servo to begin moving
        // to the new position. This target position is first bounded within the servo's min/max
        // position limits, and a warning is logged if a position outside of these limits was
        // specified.
        private void SetServoPosition(Servo servo, double position)
        {
            if (position < servo.positionLimitMin)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to minimum limit " + servo.positionLimitMin.ToString());

                // Bound to this limit.
                position = servo.positionLimitMin;
            }

            if (position > servo.positionLimitMax)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to maximum limit " + servo.positionLimitMax.ToString());

                // Bound to this limit.
                position = servo.positionLimitMax;
            }

            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " position to " + position.ToString());

            try
            {
                // Send this value to the hardware.
                // Note that the servo position values are handled in this class in units of μs,
                // to match the convention used by Pololu's Maestro Control Center application.
                // However, the servo controller hardware expects the position represented as an
                // integer value in 0.25 μs. The local value must be multiplied by 4 to convert
                // to these units.
                uscDevice.setTarget((byte)servo.index, (ushort)(position * 4));
            }
            catch (System.Exception ex)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoPosition(): " + ex.Message);
            }
        }
Ejemplo n.º 5
0
        // Starts performing all moves specified by servoPositionList, and then waits for these
        // servos to finish moving.
        public void MoveServos(List <ServoPosition> servoPositionList, double timeToDestination)
        {
            if (!IsEnabled())
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveServos failed, servos are disabled.");
                return;
            }

            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos moving servos over " + timeToDestination + " seconds.");

            foreach (ServoPosition servoPosition in servoPositionList)
            {
                Servo servo = servoList.Find(x => x.index == servoPosition.index);

                lock (servo.movePriorityLock)
                {
                    ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos set isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
                    servo.isRunningHighPriorityMove = true;
                }
            }

            if (!IsConnected())
            {
                // Simulate the move.
                Thread.Sleep((int)(timeToDestination * 1000));

                foreach (ServoPosition servoPosition in servoPositionList)
                {
                    Servo servo = servoList.Find(x => x.index == servoPosition.index);

                    lock (servo.movePriorityLock)
                    {
                        ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos cleared isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
                        servo.isRunningHighPriorityMove = false;
                    }
                }

                return;
            }

            foreach (ServoPosition servoPosition in servoPositionList)
            {
                StartTimedMove(servoPosition, timeToDestination);
            }

            WaitForMoveComplete(servoPositionList, timeToDestination);

            foreach (ServoPosition servoPosition in servoPositionList)
            {
                Servo servo = servoList.Find(x => x.index == servoPosition.index);

                lock (servo.movePriorityLock)
                {
                    ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "MoveServos cleared isRunningHighPriorityMove for servo " + servo.index.ToString() + ".");
                    servo.isRunningHighPriorityMove = false;
                }
            }
        }
Ejemplo n.º 6
0
        // Polls the servo hardware to determine if the servos are still moving. Returns when all
        // servos in servoPositionList have completed their moves.
        private void WaitForMoveComplete(List <ServoPosition> servoPositionList, double timeToDestination)
        {
            // Create a list of servos to monitor.
            List <Servo> servosToMonitor = new List <Servo>();

            foreach (ServoPosition servoPosition in servoPositionList)
            {
                Servo servo = servoList.Find(x => x.index == servoPosition.index);

                if (servo == null)
                {
                    ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "WaitForMoveComplete failed, servo " + servoPosition.index.ToString() + " not found.");
                    return;
                }

                servosToMonitor.Add(servo);
            }


            // Poll servo positions and wait until all servos reach their destinations.
            double pollTimeout      = timeToDestination + pollTimeoutAdjustment;
            int    pollTimeoutCount = (int)(pollTimeout * 1000 / (double)pollPeriod_ms);
            int    currentPollCount = 0;

            while (true)
            {
                if (currentPollCount >= pollTimeoutCount)
                {
                    ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "WaitForMoveComplete timeout, servos failed to reach destination in " + pollTimeout.ToString() + " seconds.");
                    return;
                }

                currentPollCount++;

                UpdateServoValues();

                // Determine if any servos in the list are still moving.
                bool servoIsMoving = false;
                foreach (Servo servo in servosToMonitor)
                {
                    if (servo.isMoving)
                    {
                        servoIsMoving = true;
                    }
                }

                if (!servoIsMoving)
                {
                    ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "WaitForMoveComplete succeeded, all servos reached destinations.");
                    return;
                }

                Thread.Sleep(pollPeriod_ms);
            }
        }
Ejemplo n.º 7
0
        // Sets the acceleration of the specified servo, in the servo controller hardware.
        private void SetServoAcceleration(Servo servo, long acceleration)
        {
            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " acceleration to " + acceleration.ToString());

            try
            {
                // Send this value to the hardware.
                uscDevice.setAcceleration((byte)servo.index, (ushort)acceleration);
            }
            catch (System.Exception ex)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoAcceleration(): " + ex.Message);
            }
        }
Ejemplo n.º 8
0
        public long GetServoIndex(string servoName)
        {
            Servo servo = servoList.Find(x => x.name == servoName);

            if (servo != null)
            {
                return(servo.index);
            }
            else
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "GetServoIndex() failed to find servo '" + servoName + "'.");
                return(-1);
            }
        }
Ejemplo n.º 9
0
        // Calculates the required servo speed to reach the position specified by servoPosition,
        // in the amount of time indicated by timeToDestination. Then starts this move and returns.
        private void StartTimedMove(ServoPosition servoPosition, double timeToDestination)
        {
            Servo  servo      = servoList.Find(x => x.index == servoPosition.index);
            ushort servoSpeed = 0;

            if (servo == null)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "StartTimedMove failed, servo " + servoPosition.index.ToString() + " not found.");
                return;
            }

            if (servo.polledPosition == 0)
            {
                // If the servo position is 0, then the servo is off and we have no information about its actual position.
                // We don't know the actual move distance, so just use the default servo speed.

                servoSpeed = (ushort)servo.defaultSpeed;
            }
            else
            {
                // Convert current position to μs (the hardware uses 0.25 μs increments).
                double currentPosition = ((double)servo.polledPosition) / 4;

                // Position difference in μs.
                double positionDifference = Math.Abs(servoPosition.position - currentPosition);

                // Required speed in (μs/second).
                double calculatedSpeed;
                if (timeToDestination != 0)
                {
                    calculatedSpeed = positionDifference / timeToDestination;
                }
                else
                {
                    // If the desired move time is instantaneous, use the max allowed servo speed.
                    calculatedSpeed = servo.speedLimitMax;
                }

                // Convert speed from (1 μs / second) to (0.25 μs / 10 ms), used by the hardware.
                servoSpeed = (ushort)(calculatedSpeed * (4.0 / 100.0));
            }

            SetServoSpeed(servo, servoSpeed);

            SetServoPosition(servo, servoPosition.position);
        }
Ejemplo n.º 10
0
        // Creates an example servo configuration xml file to provide a template for creating
        // a complete servo configuration file.
        public void GenerateExampleConfigFile(string fileName)
        {
            Servo exampleServo = new Servo();

            exampleServo.index            = 0;
            exampleServo.name             = "Example Servo";
            exampleServo.positionLimitMax = 0;
            exampleServo.positionLimitMin = 0;
            exampleServo.speedLimitMax    = 0;
            exampleServo.speedLimitMin    = 0;

            List <Servo> exampleServoList = new List <Servo>();

            exampleServoList.Add(exampleServo);

            XmlSerializer SerializerObj   = new XmlSerializer(typeof(List <Servo>));
            TextWriter    WriteFileStream = new StreamWriter(fileName);

            SerializerObj.Serialize(WriteFileStream, exampleServoList);
            WriteFileStream.Close();
        }
Ejemplo n.º 11
0
        // Moves the robot's head/eyes to track the specified skeleton joint.
        public void PersonTrackingUpdate(SkeletonPoint targetPosition)
        {
            // Only update if tracking is allowed and if we're not already updating in a
            // different thread.
            lock (personTrackingLock)
            {
                if (!trackingEnabled)
                {
                    return;
                }

                if (isAlreadyUpdatingTracking)
                {
                    return;
                }
                else
                {
                    isAlreadyUpdatingTracking = true;
                }
            }

            // Only update once every 100ms, even if we're receiving new joint positions
            // more frequently than this.
            TimeSpan timeBetweenUpdates = new TimeSpan(0, 0, 0, 0, 100);

            if (DateTime.Now - lastUpdateTime < timeBetweenUpdates)
            {
                lock (personTrackingLock)
                {
                    isAlreadyUpdatingTracking = false;
                }

                return;
            }

            lastUpdateTime = DateTime.Now;

            // Position of the robot neck and eye servos at which the robot faces straight forward.
            const double servoCenterPostion_HeadHorizontal = 1550;
            const double servoCenterPostion_HeadVertical   = 1500;
            const double servoCenterPostion_Eyes           = 1600;

            const double servoOffset_HeadVertical = 0;

            // Number of servo position increments per radian of rotation for each of the servos.
            const double servoIncrementsPerRadian_HeadHorizontal = 800;
            const double servoIncrementsPerRadian_HeadVertical   = 1000;
            const double servoIncrementsPerRadian_Eyes           = 800;

            // Tracking speed of each servo.
            const long servoSpeed_HeadHorizontal = 30;
            const long servoSpeed_HeadVertical   = 30;
            const long servoSpeed_Eyes           = 1000;

            if (IsConnected())
            {
                UpdateServoValues();
            }

            Servo headHorizontalServo = servoList.Find(x => x.index == 10);

            if (headHorizontalServo == null)
            {
                lock (personTrackingLock)
                {
                    isAlreadyUpdatingTracking = false;
                }

                // Log Error
                return;
            }

            // Calculate the current position of the head.
            double currentPosition_HeadHorizontal = headHorizontalServo.polledPosition / 4.0;
            double currentAngle_HeadHorizontal    = (servoCenterPostion_HeadHorizontal - currentPosition_HeadHorizontal) / servoIncrementsPerRadian_HeadHorizontal;

            // Calculate the angle to the target joint.
            double targetAngle_Horizontal = Math.Atan(targetPosition.X / targetPosition.Z);
            double targetAngle_Vertical   = Math.Atan(targetPosition.Y / targetPosition.Z);

            // Calculate the new head position to face this target joint.
            double newServoPosition_HeadHorizontal = servoCenterPostion_HeadHorizontal - targetAngle_Horizontal * servoIncrementsPerRadian_HeadHorizontal;
            double newServoPosition_HeadVertical   = servoCenterPostion_HeadVertical + servoOffset_HeadVertical + targetAngle_Vertical * servoIncrementsPerRadian_HeadVertical;

            // Eye position with head at center.
            double newServoPosition_Eyes = servoCenterPostion_Eyes + targetAngle_Horizontal * servoIncrementsPerRadian_Eyes;

            // Eye position based on current head position.
            //double newServoPosition_Eyes = servoCenterPostion_Eyes + (targetAngle_Horizontal - currentAngle_HeadHorizontal) * servoIncrementsPerRadian_Eyes;

            //ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "PersonTrackingUpdate: targetAngle_Horizontal = " + targetAngle_Horizontal + ", newServoPosition_HeadHorizontal = " + newServoPosition_HeadHorizontal + ", newServoPosition_Eyes = " + newServoPosition_Eyes);


            if (IsConnected())
            {
                // Update with head motion.
                StartSpeedMove(new ServoPosition(10, newServoPosition_HeadHorizontal), servoSpeed_HeadHorizontal);
                StartSpeedMove(new ServoPosition(9, newServoPosition_HeadVertical), servoSpeed_HeadVertical);

                // Update with only eye motion.
                //StartSpeedMove(new ServoPosition(10, servoCenterPostion_HeadHorizontal), servoSpeed_HeadHorizontal);
                //StartSpeedMove(new ServoPosition(9, servoCenterPostion_HeadVertical), servoSpeed_HeadVertical);
                //StartSpeedMove(new ServoPosition(8, newServoPosition_Eyes), servoSpeed_Eyes);
            }

            lock (personTrackingLock)
            {
                isAlreadyUpdatingTracking = false;
            }
        }
        // Sets the speed of the specified servo, in the servo controller hardware. This speed
        // value is first bounded within the servo's min/max speed limits, and a warning is
        // logged if a speed outside of these limits was specified.
        private void SetServoSpeed(Servo servo, long speed)
        {
            if (speed < servo.speedLimitMin)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to minimum limit " + servo.speedLimitMin.ToString());

                // Bound to this limit.
                speed = servo.speedLimitMin;
            }

            if (speed > servo.speedLimitMax)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to maximum limit " + servo.speedLimitMax.ToString());

                // Bound to this limit.
                speed = servo.speedLimitMax;
            }

            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " speed to " + speed.ToString());

            try
            {
                lock (uscLock)
                {
                    if (uscDevice == null)
                    {
                        throw new System.Exception("uscDevice is null");
                    }

                    // Send this value to the hardware.
                    uscDevice.setSpeed((byte)servo.index, (ushort)speed);
                }
            }
            catch (System.Exception ex)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoSpeed(): " + ex.Message);
            }
        }
        // Sets the target position of the specified servo, causing this servo to begin moving
        // to the new position. This target position is first bounded within the servo's min/max
        // position limits, and a warning is logged if a position outside of these limits was
        // specified.
        private void SetServoPosition(Servo servo, double position)
        {
            if (position < servo.positionLimitMin)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to minimum limit " + servo.positionLimitMin.ToString());

                // Bound to this limit.
                position = servo.positionLimitMin;
            }

            if (position > servo.positionLimitMax)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to maximum limit " + servo.positionLimitMax.ToString());

                // Bound to this limit.
                position = servo.positionLimitMax;
            }

            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " position to " + position.ToString());

            try
            {
                lock (uscLock)
                {
                    if (!isEnabled)
                    {
                        return;
                    }

                    if (uscDevice == null)
                    {
                        throw new System.Exception("uscDevice is null");
                    }

                    // Send this value to the hardware.
                    // Note that the servo position values are handled in this class in units of μs,
                    // to match the convention used by Pololu's Maestro Control Center application.
                    // However, the servo controller hardware expects the position represented as an
                    // integer value in 0.25 μs. The local value must be multiplied by 4 to convert
                    // to these units.
                    uscDevice.setTarget((byte)servo.index, (ushort)(position * 4));
                }
            }
            catch (System.Exception ex)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoPosition(): " + ex.Message);
            }
        }
        // Sets the acceleration of the specified servo, in the servo controller hardware.
        private void SetServoAcceleration(Servo servo, long acceleration)
        {
            ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " acceleration to " + acceleration.ToString());

            try
            {
                lock (uscLock)
                {
                    if (uscDevice == null)
                    {
                        throw new System.Exception("uscDevice is null");
                    }

                    // Send this value to the hardware.
                    uscDevice.setAcceleration((byte)servo.index, (ushort)acceleration);
                }
            }
            catch (System.Exception ex)
            {
                ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoAcceleration(): " + ex.Message);
            }
        }
        // Creates an example servo configuration xml file to provide a template for creating
        // a complete servo configuration file.
        public void GenerateExampleConfigFile(string fileName)
        {
            Servo exampleServo = new Servo();
            exampleServo.index = 0;
            exampleServo.name = "Example Servo";
            exampleServo.positionLimitMax = 0;
            exampleServo.positionLimitMin = 0;
            exampleServo.speedLimitMax = 0;
            exampleServo.speedLimitMin = 0;

            List<Servo> exampleServoList = new List<Servo>();
            exampleServoList.Add(exampleServo);

            XmlSerializer SerializerObj = new XmlSerializer(typeof(List<Servo>));
            TextWriter WriteFileStream = new StreamWriter(fileName);
            SerializerObj.Serialize(WriteFileStream, exampleServoList);
            WriteFileStream.Close();
        }