public virtual void SetParameters(int AxisIndex, ConfigPhysicalAxis Config, IMotionController Controller)
{
this.AxisIndex = AxisIndex;
if (Config == null)
{
this.IsEnabled = false;
this.AxisName = "N/A";
}
else
{
this.AxisName = Config.Name;
this.IsEnabled = Config.Enabled;
this.InitPosition = Config.OffsetAfterHome;
this.MaxSpeed = Config.MaxSpeed;
this.AccelerationSteps = Config.Acceleration;
this.Parent = Controller;
this.UnitHelper = new RealworldUnitManager(
Config.MotorizedStageProfile.TravelDistance,
Config.MotorizedStageProfile.Resolution,
Config.MotorizedStageProfile.Unit,
Config.DecimalPlacesDisplayed);
this.SCCWL = 0;
this.SCWL = this.UnitHelper.MaxSteps;
}
}
public void Dispose()
{
if (_currentColorFrame != null)
{
_currentColorFrame.Dispose();
_currentColorFrame = null;
}
if (_currentDepthFrame != null)
{
_currentDepthFrame.Dispose();
_currentDepthFrame = null;
}
if (_currentInfraredFrame != null)
{
_currentInfraredFrame.Dispose();
_currentInfraredFrame = null;
}
if (_currentSilhouetteFrame != null)
{
_currentSilhouetteFrame.Dispose();
_currentSilhouetteFrame = null;
}
if (_motionController != null)
{
_motionController.Dispose();
_motionController = null;
}
}
public void Initialize(SettingsBase settings) //throws Exception
{
//Get the Foscam camera type (MJPEG or HD)
FoscamCameraType cameraType;
string _cameraType = (string)settings[PTZSettings.SETTING_CAMERA_TYPE];
if (_cameraType == null || _cameraType == "")
{
cameraType = DEFAULT_CAMERA_TYPE;
}
else
if (string.Equals(_cameraType, SETTING_CAMERA_FOSCAM_HD, StringComparison.OrdinalIgnoreCase))
{
cameraType = FoscamCameraType.FoscamHD;
}
else if (string.Equals(_cameraType, SETTING_CAMERA_FOSCAM_MJPEG, StringComparison.OrdinalIgnoreCase))
{
cameraType = FoscamCameraType.FoscamMJPEG;
}
else
{
throw new ArgumentNullException(PTZSettings.SETTING_CAMERA_TYPE);
}
//Get the camera URL
string url = (string)settings[PTZSettings.SETTING_CAMERA_URL];
if (url == null || url == "")
{
throw new ArgumentNullException(PTZSettings.SETTING_CAMERA_URL);
}
//Get the username
string username = (string)settings[PTZSettings.SETTING_CAMERA_USERNAME] ?? DEFAULT_USERNAME;
//Get the password
string password = (string)settings[PTZSettings.SETTING_CAMERA_PASSWORD] ?? DEFAULT_PASSWORD;
//Create motion and zoom controllers
_motion = FoscamMotion.CreateFoscamMotionController(cameraType, url, username, password);
_zoom = FoscamZoom.CreateFoscamZoomController(cameraType, url, username, password);
//Create PTZ control UI
_ptz = new PTZControl()
{
MotionController = _motion, ZoomController = _zoom
};
_minPanAngle = (double?)settings[PTZSettings.SETTING_MIN_PAN_ANGLE] ?? DEFAULT_MIN_PAN_ANGLE;
_maxPanAngle = (double?)settings[PTZSettings.SETTING_MAX_PAN_ANGLE] ?? DEFAULT_MAX_PAN_ANGLE;
_presetCount = (int?)settings[PTZSettings.SETTING_PRESET_COUNT] ?? DEFAULT_PRESET_COUNT;
_presetPrefix = (string)settings[PTZSettings.SETTING_PRESET_PREFIX] ?? DEFAULT_PRESET_PREFIX;
//Unzoom
ZoomLevel = 0;
_motion.MotionGotoCenter();
}
/// <summary>
/// Creates a new ProfileFollower to follow the specified profile
/// with an <see cref="IMotionController"/> for position correction
/// </summary>
/// <param name="profile"></param>
/// <param name="kv"></param>
/// <param name="ka"></param>
/// <param name="correction"></param>
public ProfileFollower(MotionProfile profile, double kv, double ka, IMotionController correction)
{
m_profile = profile;
m_correction = correction;
m_kv = kv;
m_ka = ka;
m_timer = new Stopwatch();
SetPoint = m_profile.m_path.Last().Position;
}
public override void SetParameters(int AxisIndex, ConfigPhysicalAxis Config, IMotionController Controller)
{
base.SetParameters(AxisIndex, Config, Controller);
if (Config.ReverseDriveDirection.HasValue)
{
this.ReverseDriveDirecton = Config.ReverseDriveDirection.Value;
}
else
{
this.ReverseDriveDirecton = false;
}
}
/// <summary>
/// Turns the robot to an absolute angle using a <see cref="SimplePID"/>
/// </summary>
/// <param name="d"></param>
/// <param name="controller"></param>
/// <param name="angle"></param>
/// <param name="tolerance"></param>
/// <param name="brake"></param>
public static void TurnToAngle(this IGyroscopeDrive d, IMotionController controller, double angle, double tolerance, bool brake, double interval = 0.02)
{
controller.SetPoint = angle;
while (Math.Abs(d.Gyroscope.GetAngle() - angle) > tolerance)
{
double power = controller.Get(d.Gyroscope.GetAngle());
d.SetPowers(power, -power);
AccurateWaitSeconds(interval);
}
if(brake)
d.SetPowers(0, 0);
}
/// <summary>
/// Drives the robot to a location according to an <see cref="IMotionController"/>
/// </summary>
/// <param name="drive"></param>
/// <param name="controller"></param>
/// <param name="location"></param>
/// <param name="tolerance"></param>
/// <param name="brake"></param>
/// <param name="interval"></param>
public static void DriveToDistance(this IEncoderDrive drive, IMotionController controller, double location, double tolerance, bool brake, double interval = 0.02)
{
controller.SetPoint = location;
while (Math.Abs(location - drive.Encoders.LinearDistance) > tolerance)
{
double power = controller.Get(drive.Encoders.LinearDistance);
drive.SetPowers(power, power);
AccurateWaitSeconds(interval);
}
if (brake)
drive.SetPowers(0, 0);
}
public MainForm(IStringResources stringResources, IGoogleDriveClient googleDriveClient,
ISettingsController settingsController,
IMotionController motionController, IFormLogger logger,
ITelegramClientWrapper telegramClientWrapper)
{
InitializeComponent();
Directory.SetCurrentDirectory(AppDomain.CurrentDomain.BaseDirectory);
this.googleDriveClient = googleDriveClient;
this.settingsController = settingsController;
this.motionController = motionController;
this.logger = logger;
this.telegramClientWrapper = telegramClientWrapper;
this.stringResources = stringResources;
logger.InitForm(this);
}
private void MainWindow_Loaded(object sender, RoutedEventArgs e)
{
FoscamCameraType cameraType =
#if USE_FOSCAM_HD_CAMERA
FoscamCameraType.FoscamHD;
#else
FoscamCameraType.FoscamMJPEG;
#endif
_video = FoscamVideo.CreateFoscamVideoController(cameraType, CAMERA_URL, USERNAME, PASSWORD);
_motion = FoscamMotion.CreateFoscamMotionController(cameraType, CAMERA_URL, USERNAME, PASSWORD);
_zoom = FoscamZoom.CreateFoscamZoomController(cameraType, CAMERA_URL, USERNAME, PASSWORD);
if (_video != null)
{
UIElement player = _video.VideoDisplay;
player.SetValue(Grid.RowProperty, 0);
//player.SetValue(Canvas.ZIndexProperty, -1);
LayoutRoot.Children.Add(player);
_video.StartVideo();
}
}
public SystemService()
{
ThreadPool.SetMinThreads(50, 50);
// read version from AssemblyInfo.cs
Version version = Assembly.GetExecutingAssembly().GetName().Version;
// force to enable the log, otherwise the initial message could not be recored
LogHelper.LogEnabled = true;
StringBuilder sb = new StringBuilder();
sb.Append("\r\n");
sb.Append("> =================================================================\r\n");
sb.Append("> = 4x25G/10x10G Alignment System =\r\n");
sb.Append("> = Copyright (C) 2017 Irixi =\r\n");
sb.Append("> =================================================================\r\n");
LogHelper.WriteLine(sb.ToString());
this.LastMessage = new MessageItem(MessageType.Normal, "System startup ...");
this.LastMessage = new MessageItem(MessageType.Normal, "Application Version {0}", version);
// read the configuration from the file named SystemCfg.json
// the file is located in \Configuration
ConfigManager conf_manager = SimpleIoc.Default.GetInstance <ConfigManager>();
// whether output the log
LogHelper.LogEnabled = conf_manager.ConfSystemSetting.LogEnabled;
// initialize the properties
BusyComponents = new List <IServiceSystem>();
PhysicalMotionControllerCollection = new Dictionary <Guid, IMotionController>();
LogicalAxisCollection = new ObservableCollection <LogicalAxis>();
LogicalMotionComponentCollection = new ObservableCollection <LogicalMotionComponent>();
MeasurementInstrumentCollection = new ObservableCollection <InstrumentBase>();
ActiveInstrumentCollection = new ObservableCollection <InstrumentBase>();
State = SystemState.BUSY;
SpiralScanArgs = new SpiralScanArgs();
AlignmentXDArgs = new AlignmentXDArgs();
/*
* enumerate all physical motion controllers defined in the config file,
* and create the instance of the motion controller class.
*/
foreach (var conf in conf_manager.ConfSystemSetting.PhysicalMotionControllers)
{
IMotionController motion_controller = null;
switch (conf.Model)
{
case MotionControllerModel.LUMINOS_P6A:
motion_controller = new LuminosP6A(conf);
motion_controller.OnMoveBegin += PhysicalMotionController_OnMoveBegin;
motion_controller.OnMoveEnd += PhysicalMotionController_OnMoveEnd;
break;
case MotionControllerModel.THORLABS_TDC001:
//TODO create the instance of thorlabs TDC001
break;
case MotionControllerModel.IRIXI_EE0017:
motion_controller = new IrixiEE0017(conf);
motion_controller.OnMoveBegin += PhysicalMotionController_OnMoveBegin;
motion_controller.OnMoveEnd += PhysicalMotionController_OnMoveEnd;
((IrixiEE0017)motion_controller).OnMessageReported += ((sender, message) =>
{
Application.Current.Dispatcher.Invoke(() =>
{
this.LastMessage = new MessageItem(MessageType.Normal, string.Format("{0} {1}", sender, message));
});
});
break;
default:
this.LastMessage = new MessageItem(MessageType.Error, "Unrecognized controller model {0}.", conf.Model);
break;
}
// Add the controller to the dictionary<Guid, Controller>
if (motion_controller != null)
{
this.PhysicalMotionControllerCollection.Add(motion_controller.DeviceClass, motion_controller);
}
}
// create the instance of the Logical Motion Components
foreach (var cfg_motion_comp in conf_manager.ConfSystemSetting.LogicalMotionComponents)
{
LogicalMotionComponent comp = new LogicalMotionComponent(cfg_motion_comp.Caption, cfg_motion_comp.Icon);
int axis_id = 0;
foreach (var cfg_axis in cfg_motion_comp.LogicalAxisArray)
{
// new logical axis object will be added to the Logical Motion Component
LogicalAxis axis = new LogicalAxis(this, cfg_axis, cfg_motion_comp.Caption, axis_id);
axis.OnHomeRequsted += LogicalAxis_OnHomeRequsted;
axis.OnMoveRequsted += LogicalAxis_OnMoveRequsted;
axis.OnStopRequsted += LogicalAxis_OnStopRequsted;
// bind the physical axis instance to logical axis object
BindPhysicalAxis(axis);
comp.LogicalAxisCollection.Add(axis);
this.LogicalAxisCollection.Add(axis);
axis_id++;
}
this.LogicalMotionComponentCollection.Add(comp);
}
// create the instance of the cylinder
try
{
IrixiEE0017 ctrl = PhysicalMotionControllerCollection[Guid.Parse(conf_manager.ConfSystemSetting.Cylinder.Port)] as IrixiEE0017;
CylinderController = new CylinderController(conf_manager.ConfSystemSetting.Cylinder, ctrl);
}
catch (Exception e)
{
this.LastMessage = new MessageItem(MessageType.Error, "Unable to initialize the cylinder controller, {0}", e.Message);
}
// create instance of the keithley 2400
foreach (var cfg in conf_manager.ConfSystemSetting.Keithley2400s)
{
this.MeasurementInstrumentCollection.Add(new Keithley2400(cfg));
}
// create instance of the newport 2832C
foreach (var cfg in conf_manager.ConfSystemSetting.Newport2832Cs)
{
this.MeasurementInstrumentCollection.Add(new Newport2832C(cfg));
}
}
public AxisBase(int AxisIndex, ConfigPhysicalAxis Config, IMotionController Controller)
{
DoConstruct();
SetParameters(AxisIndex, Config, Controller);
}
public AxisBase(int AxisIndex, ConfigPhysicalAxis Configuration, IMotionController ParentController)
{
_axis_lock = new SemaphoreSlim(1);
SetParameters(AxisIndex, Configuration, ParentController);
}
public LuminosAxis(ConfigPhysicalAxis Config, IMotionController Controller) : base(Config, Controller)
{
}
/// <summary>
/// Turns the robot for a relative angle amount using a <see cref="IMotionController"/>
/// </summary>
/// <param name="d"></param>
/// <param name="controller"></param>
/// <param name="angle"></param>
/// <param name="tolerance"></param>
public static void TurnForAngle(this IGyroscopeDrive d, IMotionController controller, double angle, double tolerance, bool brake, double interval = 0.02)
{
d.TurnToAngle(controller, d.Gyroscope.GetAngle() + angle, tolerance, brake, interval);
}
/// <summary>
/// Drives the robot in a straight line for a set time using a <see cref="IMotionController"/> to correct heading
/// </summary>
/// <param name="d"></param>
/// <param name="correction"> <see cref="IMotionController"/> to use for correcting heading</param>
/// <param name="power"></param>
/// <param name="time"></param>
/// <param name="brake"></param>
public static void DriveStraightForTime(this IGyroscopeDrive d, IMotionController correction, double power, double time, bool brake, double interval = 0.02)
{
correction.SetPoint = d.Gyroscope.GetAngle();
Stopwatch s = new Stopwatch();
while (s.Elapsed.TotalSeconds < time)
{
double correctingPower = correction.Get(d.Gyroscope.GetAngle());
d.SetPowers(power + correctingPower, power - correctingPower);
AccurateWaitSeconds(interval);
}
if(brake)
d.SetPowers(0, 0);
}
/// <summary>
/// Drive the robot to a relative position according to an <see cref="IMotionController"/>
/// </summary>
/// <param name="drive"></param>
/// <param name="controller"></param>
/// <param name="distance"></param>
/// <param name="tolerance"></param>
/// <param name="brake"></param>
/// <param name="interval"></param>
public static void DriveForDistance(this IEncoderDrive drive, IMotionController controller, double distance, double tolerance, bool brake, double interval = 0.02)
{
drive.DriveToDistance(controller, distance + drive.Encoders.LinearDistance, tolerance, brake, interval);
}
/// <summary>
/// Drives the robot at a set speed for a set distance
/// </summary>
/// <param name="drive"></param>
/// <param name="speedController"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="brake"></param>
/// <param name="interval"></param>
public static void DriveForAtSpeed(this IEncoderDrive drive, IMotionController speedController, double speed, double distance, bool brake, double interval = 0.02)
{
drive.DriveToAtSpeed(speedController, speed, distance + drive.Encoders.LinearDistance, brake, interval);
}
/// <summary>
/// Drives the robot at a set speed to a location
/// </summary>
/// <param name="drive"></param>
/// <param name="speedController"></param>
/// <param name="speed"></param>
/// <param name="location"></param>
/// <param name="brake"></param>
/// <param name="interval"></param>
public static void DriveToAtSpeed(this IEncoderDrive drive, IMotionController speedController, double speed, double location, bool brake, double interval = 0.02)
{
speedController.SetPoint = drive.Encoders.LinearDistance < location ? speed : -speed;
while ((drive.Encoders.LinearDistance - location)*Math.Sign(speedController.SetPoint) > 0)
{
double power = speedController.Get(drive.Encoders.LinearSpeed);
drive.SetPowers(power, power);
AccurateWaitSeconds(interval);
}
if (brake)
drive.SetPowers(0, 0);
}
public MotionController()
{
_motionController = MotionControllerFactory.CreateMotionController(
MotionControllerAPI.Kinectv2);
_currentFrameState = FrameState.Silhouette;
}
public IrixiAxis(ConfigPhysicalAxis Config, IMotionController Controller) : base(Config, Controller)
{
}
///// <summary>
///// If you create this object without any parameters, the SetParameters() function MUST BE implemented
///// </summary>
//public AxisBase()
//{
// GenericConstructor();
//}
public AxisBase(ConfigPhysicalAxis Config, IMotionController Controller)
{
GenericConstructor();
Setup(Config, Controller);
}
