// 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); } }
// 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); } }
// Disables all servos by sending target position values of '0' to the hardware, for // each servo. This '0' value has a special function of relaxing the servo, rather // than moving to this position. public void DisableAllServos() { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "Disabling all servos."); lock (uscLock) { isEnabled = false; } foreach (Servo servo in servoList) { try { lock (uscLock) { if (uscDevice == null) { throw new System.Exception("uscDevice is null"); } uscDevice.setTarget((byte)servo.index, 0); } } catch (System.Exception ex) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in DisableAllServos(): " + ex.Message); } } }
// Disconnects from the Pololu Maestro servo controller. // Based on the 'TryToDisconnect' method from MaestroAdvancedExample in the pololu-usb-sdk. public void DisconnectFromHardware() { lock (uscLock) { if (uscDevice == null) { // Already disconnected return; } try { uscDevice.Dispose(); // Disconnect } catch (Exception ex) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "DisconnectFromHardware failed to cleaning disconnect the servo hardware: " + ex.Message); } finally { // do this no matter what uscDevice = null; } } }
// Moves both of the wheels to achieve the specified translation (in millimeters) and rotation (in degrees) // over the time specified by timeToDestination (in seconds). public void MoveBothWheels(double translationChange, double rotationChange, double timeToDestination) { if (!IsEnabled()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveBothWheels failed, motion is disabled."); return; } lock (movePriorityLock) { isRunningHighPriorityMove = true; } double translationVelocity = translationChange / timeToDestination; double rotationVelocity = rotationChange / timeToDestination; // Robot base is backwards with respect to torso, so translation velocity must be reversed. translationVelocity = -translationVelocity; StopTimer(); // Start wheel motion. SetVelocity(translationVelocity); SetRotationVelocity(rotationVelocity); // Wait for motion to complete. StartTimer(timeToDestination); waitHandle.WaitOne(); lock (movePriorityLock) { isRunningHighPriorityMove = 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 { // 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); } }
// Moves the wheels to achieve the specified change in left and right wheel positions (in millimeters) // over the time specified by timeToDestination (in seconds). public void MoveEachWheel(double leftWheelChange, double rightWheelChange, double timeToDestination) { if (!IsEnabled()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "MoveEachWheel failed, motion is disabled."); return; } lock (movePriorityLock) { isRunningHighPriorityMove = true; } double leftWheelVelocity = leftWheelChange / timeToDestination; double rightWheelVelocity = rightWheelChange / timeToDestination; // Robot base is backwards with respect to torso, so wheels are swapped (right is left) and velocity is reversed. double newLeftWheelVelocity = -rightWheelVelocity; double newRightWheelVelocity = -leftWheelVelocity; StopTimer(); // Start wheel motion. SetWheelVelocity(newLeftWheelVelocity, newRightWheelVelocity); // Wait for motion to complete. StartTimer(timeToDestination); waitHandle.WaitOne(); lock (movePriorityLock) { isRunningHighPriorityMove = false; } }
// Connects to the Pololu Maestro servo controller through USB. Only one controller is // currently expected, so this method just connects to the first controller it sees. // Based on the 'connectToDevice' method from MaestroEasyExample in the pololu-usb-sdk. public void ConnectToHardware() { try { DisconnectFromHardware(); // Get a list of all connected devices of this type. List <DeviceListItem> connectedDevices = Usc.getConnectedDevices(); foreach (DeviceListItem dli in connectedDevices) { // If you have multiple devices connected and want to select a particular // device by serial number, you could simply add a line like this: // if (dli.serialNumber != "00012345"){ continue; } uscDevice = new Usc(dli); // Connect to the device. break; // Use first device (should only be one, anyway). } if (uscDevice == null) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "ConnectToHardware() failed, no servo hardware found."); } else { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "ConnectToHardware() succeeded, connected to servo hardware."); } InitializeHardware(); } catch (System.Exception ex) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in Initialize(): " + ex.Message); } }
// Starts running the selected sequence in a new thread. Fails if another sequence // is already running. public void RunSequence(string sequenceName) { Sequence sequence = sequenceList.GetSequences().Find(x => x.name == sequenceName); if (sequence == null) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Sequence '" + sequenceName + "' not found."); return; } lock (sequenceLock) { if (sequenceIsRunning) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "RunSequence() failed to run sequence '" + sequenceName + "', another sequence is already running."); return; } else { sequenceIsRunning = true; } } runningSequence = sequence; Thread newThread = new Thread(new ThreadStart(RunSequenceThread)); // Start the thread newThread.Start(); }
// 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; } } }
// 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); } }
// Performs the sequence by stepping through each frame and executing the required // voice synthesizer, delay, and motion actions. private void RunSequenceThread() { if (!servoManager.IsConnected()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Servo hardware is disconnected, running sequence '" + runningSequence.name + "' in simulation mode."); } // Disable the person tracking feature while a sequence is executing. servoManager.PersonTrackingEnable(false); ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequence(): sequence '" + runningSequence.name + "' started."); foreach (Frame frame in runningSequence.GetFrames()) { // Run the speech synthesizer asynchronously. Speaks while continuing to // perform moves in this and subsequent frames. if (frame.speechString != null) { speechSynthesizer.SpeakAsync(frame.speechString); } // Go to the relevant PPT Slide // perform moves in this and subsequent frames. if (frame.pptIndex != null) { if (pptController.IsPptActive()) { pptController.GoToSlide(frame.pptIndex); } } // Wait for the specified amount of time. if (frame.delay > 0) { Thread.Sleep((int)(frame.delay * 1000)); } // Move all servos in the frame list. if (frame.GetServoPositions().Count > 0) { servoManager.MoveServos(frame.GetServoPositions(), frame.timeToDestination); } } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequence(): sequence '" + runningSequence.name + "' completed."); // Re-enable the person tracking feature. servoManager.PersonTrackingEnable(true); lock (sequenceLock) { sequenceIsRunning = false; } }
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); } }
// 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); } }
// Stops the current motion of the wheels. private void StopMotion() { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Stopping wheel motion."); if (IsConnected()) { lock (ariaLock) { // Always stop wheels, regardless of isEnabled flag. setVel_dll(0); setRotVel_dll(0); } } }
public void SetEyeState(bool leftEyeOpen, bool rightEyeOpen) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "SetEyeState setting left eye " + (leftEyeOpen ? "Open" : "Closed") + " and right eye " + (rightEyeOpen ? "Open" : "Closed") + "."); lock (eyeStateLock) { leftEyeOpenState = leftEyeOpen; rightEyeOpenState = rightEyeOpen; if (!blinkInProgress) { SetHardwareState(leftEyeOpen, rightEyeOpen); } } }
// Disables all servos by sending target position values of '0' to the hardware, for // each servo. This '0' value has a special function of relaxing the servo, rather // than moving to this position. public void DisableAllServos() { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "Disabling all servos."); foreach (Servo servo in servoList) { try { uscDevice.setTarget((byte)servo.index, 0); } catch (System.Exception ex) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in DisableAllServos(): " + ex.Message); } } }
// 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); }
// Sets the rotation velocity (deg/s) using both wheels. private void SetRotationVelocity(double velocity) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting wheel rotation velocity to " + velocity.ToString()); if (IsConnected()) { lock (ariaLock) { if (!isEnabled) { return; } //ArRobot::setRotVel(velocity); setRotVel_dll(velocity); } } }
// Sets the velocity (mm/s) of each wheel separately. private void SetWheelVelocity(double leftWheelVelocity, double rightWheelVelocity) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting left wheel velocity to " + leftWheelVelocity.ToString() + " and right wheel velocity to " + rightWheelVelocity.ToString()); if (IsConnected()) { lock (ariaLock) { if (!isEnabled) { return; } //ArRobot::setVel2(leftWheelVelocity, rightWheelVelocity); setVel2_dll(leftWheelVelocity, rightWheelVelocity); } } }
// Used by PersonTracking to track a person by rotating the robot. This tracking motion can be // interrupted by moves issued through Sequences, since these are higher priority. public void SetPersonTrackingRotation(double velocity) { if (!IsEnabled()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SetPersonTrackingRotation failed, motion is disabled."); return; } lock (movePriorityLock) { if (isRunningHighPriorityMove) { // Ignore this command if a higher priority (i.e. Sequence) move is already running. return; } SetRotationVelocity(velocity); } }
// Loads the configuration settings for each servo from the xml file specified by 'fileName', // and then initializes the servo hardware with these new settings. The InitializeHardware // step can be skipped, if desired (e.g. on startup when the hardware is not yet connected). public void LoadServoConfiguration(string fileName, bool initializeHardware = true) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "Loading servo config from " + fileName); XmlSerializer SerializerObj = new XmlSerializer(typeof(List <Servo>)); FileStream ReadFileStream = new FileStream(fileName, FileMode.Open, FileAccess.Read, FileShare.Read); servoList = (List <Servo>)SerializerObj.Deserialize(ReadFileStream); ReadFileStream.Close(); foreach (Servo servo in servoList) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Loaded Servo " + servo.index + ": Name = " + servo.name + ", PosMin = " + servo.positionLimitMin + ", PosMax = " + servo.positionLimitMax + ", SpeedMin = " + servo.speedLimitMin + ", SpeedMax = " + servo.speedLimitMax); } if (initializeHardware) { InitializeHardware(); } }
public void InitializeAriaHardware() { // Connect to Aria hardware int hr = InitializeRobot_dll(); switch (hr) { case 0: ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "InitializeAria() succeeded, connected to robot base control."); break; case 1: ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeAria() failed, could not parse arguments."); break; case 2: ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeAria() failed, could not connect to robot base."); break; } isHardwareInitialized = true; }
// Sets the servo controller with the default speeds and accelerations, for each servo. // This should be called whenever servo hardware is connected, or when new default // servo parameters have been loaded. private void InitializeHardware() { if (!IsConnected()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeHardware() failed, not connected to servo hardware."); return; } if (servoList.Count == 0) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeHardware() failed, no servos have been defined."); return; } foreach (Servo servo in servoList) { SetServoSpeed(servo, servo.defaultSpeed); SetServoAcceleration(servo, servo.defaultAcceleration); UpdateServoValues(); } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "InitializeHardware() succeeded, servo hardware was initialized."); }
// [Add new GestureTriggers here] // Raised whenever new skeleton data arrives from the Kinect sensor. // Updates person tracking and performs gesture recognition. private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e) { Skeleton[] skeletons = new Skeleton[0]; using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) { if (skeletonFrame != null) { skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength]; skeletonFrame.CopySkeletonDataTo(skeletons); } } if (skeletons.Length != 0) { bool isTrackingSameSkeleton = false; int skeletonIdClosestToCenter = 0; float shortestDistanceFromCenter = 0; // First, check if this skeleton frame contains the skeleton that we were already tracking. foreach (Skeleton skel in skeletons) { if (skel.TrackingState == SkeletonTrackingState.Tracked) { if (skel.TrackingId == trackedSkeletonId) { isTrackingSameSkeleton = true; break; } else if (skeletonIdClosestToCenter == 0 || Math.Abs(skel.Joints[JointType.ShoulderCenter].Position.X) < shortestDistanceFromCenter) { skeletonIdClosestToCenter = skel.TrackingId; shortestDistanceFromCenter = Math.Abs(skel.Joints[JointType.ShoulderCenter].Position.X); } } } // If the skeleton frame does not contain the one we were tracking, then track the skeleton that is closest to the center of the image (x-axis). if (!isTrackingSameSkeleton) { trackedSkeletonId = skeletonIdClosestToCenter; ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SensorSkeletonFrameReady() now tracking skeleton ID " + trackedSkeletonId.ToString() + "."); } // Update person tracking and check gestures from the tracked skeleton. foreach (Skeleton skel in skeletons) { // Only pay attention to the tracked skeleton. if (skel.TrackingId == trackedSkeletonId) { // Only pay attention to the skeleton if it is within the right and left edges of the view. // This should help prevent detecting false gestures, due to the skeleton being scrambled as it leaves the viewing area. if ((skel.ClippedEdges & FrameEdges.Left) == 0 && (skel.ClippedEdges & FrameEdges.Right) == 0) { personTracking.UpdatePosition(skel.Joints[JointType.ShoulderCenter].Position); if (gestureRecognitionEnabled) { // Check gesture recognition conditions. //leftHandGesture.Evaluate(sequenceProcessor, skel.Joints[JointType.HandLeft].Position.Y > skel.Joints[JointType.ElbowLeft].Position.Y, "Left Hand Raised"); //rightHandGesture.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.ElbowRight].Position.Y, "Right Hand Raised"); RightHandAboveHead.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.Head].Position.Y, "Hello"); RightHandBetweenSpineAndShoulderCenter.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.Spine].Position.Y && skel.Joints[JointType.HandRight].Position.Y < skel.Joints[JointType.ShoulderCenter].Position.Y && skel.Joints[JointType.HandRight].Position.X < (skel.Joints[JointType.Spine].Position.X + 0.1), "PLAYMUSIC"); LeftElbowAboveLeftShoulder.Evaluate(sequenceProcessor, skel.Joints[JointType.ElbowLeft].Position.Y > skel.Joints[JointType.ShoulderLeft].Position.Y, "NeckRotate"); WalkCloseToKinect.Evaluate(sequenceProcessor, skel.Joints[JointType.Spine].Position.Z < 1.5, "ArmRaise"); // [Add new Evaluate calls here] } } else { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SensorSkeletonFrameReady() skipped update due to skeleton being outside of view."); } break; } } } }
/// <summary> /// Handler for recognized speech events. /// </summary> /// <param name="sender">object sending the event.</param> /// <param name="e">event arguments.</param> private void SpeechRecognized(object sender, SpeechRecognizedEventArgs e) { if (!speechRecognitionEnabled) { return; } // Check if the speech synthesizer is already speaking. Ignore any detected // speech during this time, since it might have been generated by the synthesizer. lock (speechLock) { if (synthesizerIsSpeaking) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "SpeechRecognized: Ignored word '" + e.Result.Text + "', since synthesizer is speaking."); return; } } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "SpeechRecognized: Detected word '" + e.Result.Text + "' with confidence " + e.Result.Confidence.ToString()); // Speech utterance confidence below which we treat speech as if it hadn't been heard const double ConfidenceThreshold = 0.5; if (e.Result.Confidence >= ConfidenceThreshold) { string sequenceName = ""; // Perform the specified Sequence depending on the recognized speech. switch (e.Result.Semantics.Value.ToString()) { case "Hello": sequenceName = "Hello"; break; case "HowAreUDoing": sequenceName = "HowAreUDoing"; break; case "Introduce": sequenceName = "Introduce"; break; case "Number": sequenceName = "Number"; break; case "DoWhat": sequenceName = "DoWhat"; break; case "Thanks": sequenceName = "Thanks"; break; case "Count": sequenceName = "Count"; break; case "OneAndTwo": sequenceName = "OneAndTwo"; break; case "FiveMinusOne": sequenceName = "FiveMinusOne"; break; case "Gesture": sequenceName = "Gesture"; break; case "NextOne": sequenceName = "NextOne"; break; case "NextTwo": sequenceName = "NextTwo"; break; case "NextThree": sequenceName = "NextThree"; break; case "PlayHarp": sequenceName = "PlayHarp"; break; case "PLAYMUSIC": sequenceName = "PLAYMUSIC"; break; case "lab": sequenceName = "lab"; break; case "GoAhead": sequenceName = "GoAhead"; break; case "NAME": sequenceName = "NAME"; break; case "Bye": sequenceName = "Bye"; break; case "GoodMorning": sequenceName = "GoodMorning"; break; case "URwelcome": sequenceName = "URwelcome"; break; case "GladSU": sequenceName = "GladSU"; break; case "DEFAULT": sequenceName = "DEFAULT"; break; } if (sequenceName != "") { sequenceProcessor.RunSequence(sequenceName); } } else { // If speech is not recognized, then use a random default response. Random randomGenerator = new Random(); int randomSequenceNumber = randomGenerator.Next(1, 5); string sequenceName = "DEFAULT" + randomSequenceNumber.ToString(); if (sequenceName != "") { sequenceProcessor.RunSequence(sequenceName); } } }
// Reads the servo status from the servo controller hardware, and then updates the // servoList with these new polled values. Also tracks whether each servo is currently // moving by comparing their current and target positions. public void UpdateServoValues() { if (!IsConnected()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, not connected to servo hardware."); return; } if (servoList.Count == 0) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, no servos have been defined."); return; } try { // Get the servo parameters from the hardware. ServoStatus[] servoStatusArray; uscDevice.getVariables(out servoStatusArray); // Update the servoList with these parameters. foreach (Servo servo in servoList) { if (servo.index < 0 || servo.index >= uscDevice.servoCount) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "UpdateServoValues() failed, servo index out of range. Servo index = " + servo.index.ToString()); } else { servo.polledPosition = servoStatusArray[servo.index].position; servo.polledTarget = servoStatusArray[servo.index].target; servo.polledSpeed = servoStatusArray[servo.index].speed; servo.polledAcceleration = servoStatusArray[servo.index].acceleration; ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index.ToString() + ": Target = " + servo.polledTarget.ToString() + ", Position = " + servo.polledPosition.ToString() + ", Speed = " + servo.polledSpeed.ToString() + ", Acceleration = " + servo.polledAcceleration.ToString()); if (servo.isMoving == false && servo.polledTarget != servo.polledPosition) { // Servo has started moving. ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " has started moving from " + servo.polledPosition.ToString() + " to " + servo.polledTarget.ToString()); servo.isMoving = true; } else if (servo.isMoving == true && servo.polledTarget == servo.polledPosition) { // Servo has stopped moving. ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " has stopped moving at " + servo.polledPosition.ToString()); servo.isMoving = false; } if (servo.isMoving) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Servo " + servo.index + " is at position " + servo.polledPosition.ToString()); } } } } catch (System.Exception ex) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in UpdateServoValues(): " + ex.Message); } }
/// <summary> /// Handler for recognized speech events. /// </summary> /// <param name="sender">object sending the event.</param> /// <param name="e">event arguments.</param> private void SpeechRecognized(object sender, SpeechRecognizedEventArgs e) { if (!speechRecognitionEnabled) { return; } // Check if the speech synthesizer is already speaking. Ignore any detected // speech during this time, since it might have been generated by the synthesizer. lock (speechLock) { if (synthesizerIsSpeaking) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "SpeechRecognized: Ignored word '" + e.Result.Text + "', since synthesizer is speaking."); return; } } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "SpeechRecognized: Detected word '" + e.Result.Text + "' with confidence " + e.Result.Confidence.ToString()); // Speech utterance confidence below which we treat speech as if it hadn't been heard const double ConfidenceThreshold = 0.5; if (e.Result.Confidence >= ConfidenceThreshold) { string sequenceName = ""; // Perform the specified Sequence depending on the recognized speech. switch (e.Result.Semantics.Value.ToString()) { case "Hello": sequenceName = "Hello"; break; case "PlayHarp": sequenceName = "PlayHarp"; break; case "PLAYMUSIC": sequenceName = "PLAYMUSIC"; break; case "Count": sequenceName = "Count"; break; case "NAME": sequenceName = "NAME"; break; case "DEFAULT": sequenceName = "DEFAULT"; break; } if (sequenceName != "") { sequenceProcessor.RunSequence(sequenceName); } } }
// Performs the sequence by stepping through each frame and executing the required // voice synthesizer, delay, and motion actions. private void RunSequenceThread() { if (!servoManager.IsConnected()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Servo hardware is disconnected, running servos in simulation mode for sequence '" + runningSequence.name + "'."); } if (!ariaManager.IsConnected()) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Robot base hardware is disconnected, running robot base in simulation mode for sequence '" + runningSequence.name + "'."); } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequenceThread(): sequence '" + runningSequence.name + "' started."); foreach (Frame frame in runningSequence.GetFrames()) { // Run the speech synthesizer asynchronously. Speaks while continuing to // perform moves in this and subsequent frames. if (frame.speechString != null) { robotSpeech.Speak(frame.speechString); } // Set the state of the LED eyes. if (frame.eyeState != null) { switch (frame.eyeState) { case "Open": robotEyes.SetEyeState(true, true); break; case "Closed": robotEyes.SetEyeState(false, false); break; case "LeftClosed": robotEyes.SetEyeState(false, true); break; case "RightClosed": robotEyes.SetEyeState(true, false); break; } } // Wait for the specified amount of time. if (frame.delay > 0) { Thread.Sleep((int)(frame.delay * 1000)); } // Servo motion and wheel motion are performed simultaneously by creating two separate threads and // waiting until both threads have completed their motion. ManualResetEvent[] manualEvents = new ManualResetEvent[] { new ManualResetEvent(false), new ManualResetEvent(false) }; // Start servo motion thread. Thread servoThread = new Thread(new ParameterizedThreadStart(MoveServos)); servoThread.Start(new SequenceThreadData(frame, manualEvents[0])); // Start wheel motion thread. Thread wheelsThread = new Thread(new ParameterizedThreadStart(MoveWheels)); wheelsThread.Start(new SequenceThreadData(frame, manualEvents[1])); // Wait until both servo motion and wheel motion has completed. WaitHandle.WaitAll(manualEvents); } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "RunSequenceThread(): sequence '" + runningSequence.name + "' completed."); lock (sequenceLock) { sequenceIsRunning = false; } }
// Connect to the Kinect sensor and begin processing events. public void InitializeKinect() { this.sensor = null; // Look through all sensors and start the first connected one. // This requires that a Kinect is connected at the time of app startup. // To make your app robust against plug/unplug, // it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit (See components in Toolkit Browser). foreach (var potentialSensor in KinectSensor.KinectSensors) { if (potentialSensor.Status == KinectStatus.Connected) { this.sensor = potentialSensor; break; // Connect to first Kinect. } } if (null == this.sensor) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeKinect() failed, not connected to Kinect sensor."); return; } else { // Turn on the skeleton stream to receive skeleton frames this.sensor.SkeletonStream.Enable(); // Add an event handler to be called whenever there is new color frame data SkeletonFrameReady += SensorSkeletonFrameReady; // Start the sensor! try { this.sensor.Start(); } catch (IOException) { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeKinect() failed, unable to Start Kinect sensor."); this.sensor = null; return; } } RecognizerInfo ri = GetKinectRecognizer(); if (null != ri) { this.speechEngine = new SpeechRecognitionEngine(ri.Id); /**************************************************************** * * Use this code to create grammar programmatically rather than from * a grammar file. * * var directions = new Choices(); * directions.Add(new SemanticResultValue("forward", "FORWARD")); * directions.Add(new SemanticResultValue("forwards", "FORWARD")); * directions.Add(new SemanticResultValue("straight", "FORWARD")); * directions.Add(new SemanticResultValue("backward", "BACKWARD")); * directions.Add(new SemanticResultValue("backwards", "BACKWARD")); * directions.Add(new SemanticResultValue("back", "BACKWARD")); * directions.Add(new SemanticResultValue("turn left", "LEFT")); * directions.Add(new SemanticResultValue("turn right", "RIGHT")); * * var gb = new GrammarBuilder { Culture = ri.Culture }; * gb.Append(directions); * * var g = new Grammar(gb); * ****************************************************************/ // Create a grammar from grammar definition XML file. using (var memoryStream = new MemoryStream(Encoding.ASCII.GetBytes(Properties.Resources.SpeechGrammar))) { var g = new Grammar(memoryStream); speechEngine.LoadGrammar(g); } speechEngine.SpeechRecognized += SpeechRecognized; speechEngine.SpeechRecognitionRejected += SpeechRejected; robotSpeech.SpeakStarted += SpeakStarted; robotSpeech.SpeakCompleted += SpeakCompleted; // For long recognition sessions (a few hours or more), it may be beneficial to turn off adaptation of the acoustic model. // This will prevent recognition accuracy from degrading over time. ////speechEngine.UpdateRecognizerSetting("AdaptationOn", 0); speechEngine.SetInputToAudioStream( sensor.AudioSource.Start(), new SpeechAudioFormatInfo(EncodingFormat.Pcm, 16000, 16, 1, 32000, 2, null)); speechEngine.RecognizeAsync(RecognizeMode.Multiple); } else { ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "InitializeKinect() failed, no speech recognizer."); } ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "InitializeKinect() succeeded, Kinect sensor is ready."); }