private void InitializeHardware_Click(object sender, RoutedEventArgs e)
{
servoManager.LoadServoConfiguration(servoConfigFileName, false);
servoManager.ConnectToHardware();
UpdateConnectedTextblock(servoManager.IsConnected(), servoHardwareState);
ariaManager.InitializeAria();
UpdateConnectedTextblock(ariaManager.IsConnected(), ariaHardwareState);
kinectManager.InitializeKinect();
UpdateConnectedTextblock(kinectManager.IsConnected(), kinectHardwareState);
}
public ControlWindow()
{
InitializeComponent();
servoManager = new ServoManager(servoConfigFileName);
servoManager.ConnectToHardware();
UpdateConnectedTextblock(servoManager.IsConnected(), servoHardwareState);
ariaManager = new AriaManager();
ariaManager.InitializeAria();
UpdateConnectedTextblock(ariaManager.IsConnected(), ariaHardwareState);
robotSpeech = new RobotSpeech(servoManager);
robotEyes = new RobotEyes();
robotEyes.InitializeHardware();
personTracking = new PersonTracking(servoManager, ariaManager);
sequenceProcessor = new SequenceProcessor(servoManager, ariaManager, robotSpeech, robotEyes, sequenceFileName);
kinectManager = new KinectManager(sequenceProcessor, personTracking, robotSpeech);
kinectManager.InitializeKinect();
UpdateConnectedTextblock(kinectManager.IsConnected(), kinectHardwareState);
UpdateMotionEnabledDisplay();
logUpdateTimer.Tick += new EventHandler(logUpdateTimer_Tick);
logUpdateTimer.Interval = new TimeSpan(0, 0, 0, 0, 500);
logUpdateTimer.Start();
}
public ControlWindow()
{
InitializeComponent();
servoManager = new ServoManager(servoConfigFileName);
servoManager.ConnectToHardware();
UpdateConnectedTextblock(servoManager.IsConnected(), servoHardwareState);
ariaManager = new AriaManager();
ariaManager.InitializeAria();
UpdateConnectedTextblock(ariaManager.IsConnected(), ariaHardwareState);
robotSpeech = new RobotSpeech(servoManager);
robotEyes = new RobotEyes();
robotEyes.InitializeHardware();
personTracking = new PersonTracking(servoManager, ariaManager);
sequenceProcessor = new SequenceProcessor(servoManager, ariaManager, robotSpeech, robotEyes, sequenceFileName);
kinectManager = new KinectManager(sequenceProcessor, personTracking, robotSpeech);
kinectManager.InitializeKinect();
UpdateConnectedTextblock(kinectManager.IsConnected(), kinectHardwareState);
UpdateMotionEnabledDisplay();
logUpdateTimer.Tick += new EventHandler(logUpdateTimer_Tick);
logUpdateTimer.Interval = new TimeSpan(0, 0, 0, 0, 500);
logUpdateTimer.Start();
}
// 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;
}
}
// Moves the robot's head/eyes to track the specified skeleton joint.
public void UpdatePosition(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 = 2000;
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;
// Wheeled base tracking parameters.
const double baseRotationSpeed = 5;
const double trackingThreshold = 0.1;
if (servoManager.IsConnected())
{
servoManager.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 (servoManager.IsConnected())
{
// Update with head motion.
//servoManager.StartSpeedMove(new ServoPosition(10, newServoPosition_HeadHorizontal), servoSpeed_HeadHorizontal);
//servoManager.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);
}
if (ariaManager.IsConnected())
{
if (Math.Abs(targetAngle_Horizontal) > trackingThreshold)
{
double trackingVelocity = 0;
if (targetAngle_Horizontal > 0)
{
trackingVelocity = baseRotationSpeed;
}
else
{
trackingVelocity = -baseRotationSpeed;
}
ariaManager.SetPersonTrackingRotation(trackingVelocity);
// Restart watchdog timer.
wheelWatchdogTimer.Stop();
wheelWatchdogTimer.Start();
}
else
{
ariaManager.SetPersonTrackingRotation(0);
wheelWatchdogTimer.Stop();
}
}
lock (personTrackingLock)
{
isAlreadyUpdatingTracking = false;
}
}
