public MainWindow()
{
InitializeComponent();
routePlanner = new RoutePlanner(mapperVicinity);
this.Closing += new System.ComponentModel.CancelEventHandler(MainWindow_Closing);
//create picpxmod device.
_picpxmod = new ProximityModule(0x0925, 0x7001); // see PIC Firmware - usb_descriptors.c lines 178,179
_picpxmod.HasReadFrameEvent += pmFrameCompleteHandler;
_picpxmod.DeviceAttachedEvent += new EventHandler<ProximityModuleEventArgs>(_picpxmod_DeviceAttachedEvent);
_picpxmod.DeviceDetachedEvent += new EventHandler<ProximityModuleEventArgs>(_picpxmod_DeviceDetachedEvent);
bool deviceAttached = _picpxmod.FindTheHid();
pmValuesLabel.Content = deviceAttached ?
"Proximity Board Found" : string.Format("Proximity Board NOT Found\r\nYour USB Device\r\nmust have:\r\n vendorId=0x{0:X}\r\nproductId=0x{1:X}", _picpxmod.vendorId, _picpxmod.productId);
mapperVicinity.robotPosition = (GeoPosition)robotPositionDefault.Clone();
mapperVicinity.robotDirection = (Direction)robotDirectionDefault.Clone();
// we will need this later:
robotCornerDistanceMeters = Math.Sqrt(mapperVicinity.robotState.robotLengthMeters * mapperVicinity.robotState.robotLengthMeters + mapperVicinity.robotState.robotWidthMeters * mapperVicinity.robotState.robotWidthMeters) / 2.0d;
// --------- debug ------------
GeoPosition pos1 = (GeoPosition)mapperVicinity.robotPosition.Clone();
//pos1.translate(new Distance(1.0d), new Distance(1.0d)); // geo coordinates - East North
pos1.translate(new Direction() { heading = mapperVicinity.robotDirection.heading, bearingRelative = 45.0d }, new Distance(1.0d)); // robot coordinates - forward right
DetectedObstacle dobst1 = new DetectedObstacle(pos1) { color = Colors.Red };
mapperVicinity.Add(dobst1);
GeoPosition pos2 = (GeoPosition)mapperVicinity.robotPosition.Clone();
//pos2.translate(new Distance(-1.0d), new Distance(1.0d)); // geo coordinates - West North
pos2.translate(new Direction() { heading = mapperVicinity.robotDirection.heading, bearingRelative = -45.0d }, new Distance(1.0d)); // robot coordinates - forward left
DetectedObstacle dobst2 = new DetectedObstacle(pos2) { color = Colors.Yellow };
//mapperVicinity.Add(dobst2);
mapperVicinity.computeMapPositions();
// --------- end debug ------------
}
private void Window_Loaded(object sender, EventArgs e)
{
PanTiltAlignment.Restore();
PanTiltAlignment pta = PanTiltAlignment.getInstance();
panAlignScrollBar.Value = pta.panAlign;
panFactorScrollBar.Value = pta.panFactor;
tiltAlignScrollBar.Value = pta.tiltAlign;
tiltFactorScrollBar.Value = pta.tiltFactor;
// =============================================================
//create picpxmod device:
_picpxmod = new ProximityModule(0x0925, 0x7001); // see PIC Firmware - usb_descriptors.c lines 178,179
_picpxmod.HasReadFrameEvent += pmFrameCompleteHandler;
_picpxmod.DeviceAttachedEvent += new EventHandler<ProximityModuleEventArgs>(_picpxmod_DeviceAttachedEvent);
_picpxmod.DeviceDetachedEvent += new EventHandler<ProximityModuleEventArgs>(_picpxmod_DeviceDetachedEvent);
deviceAttached = _picpxmod.FindTheHid();
pmValuesLabel.Content = deviceAttached ?
"Proximity Board Found" : string.Format("Proximity Board NOT Found\r\nYour USB Device\r\nmust have:\r\n vendorId=0x{0:X}\r\nproductId=0x{1:X}", _picpxmod.vendorId, _picpxmod.productId);
// =============================================================
// create Kinect device:
nui = new Runtime();
try
{
nui.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor);
}
catch (InvalidOperationException)
{
System.Windows.MessageBox.Show("Runtime initialization failed. Please make sure Kinect device is plugged in.");
return;
}
// parameters used to smooth the skeleton data
nui.SkeletonEngine.TransformSmooth = true;
TransformSmoothParameters parameters = new TransformSmoothParameters();
parameters.Smoothing = 0.7f;
parameters.Correction = 0.3f;
parameters.Prediction = 0.4f;
parameters.JitterRadius = 1.0f;
parameters.MaxDeviationRadius = 0.5f;
nui.SkeletonEngine.SmoothParameters = parameters;
try
{
nui.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);
}
catch (InvalidOperationException)
{
System.Windows.MessageBox.Show("Failed to open stream. Please make sure to specify a supported image type and resolution.");
return;
}
lastTime = DateTime.Now;
nui.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);
nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);
nui.VideoFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_ColorFrameReady);
speak("come on!");
}
