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 ------------
}
/// <summary>
/// Service constructor
/// </summary>
public TrackroamerLocationAndMappingService(DsspServiceCreationPort creationPort)
: base(creationPort)
{
_routePlanner = new RoutePlanner(_mapperVicinity);
_mapperVicinity.robotDirection = new Direction() { heading = 45.0d, bearing = 110.0d };
}
public void setMapper(MapperVicinity mapper, RoutePlanner routePlanner)
{
if (mapWindow != null)
{
_mapperVicinity = mapper;
_routePlanner = routePlanner;
mapWindow.mapperViewControl1.CurrentMapper = mapper;
mapWindow.mapperViewControl1.CurrentRoutePlanner = routePlanner;
}
}
public TrackRoamerBehaviorsService(DsspServiceCreationPort creationPort)
: base(creationPort)
{
LogInfo("DriveBehaviorServiceBase:DriveBehaviorServiceBase() -- port: " + creationPort.ToString() + " threads per CPU: " + Dispatcher.ThreadsPerCpu);
_routePlanner = new RoutePlanner(_mapperVicinity);
_mapperVicinity.robotDirection = new Direction() { heading = 45.0d, bearing = -110.0d };
_panTiltAlignment = PanTiltAlignment.RestoreOrDefault();
// Kinect UI - by default, lets show all the good stuff:
this.IncludeDepth = true;
this.IncludeVideo = true;
this.IncludeSkeletons = true;
}
