aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Visual_Studio/PPC/Tango.PPC.UI/PPCApplication/DefaultPPCApplicationManager.cs
blob: 003938303f8c1700281d5a6a13d604a2d817350f (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Tango.BL;
using Tango.BL.Entities;
using Tango.Integration.Operation;
using Tango.Settings;
using Tango.PPC.Common.Application;
using Tango.Core.DI;
using Tango.PPC.Common;
using System.Windows;
using Tango.Core;
using System.IO;
using Tango.Core.Helpers;
using Tango.PPC.Common.Modules;
using System.Windows.Threading;
using Tango.Transport.Adapters;
using Tango.PMR.Connection;
using Tango.PPC.Common.Connection;
using Tango.SQLExaminer;
using System.Data.SqlClient;
using Tango.BL.Builders;
using Tango.PPC.Common.Threading;
using System.Diagnostics;
using Tango.PPC.Common.EventLogging;
using Tango.BL.Enumerations;
using Tango.PPC.Common.Notifications;
using Tango.PPC.Common.WatchDog;
using Tango.PPC.UI.Dialogs;
using Tango.Core.Threading;
using Tango.PPC.Common.Messages;
using Tango.Core.ExtensionMethods;
using Tango.PPC.Common.Navigation;
using Tango.PPC.Common.Synchronization;

namespace Tango.PPC.UI.PPCApplication
{
    /// <summary>
    /// Represents the default PPC application manager.
    /// </summary>
    /// <seealso cref="Tango.Core.ExtendedObject" />
    /// <seealso cref="Tango.PPC.Common.Application.IPPCApplicationManager" />
    public class DefaultPPCApplicationManager : ExtendedObject, IPPCApplicationManager
    {
        private List<PPCViewModel> _notifiedViewModels;
        private IMachineProvider _machineProvider;
        private Machine _machine;
        private IDispatcherProvider _dispatcher;
        private IEventLogger _eventLogger;
        private IPPCModuleLoader _moduleLoader;
        private INotificationProvider _notificationProvider;
        private IMachineDataSynchronizer _machineDataSynchronizer;
        private WatchDogServer _watchdogServer;
        private ObservablesContext _machineContext;
        private ActionTimer _screenLockTimer;

        /// <summary>
        /// Occurs when a system restart is required.
        /// </summary>
        public event EventHandler SystemRestartRequired;

        /// <summary>
        /// Occurs when the application has started.
        /// </summary>
        public event EventHandler ApplicationStarted;

        /// <summary>
        /// Occurs when all PPC modules are ready and initialized.
        /// </summary>
        public event EventHandler ModulesInitialized;

        /// <summary>
        /// Occurs when the application is ready and all modules are views are loaded.
        /// </summary>
        public event EventHandler ApplicationReady;

        /// <summary>
        /// Occurs when machine setup is required.
        /// </summary>
        public event EventHandler SetupRequired;

        /// <summary>
        /// Occurs when the main window content has been rendered.
        /// </summary>
        public event EventHandler ContentRendered;

        /// <summary>
        /// Occurs when the application has encountered an error when initializing.
        /// </summary>
        public event EventHandler<Exception> ApplicationInitializationError;

        /// <summary>
        /// Gets a value indicating whether the application is shutting down.
        /// </summary>
        public bool IsShuttingDown { get; private set; }

        private bool _isInTechnicianMode;
        /// <summary>
        /// Gets a value indicating whether the application is in technician mode.
        /// </summary>
        public bool IsInTechnicianMode
        {
            get { return _isInTechnicianMode; }
            set { _isInTechnicianMode = value; RaisePropertyChangedAuto(); }
        }

        /// <summary>
        /// Gets the application version.
        /// </summary>
        public Version Version
        {
            get
            {
                return AssemblyHelper.GetCurrentAssemblyVersion();
            }
        }

        /// <summary>
        /// Gets the application build date.
        /// </summary>
        public String BuildDate
        {
            get
            {
                return AssemblyHelper.GetCurrentAssemblyBuildDate().ToShortDateString();
            }
        }

        /// <summary>
        /// Gets the application startup date.
        /// </summary>
        public DateTime StartUpDate { get; private set; }

        /// <summary>
        /// Gets a value indicating whether an update has occurred before the application started.
        /// </summary>
        public bool IsAfterUpdate { get; private set; }

        private bool _isScreenLocked;
        /// <summary>
        /// Gets or sets a value indicating whether the screen is currently locked.
        /// </summary>
        public bool IsScreenLocked
        {
            get { return _isScreenLocked; }
            set { _isScreenLocked = value; RaisePropertyChangedAuto(); }
        }

        /// <summary>
        /// Gets the firmware version.
        /// </summary>
        public Version FirmwareVersion
        {
            get
            {
                return Version.Parse(SettingsManager.Default.GetOrCreate<PPCSettings>().FirmwareVersion);
            }
        }

        /// <summary>
        /// Gets or sets the application folder.
        /// </summary>
        public String StartPath { get; private set; }

        /// <summary>
        /// Initializes a new instance of the <see cref="DefaultPPCApplicationManager"/> class.
        /// </summary>
        public DefaultPPCApplicationManager(IMachineProvider machineProvider, IDispatcherProvider dispatcherProvider, IEventLogger eventLogger, IPPCModuleLoader moduleLoader, INotificationProvider notificationProvider, IMachineDataSynchronizer machineDataSynchronizer)
        {
            StartPath = AssemblyHelper.GetCurrentAssemblyFolder();

            _notificationProvider = notificationProvider;
            _machineProvider = machineProvider;
            _dispatcher = dispatcherProvider;
            _eventLogger = eventLogger;
            _moduleLoader = moduleLoader;
            _machineDataSynchronizer = machineDataSynchronizer;

            if (!DesignMode)
            {
                _notifiedViewModels = new List<PPCViewModel>();

                MainWindow.Instance.ContentRendered += (_, __) =>
                {
                    OnMainWindowContentRendered();
                };
            }
        }

        /// <summary>
        /// Called when the main window content has been rendered
        /// </summary>
        private void OnMainWindowContentRendered()
        {
            LogManager.Log("Main window content rendered.");

            ContentRendered?.Invoke(this, new EventArgs());

            StartApplication();
        }

        private async void StartApplication()
        {
            PPCSettings settings = null;

            bool initialized = false;
            bool isAfterSetup = false;

            StartUpDate = DateTime.Now;

            await Task.Factory.StartNew(() =>
            {
                try
                {
                    LogManager.Log("Reading PPC settings...");
                    settings = SettingsManager.Default.GetOrCreate<PPCSettings>();

                    LogManager.Log(settings.ToJsonString());

                    //Start watchdog
                    _watchdogServer = new WatchDogServer(Application.Current.Dispatcher);
#if !DEBUG

                    if (settings.EnableWatchDog)
                    {
                        _watchdogServer.Start();
                    }
#endif

                    LogManager.Log("Reading Core settings...");
                    var coreSettings = SettingsManager.Default.GetOrCreate<CoreSettings>();

                    if (!SettingsManager.Default.IsFileExists())
                    {
                        LogManager.Log("Settings file does not exists. creating...");
                        settings.Save();
                    }

                    if (App.StartupArgs.Contains("-update_ok"))
                    {
                        LogManager.Log("Application started with '-update_ok' startup arguments. The application has been successfully updated.");

                        if (settings.ApplicationState == ApplicationStates.PreSetup || settings.ApplicationState == ApplicationStates.FactoryRestore)
                        {
                            isAfterSetup = true;
                            LogManager.Log("System restart is required.");
                        }
                        else
                        {
                            IsAfterUpdate = true;
                        }

                        settings.ApplicationState = ApplicationStates.Ready;
                        settings.Save();

                        if (isAfterSetup)
                        {
                            SystemRestartRequired?.Invoke(this, new EventArgs());
                            return;
                        }
                    }

                    if (settings.ApplicationState == ApplicationStates.Ready)
                    {
                        LogManager.Log("Initializing ObservablesStaticCollections...");
                        ObservablesStaticCollections.Instance.Initialize();
                        LogManager.Log("Loading machine from database...");
                        _machineContext = ObservablesContext.CreateDefault();
                        _machine = new MachineBuilder(_machineContext).SetFirst().WithVersion().WithSettings().WithOrganization().WithConfiguration().WithSpools().WithCats().Build();


                    }

                    initialized = true;
                }
                catch (Exception ex)
                {
                    LogManager.Log(ex, "Application Initialization Error!");
                    ApplicationInitializationError?.Invoke(this, ex);
                    return;
                }
            });


            if (initialized)
            {
                try
                {
                    if (settings.ApplicationState == ApplicationStates.PreSetup || settings.ApplicationState == ApplicationStates.FactoryRestore)
                    {
                        LogManager.Log($"The application is in {settings.ApplicationState} state. database initialization skipped. Invoking setup required event!");
                        SetupRequired?.Invoke(this, new EventArgs());
                    }
                    else
                    {
                        PostDbInitialize();
                    }
                }
                catch (Exception ex)
                {
                    LogManager.Log(ex, "Application Post Initialization Error!");
                    ApplicationInitializationError?.Invoke(this, ex);
                    return;
                }
            }
        }

        /// <summary>
        /// Called when the database has been initialized
        /// </summary>
        private void PostDbInitialize()
        {
            LogManager.Log($"Raising {nameof(ApplicationStarted)} event...");

            _eventLogger.Log(EventTypes.APPLICATION_STARTED, "Application Started!");
            ApplicationStarted?.Invoke(this, new EventArgs());

            LogManager.Log("Invoking PPC view models OnApplicationStarted methods...");
            foreach (var vm in TangoIOC.Default.GetAllInstancesByBase<PPCViewModel>())
            {
                if (!_notifiedViewModels.Contains(vm))
                {
                    LogManager.Log($"Invoking {vm.GetType().Name}.OnApplicationStarted...");
                    vm.OnApplicationStarted();
                    _notifiedViewModels.Add(vm);
                }
            }

            var internalModules = this.GetType().Assembly.GetTypes().Where(xx => typeof(PPCModuleBase).IsAssignableFrom(xx)).ToList();

            LogManager.Log("Waiting for IPPCModuleLoader instance injection...");
            TangoIOC.Default.GetInstanceWhenAvailable<IPPCModuleLoader>((loader) =>
            {
                LogManager.Log("Module loader instance has been registered. Registering for the ModulesLoaded event...");

                loader.ModulesLoaded += (x, y) =>
                {
                    LogManager.Log("Loading modules views");
                    _dispatcher.InvokeBlock(() =>
                    {
                        foreach (var module in TangoIOC.Default.GetInstance<IPPCModuleLoader>().UserModules)
                        {
                            if (!Views.LayoutView.Instance.NavigationControl.Elements.ToList().Exists(m => m.GetType() == module.MainViewType))
                            {
                                try
                                {
                                    LogManager.Log("Loading module view " + module.Name + "...");
                                    FrameworkElement view = Activator.CreateInstance(module.MainViewType) as FrameworkElement;
                                    SharedUI.Controls.NavigationControl.SetNavigationName(view, module.Name);
                                    Views.LayoutView.Instance.NavigationControl.Elements.Add(view);
                                }
                                catch (Exception ex)
                                {
                                    LogManager.Log(ex, $"Error loading module view for module {module.Name}.");
                                }
                            }
                        }

                        //Adding internal modules.
                        LogManager.Log("Loading internal modules...");
                        foreach (var type in internalModules)
                        {
                            var module = Activator.CreateInstance(type) as IPPCModule;
                            LogManager.Log("Loading module view " + module.Name + "...");
                            FrameworkElement view = Activator.CreateInstance(module.MainViewType) as FrameworkElement;
                            SharedUI.Controls.NavigationControl.SetNavigationName(view, module.Name);
                            Views.LayoutView.Instance.NavigationControl.Elements.Add(view);
                            _moduleLoader.AllModules.Add(module);
                            _moduleLoader.UserModules.Add(module);
                        }
                    });

                    LogManager.Log($"{loader.UserModules.Count} modules loaded.");

                    LogManager.Log($"Invoking {nameof(ModulesInitialized)} event.");
                    ModulesInitialized?.Invoke(this, new EventArgs());

                    FinalizeModuleInitialization();
                };
            });
        }

        /// <summary>
        /// Finalizes the module initialization.
        /// </summary>
        private void FinalizeModuleInitialization()
        {
            var settings = SettingsManager.Default.GetOrCreate<PPCSettings>();

            LogManager.Log("Finalizing application initialization...");

            LogManager.Log("Initializing Machine Provider...");
            _machineProvider.Init(_machine, _machineContext);

            LogManager.Log("Starting Machine Data Synchronizer...");
            _machineDataSynchronizer.IsEnabled = true;

            LogManager.Log("Applications initialization completed!");

            LogManager.Log("Checking for un-notified PPC view models...");
            foreach (var vm in TangoIOC.Default.GetAllInstancesByBase<PPCViewModel>())
            {
                if (!_notifiedViewModels.Contains(vm))
                {
                    LogManager.Log($"Invoking {vm.GetType().Name}.OnApplicationStarted...");
                    vm.OnApplicationStarted();
                    _notifiedViewModels.Add(vm);
                }
            }

            _dispatcher.Invoke(() =>
            {
                LogManager.Log($"Invoking {nameof(ApplicationReady)} event.");
                ApplicationReady?.Invoke(this, new EventArgs());

                LogManager.Log("Notifying view models about application ready...");
                foreach (var vm in TangoIOC.Default.GetAllInstancesByBase<PPCViewModel>())
                {
                    LogManager.Log($"Invoking {vm.GetType().Name}.OnApplicationReady...");
                    vm.OnApplicationReady();
                }

                if (settings.EnableTechnicianModeByDefault)
                {
                    EnterTechnicianMode(false);
                }

                if (settings.EnableLockScreen)
                {
                    _screenLockTimer = new ActionTimer(settings.LockScreenTimeout);
                    _screenLockTimer.ResetReplace(ScreenLockTimerAction);
                }

                TangoMessenger.Default.Register<MachineSettingsSavedMessage>((msg) =>
                {
                    if (_screenLockTimer != null)
                    {
                        _screenLockTimer.Dispose();
                        _screenLockTimer = null;
                    }

                    if (settings.EnableLockScreen)
                    {
                        _screenLockTimer = new ActionTimer(settings.LockScreenTimeout);
                        _screenLockTimer.ResetReplace(ScreenLockTimerAction);
                    }
                });
            });
        }

        /// <summary>
        /// Shutdown the application.
        /// </summary>
        public void ShutDown()
        {
            if (IsShuttingDown) return;

            IsShuttingDown = true;

            try
            {
                LogManager.Log("Shutting down application...");
                _watchdogServer.Dispose();

                foreach (var vm in TangoIOC.Default.GetAllInstancesByBase<PPCViewModel>())
                {
                    vm.OnApplicationShuttingDown();
                }
            }
            catch { }

            Environment.Exit(0);
        }

        /// <summary>
        /// Restarts the application.
        /// </summary>
        public async void Restart()
        {
            if (IsShuttingDown) return;

            IsShuttingDown = true;

            try
            {
                _dispatcher.Invoke(() =>
                {
                    var nav = TangoIOC.Default.GetInstance<INavigationManager>();
                    if (nav != null)
                    {
                        nav.NavigateTo(NavigationView.RestartingView);
                    }
                });

                LogManager.Log("Restarting the application...");

                await Task.Delay(8000);

                _watchdogServer.Dispose();

                foreach (var vm in TangoIOC.Default.GetAllInstancesByBase<PPCViewModel>())
                {
                    vm.OnApplicationShuttingDown();
                }
            }
            catch { }

            try
            {
                if (_machineProvider.MachineOperator.State == Transport.TransportComponentState.Connected)
                {
                    _machineProvider.MachineOperator.Adapter.Disconnect().Wait();
                }
            }
            catch { }

            Process.Start(Application.ResourceAssembly.Location);
            Environment.Exit(0);
        }

        /// <summary>
        /// Runs the updater utility and exits the application.
        /// </summary>
        public void UpdateApplication(String updaterPath, String arguments)
        {
            if (IsShuttingDown) return;

            IsShuttingDown = true;

            try
            {
                _watchdogServer.Dispose();

                foreach (var vm in TangoIOC.Default.GetAllInstancesByBase<PPCViewModel>())
                {
                    vm.OnApplicationShuttingDown();
                }
            }
            catch { }

            try
            {
                SettingsManager.Default.GetOrCreate<PPCSettings>().PreviousApplicationVersion = Version.ToString();
                SettingsManager.Default.Save();
            }
            catch { }

            LogManager.Log($"Executing '{updaterPath}' with arguments '{arguments}'...");
            Process.Start(updaterPath, arguments);

            LogManager.Log("Terminating application...");
            Environment.Exit(0);
        }

        /// <summary>
        /// Enteres the application technician mode.
        /// </summary>
        public async void EnterTechnicianMode(bool displayNotification = true)
        {
            if (displayNotification)
            {
                var vm = await _notificationProvider.ShowDialog<TechnicianModeLoginViewVM>();

                if (vm.DialogResult)
                {
                    if (vm.Password == "Aa123456")
                    {
                        IsInTechnicianMode = true;
                        _moduleLoader.AllModules.ToList().ForEach(x => x.OnTechnicianEntered());
                        await _notificationProvider.ShowInfo("Technician mode is now enabled.");
                    }
                    else
                    {
                        await _notificationProvider.ShowError("Invalid technician mode password.");
                        EnterTechnicianMode();
                    }
                }
            }
            else
            {
                IsInTechnicianMode = true;
                _moduleLoader.AllModules.ToList().ForEach(x => x.OnTechnicianEntered());
            }
        }

        /// <summary>
        /// Exits the application technician mode.
        /// </summary>
        public void ExitTechnicianMode()
        {
            IsInTechnicianMode = false;
            _moduleLoader.AllModules.ToList().ForEach(x => x.OnTechnicianExited());
            _notificationProvider.ShowInfo("Technician mode is now disabled.");
        }

        /// <summary>
        /// Invokes a dialog for entering a password and releasing the screen lock.
        /// </summary>
        public async void ReleaseScreenLock()
        {
            if (IsScreenLocked)
            {
                var vm = await _notificationProvider.ShowDialog<ScreenLockViewVM>();

                if (vm.DialogResult)
                {
                    if (vm.Password == SettingsManager.Default.GetOrCreate<PPCSettings>().LockScreenPassword)
                    {
                        IsScreenLocked = false;
                        ResetScreenLockTimer();
                    }
                }
            }
        }

        public void ResetScreenLockTimer()
        {
            if (_screenLockTimer != null)
            {
                _screenLockTimer.ResetReplace(ScreenLockTimerAction);
            }
        }

        private void ScreenLockTimerAction()
        {
            IsScreenLocked = true;
        }
    }
}
#else #ifdef BTSR_NO_PULLER_TFU if (CurrentControlledSpeed[WINDER_MOTOR]>100) length = dyeingspeed/10; #endif #endif PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION CurrentProcessedLength+=length; static int puller_counter = 0; puller_counter++; if (puller_counter%10 == 0) { if (PrepareState == true) { TemperatureListString(Lenstr); SendJobProgress(0.0,0,false, Lenstr); } else { SendJobProgress(PoolerTotalProcessedLength,0,false, NULL); } } if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0)) { usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Lenstr); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0); // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); } #endif return OK; } float SpeedSamples[MAX_CONTROL_SAMPLES] = {0}; uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle int index=MAX_THREAD_MOTORS_NUM; int32_t i, avreageSampleValue = 0; //double tempcalcspeed = 0; double calculated_speed; float speed = getSensorSpeedData(); if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += SpeedSamples[i]; avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode; if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0)) { MotorControlConfig[index].m_mesuredParam = ReadValue; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError); calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (fabs(calculated_speed-CurrentControlledSpeed[index])>2) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } } return OK; } float _speed; uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) { int index; if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { int MotorId = ThreadMotorIdToMotorId[index]; _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId); } return OK; } uint16_t BreakSensorCounter = 0; uint16_t BreakSensorLatchCounter = 0; char TMessage[150]; #ifdef FOUR_WINDERS char ATMessage[MAX_THREAD_MOTORS_NUM][150]; int c = 0; #endif uint32_t checkBreakSensor(uint32_t index) { if (BreakSensorenabled == true) { if (JobCounter > eOneSecond) { if (ReadBreakSensor()==ERROR) { BreakSensorCounter++; BreakSensorLatchCounter++; if (BreakSensorCounter>=BreakSensordebouncetimemilli) { //consider applying the debouce parameters later usnprintf(TMessage, 60, "ReadBreakSensor Error"); //BreakSensordebouncetimemilli JobEndReason = JOB_THREAD_BREAK; ThreadControlActive = false; SendJobProgress(0.0,0,false, TMessage); strcpy(AlarmReasonStr,TMessage); SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); return ERROR; } //passed limit }//ReadBreakSensor()==ERROR else //reset counter - we are looking for consequent calls { if (BreakSensorCounter) { ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); } BreakSensorCounter = 0; } if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3)) { BreakSensorCounter++; BreakSensorLatchCounter++; if (BreakSensorCounter>=BreakSensordebouncetimemilli) { //consider applying the debouce parameters later usnprintf(TMessage, 60, "thread speed too low"); strcpy(AlarmReasonStr,TMessage); JobEndReason = JOB_THREAD_BREAK; ThreadControlActive = false; SendJobProgress(0.0,0,false, TMessage); SendSegmentFail(); //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0); return ERROR; } } } } return OK; } int controlIndex = 0; bool keepdata = true; int32_t KeepReadValue = 0; //double eNormalizedError[100]; //int TranslatedreadValue[100]; #ifdef TEST_PID_THREAD #define MAX_THREAD_CONTROL_LOG 100 double calculatedError[MAX_THREAD_CONTROL_LOG+1]; double NormError[MAX_THREAD_CONTROL_LOG+1]; double mIntegral[MAX_THREAD_CONTROL_LOG+1]; int MotorId[MAX_THREAD_CONTROL_LOG+1]; int readValue[MAX_THREAD_CONTROL_LOG+1]; int AveragereadValue[MAX_THREAD_CONTROL_LOG+1]; int calculatedspeed[MAX_THREAD_CONTROL_LOG+1]; int timestamp[MAX_THREAD_CONTROL_LOG+1]; void testDancersControl() { int mm20,mm10,mm5,mm2,mm1; mm20 = (20*DancerStopActivityLimit[FEEDER_MOTOR])/(DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].maximalmovementmm*2); mm2 = mm20/10; mm5 = mm20/4; mm10 = mm20/2; mm1 = mm20/20; char time[150]; int len; ThreadControlActive = true; SetOriginMotorSpeed(50.0); MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = 0; MotorsControl[FEEDER_MOTOR].controloutputtype = 0.001; MotorControlConfig[FEEDER_MOTOR].m_params.dt = 0.001; DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint = 10000; MotorsControl[FEEDER_MOTOR].pvinputfilterfactormode = 1; len = usnprintf(time, 150, "params: speed 50, divider %d p %d * %d i %d * %d Dt*1000 %d Norm Coef %d initial speed %d",NORMAL_COEF_DIVIDER,(int)MotorsControl[FEEDER_MOTOR].proportionalgain,(int)MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue, (int)MotorsControl[FEEDER_MOTOR].integraltime,(int)MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp,(int)(MotorsControl[FEEDER_MOTOR].controloutputtype*1000), (int)(NormalizedErrorCoEfficient[FEEDER_MOTOR]*1000000000),OriginalMotorSpd_2PPS[FEEDER_MOTOR]); ReportWithPackageFilter(ThreadFilter,time,__FILE__,__LINE__,111,RpError,111,0); Task_sleep(100); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm5); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm2); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm1); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm1); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm2); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm5); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm10); ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm20); ThreadControlActive = false; } #endif int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; //char time[150]; bool FirstCalcInJob = true; uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0; //double tempcalcspeed = 0; double calculated_speed; double NormalizedError; if (ThreadControlActive == false) return OK; #ifndef TEST_PID_THREAD if (PrepareState == true) return OK; #endif if (IfIndex>>8 != IfTypeThread) { ReportWithPackageFilter(ThreadFilter,"Wrong Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0); return 0xFFFFFFFF; } index = IfIndex&0xFF; if (index == WINDER_MOTOR) //move break sensor handling up to ensure handling even if tiing control is > 1 msec { if (checkBreakSensor(index) == ERROR) return OK; } if (MotorTiming[index]>1) { MotorTimer[index]++; if (MotorTimer[index]>=MotorTiming[index]) { MotorTimer[index]=0; } else { return OK; } } if(MotorControlConfig[index].m_isEnabled ) { //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true) // return OK; DancerId = ThreadMotorIdToDancerId[index]; /* if (ReadValue < 10) { MotorFailedSample[index]++; ReportWithPackageFilter(ThreadFilter,"Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; }*/ if (ReadValue == 0x3FFF) { if (Read_Dryer_Status(DancerId) != OK) { ReportWithPackageFilter(ThreadFilter,"Dancer value invalid.",__FILE__,ReadValue,(int)DancerId,RpError,(int)Read_Dryer_Status(DancerId),0); MotorFailedSample[index]++; return OK; } } KeepReadValue = ReadValue; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (abs(TranslatedReadValue) > 0x2000) { TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment } if ((index == POOLER_MOTOR)||((index == FEEDER_MOTOR)&&(DancersCfg[DancerId].assemblydirectionright == true))) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } #ifdef BTSR_ROTATED_WINDER_TFU if (index == WINDER_MOTOR) TranslatedReadValue = (-1*TranslatedReadValue); #endif #ifdef FOUR_WINDERS if (index == WINDER_MOTOR) { c++; } if (index == WINDER_2_MOTOR) { c++; } if (index == WINDER_3_MOTOR) { c++; } if (index == WINDER_4_MOTOR) { c++; } //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR)) /*if (index >= WINDER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); }*/ #endif if (index == WINDER_MOTOR) { //pooler dancer is right sided: data is opposite JobCounter++; } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; #ifdef TEST_LONGER_PID_THREAD else // test: handle tension once in pvinputfilterfactormode milliseconds return OK; #endif for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode; //Stop Execution if the dancer moves too much if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) { keepdata = false; usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint); ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0); //JobAbortedByUser = true; ThreadControlActive = false; //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId; #ifdef FOUR_WINDERS if (DancerId>HARDWARE_DANCER_2) JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0; #endif SendJobProgress(0.0,0,false, TMessage); strcpy(AlarmReasonStr,TMessage); //EndState(CurrentJob,TMessage ); SendSegmentFail(); /*switch (index) { case POOLER_MOTOR: AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,true); break; case FEEDER_MOTOR: AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,true); break; case WINDER_MOTOR: AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,true); break; }*/ ReportWithPackageFilter(ThreadFilter,"Dancer Failure",__FILE__,DancerId,(int)avreageSampleValue,RpError,(int)JobCounter,0); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit { NormalizedError = (-1*NormalizedError); } MotorControlConfig[index].m_mesuredParam = NormalizedError; DancerError[DancerId] = NormalizedError; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); /*else { //KeepNormalizedError = NormalizedError; }*/ /*if ((JobCounter % 100) == 0) { //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit //{ // ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); //} /`*if (JobCounter >= 3000) { MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue); MotorSpeedSamplePointer[index]++; if (MotorSpeedSamplePointer[index] >= MAX_CONTROL_SAMPLES) MotorSpeedSamplePointer[index] = 0; for (i=0;i<MAX_CONTROL_SAMPLES;i++) avreageMotorSampleValue += MotorSpeedSamples[index][i]; avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES; //ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0); OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue; }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (index < WINDER_MOTOR) calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; //if (0)//(JobCounter % 1000 == 0) #ifdef FOUR_WINDERS if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true) { if (index >= WINDER_MOTOR) { // FirstCalcInJob = false; usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1, TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR])); ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0); } JobCounter++; } #endif #ifndef TEST_PID_THREAD if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) #else if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit #endif { if (calculated_speed>5.0) { /*if (calculated_speed>(CurrentControlledSpeed[index]+100)) { ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,index,0); calculated_speed=CurrentControlledSpeed[index]+100; }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } else { if (calculated_speed<0) ReportWithPackageFilter(ThreadFilter,"Negative speed",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); } /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit { ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); }*/ #ifdef TEST_PID_THREAD if ((JobCounter % 2000) == index*100) //if (keepdata == true) { /*calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; MotorId[controlIndex] = index; readValue[controlIndex] = ReadValue; AveragereadValue[controlIndex] = avreageSampleValue; calculatedspeed[controlIndex] = calculated_speed; NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam; mIntegral[controlIndex] = MotorControlConfig[index].m_integral; timestamp[controlIndex] = msec_millisecondCounter;*/ usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed); ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); //Task_sleep(100); //if (controlIndex++>=MAX_THREAD_CONTROL_LOG) // controlIndex = 0; } #endif } else { MotorFailedSample[index]++; //LOG_ERROR(index,"No change in speed"); } } return OK; } //******************************************************************************************************************** double ThreadGetMotorSpeed(threadMotorsEnum MotorId) { return CurrentControlledSpeed[MotorId]; } //******************************************************************************************************************** double ThreadGetMotorCalculatedError(int DancerId) { switch (DancerId) { case FEEDER_DANCER: return (double)MotorControlConfig[FEEDER_MOTOR].m_calculatedError; case POOLER_DANCER: return (double)MotorControlConfig[POOLER_MOTOR].m_calculatedError; case WINDER_DANCER: return (double)MotorControlConfig[WINDER_MOTOR].m_calculatedError; } return 0; } //******************************************************************************************************************** uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams) { if (ThreadParams == NULL) { return OK; } if((ThreadParams->bypassrockers != true)&&(ThreadParams->bypassrockers != false)) { ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0); return OK; } if ((ThreadParams->feederp>100000)||(ThreadParams->feederi>100000)) { ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0); return OK; } if(ThreadParams->feederp) MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp; if(ThreadParams->feederi) MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi; if(ThreadParams->feederd) MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd; if(ThreadParams->pullerp) MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp; if(ThreadParams->pulleri) MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri; if(ThreadParams->pullerd) MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd; if(ThreadParams->winderp) { MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp; #ifdef FOUR_WINDERS MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp; MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp; MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp; #endif } if(ThreadParams->winderi) { MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi; #ifdef FOUR_WINDERS MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi; MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi; MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi; #endif } if(ThreadParams->winderd) { MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd; #ifdef FOUR_WINDERS MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd; MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd; MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd; #endif } ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0); if(ThreadParams->bypassrockers) Thread_Rockers_Bypass = true; return OK; } bool RTFU_Up = false; uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag) { //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency); Report("Release_Right_TFU_TensionCallback",__FILE__,deviceID,0,RpMessage,0,0); MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Hiz); //per L6470 errata between mov and run commands return OK; } uint32_t Release_Right_TFU_Tension() { uint32_t status = OK; #ifndef BTSR_NO_FEEDER_TFU if (RTFU_Up == true) { Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0); RTFU_Up = false; status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000); } #endif return status; } int SecondFeederCorrection = 4; int PrepareWaitCount = 0; uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue) { #ifndef BTSR_NO_FEEDER_TFU MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands Report("Adjust_Right_TFU_Tension_2ndCallback x more steps",__FILE__,__LINE__,MotorId,RpMessage,SecondFeederCorrection,0); if (JobIsActive()==false) { Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0); Release_Right_TFU_Tension(); } if (PrepareWaitCount) { ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); PrepareWaitCount--; } if ((PrepareWaitCount == 0)&&(PrepareState == true)) { PrepareState = false; ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); PrepareReady(Module_Thread,ModuleDone); } #endif return OK; } uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue) { #ifndef BTSR_NO_FEEDER_TFU Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0); MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,SecondFeederCorrection* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Adjust_Right_TFU_Tension_2nd_Callback,1000); RTFU_Up = true; #endif return OK; } uint32_t Adjust_Right_TFU_Tension(double tension) { uint32_t status = OK; #ifndef BTSR_NO_FEEDER_TFU if (tension > 0.5) //0 = lower position, 1 = high position { if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT) { PrepareWaitCount++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000); Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,PrepareWaitCount,0); } } #endif return status; } uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension) { //MotorStop(MotorId,Hard_Hiz); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0); if (PrepareWaitCount) { PrepareWaitCount--; } if ((PrepareWaitCount == 0)&&(PrepareState == true)) { PrepareState = false; ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0); PrepareReady(Module_Thread,ModuleDone); } return OK; } uint32_t ThreadPrepare_Tension (int DancerId, double tension) { int current, request = (int)tension,movement; int HW_Motor_Id; bool direction; uint32_t status = OK, address = 0; switch (DancerId) { case HARDWARE_DANCER_TYPE__LeftDancer: address = EEPROM_WINDER_TENSION_POSITION; HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER1; break; case HARDWARE_DANCER_TYPE__MiddleDancer: if (Is_PP_Machine() == false) //LP machine - old LTFU return OK; address = EEPROM_PULLER_TENSION_POSITION; HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2; break; case HARDWARE_DANCER_TYPE__RightDancer: return Adjust_Right_TFU_Tension(tension); //break; default: return ERROR; } if (tension < 100) return OK; //do not handle tension of zero status |= MCU_E2PromRead(address,&current); if ((status!= OK )||(current == 0xFFFF)) return status; if (abs(current - request)<100) return status; else { if (current < request) //go down { direction = MotorsCfg[HW_Motor_Id].directionthreadwize; movement = request - current; } else { direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize; movement = current - request; } MotorSetMaxSpeed (HW_Motor_Id, 800); MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000); PrepareWaitCount++; ReportWithPackageFilter(ThreadFilter,"PrepareWaitCount",__FILE__,PrepareWaitCount,current,RpWarning,request,0); status |= MCU_E2PromProgram(address,request); } if (DancerId == HARDWARE_DANCER_TYPE__LeftDancer) { usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d", DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount); ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,current,RpWarning,request,0); } else { usnprintf(TMessage, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d", DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount); ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,address,current,RpWarning,request,0); } return status; } //******************************************************************************************************************** bool SkipOpenLids = false; uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i,i, HW_Motor_Id, Pid_Id; JobTicket* JobTicket = JobDetails; uint32_t status = OK; CurrentSegmentId = 0; float temp_dt = 0; JobCounter = 0; TotalProcessedLength = 0.0; PoolerTotalProcessedLength = 0.0; InitialProcess = true; initialpos = 0xFFFF; Poolerinitialpos = 0xFFFF; PrepareState = true; #ifdef UFEEDER_BTSR int application = JobTicket->btsrparameters->btsrapplicationtype; int type = JobTicket->btsrparameters->btsryarntype; uint16_t tension = (uint16_t)(JobTicket->btsrparameters->feedingtension*10); uint16_t tension_err = (uint16_t)(JobTicket->btsrparameters->tensionerror*10); uint16_t alarm_time = 5; //no parameter threadlengthfactor = JobTicket->btsrparameters->threadlengthfactor; #endif AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,false); AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false); AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,false); // status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint); // double feedertension = 0; // double pullertension = 0; // double windertension = 0; EnableLubrication = JobTicket->enablelubrication; EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; PrepareWaitCount = 0; #ifdef UFEEDER_BTSR status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, JobTicket->btsrparameters->exittension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)JobTicket->btsrparameters->exittension,0); BTSR_RML_Settings(RUFeeder1, application, type, tension, tension_err, alarm_time); BTSR_Reset_Length(RUFeeder1, HIGHEST); BTSR_Read_Length(RUFeeder1, HIGHEST); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension BTSR",__FILE__,application,type,RpError,tension,tension_err); #else status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); #endif #ifndef BTSR_NO_PULLER_TFU status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); #endif #ifndef BTSR_NO_FEEDER_TFU status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0); #endif FirstCalcInJob = true; if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING; } else { ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop); /*if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); //PrepareReady(Module_Thread,ModuleFail); //return ERROR; } if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; // usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); //PrepareReady(Module_Thread,ModuleFail); //return ERROR; }*/ // if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false)) if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)&&(JoggingJobActive == false)) { if(Head_Type != HEAD_TYPE_ARC) { ReportWithPackageFilter(ThreadFilter,"Dyeing head is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpError,LIMIT,0); if (SkipOpenLids == false) { JobEndReason = JOB_LIDS_OPEN; usnprintf(AlarmReasonStr, 100, "Dyeing head is open!!!"); PrepareReady(Module_Thread,ModuleFail); return ERROR; } } } if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) { if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } } // if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false)) if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)&&(JoggingJobActive == false)) { ReportWithPackageFilter(ThreadFilter,"Dryer lid is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpError,LIMIT,0); if (SkipOpenLids == false) { JobEndReason = JOB_LIDS_OPEN; usnprintf(AlarmReasonStr, 100, "Dryer lid is open!!!"); PrepareReady(Module_Thread,ModuleFail); return ERROR; } } //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ #ifdef FOUR_WINDERS if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR; if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR; if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR; #endif MotorControlConfig[Motor_i].m_params.MAX = 1; MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; MotorControlConfig[Motor_i].m_ingnoreValue = MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; MotorControlConfig[Motor_i].m_isReady = true; MotorControlConfig[Motor_i].m_mesuredParam = 0; MotorControlConfig[Motor_i].m_preError = 0; MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage HandleJobThreadControlParameters(JobTicket->threadparameters); //OVERRIDES CONFIGURATION PARAMETERS!!! temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; MotorTiming[Motor_i] = (int)temp_dt; if (MotorTiming[Motor_i]) { MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0); } ////////////////////////////////////////////////// for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) { //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled MotorSamples[Motor_i][i] = 0; if (Motor_i == FEEDER_MOTOR) MotorSamples[Motor_i][i] = -500; // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) // MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; //MotorSpeedSamples[Motor_i][i] = 0; } MotorSamplePointer[Motor_i] = 0; ///////////////////////////////////////////////////// MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); #ifndef BTSR_NO_FEEDER_TFU if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (SpeedControlId != 0xFF) { RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); SpeedControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } #endif //#ifndef BTSR_NO_PULLER_TFU if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (PoolerSpeedControlId != 0xFF) { if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); PoolerSpeedControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } //#endif #ifndef BTSR_NO_FEEDER_TFU if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); #endif } #endif #ifndef BTSR_NO_PULLER_TFU if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } #endif if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { ReportWithPackageFilter(ThreadFilter,"Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } #ifdef FOUR_WINDERS if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0); if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } #ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } #endif } #ifdef TEST_PID_THREAD testDancersControl(); #endif if (PrepareWaitCount == 0) PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions return status; } uint32_t UpdatePidDuringRun(HardwarePidControl *request) { int Motor_i = MAX_THREAD_MOTORS_NUM,i; double temp_dt; for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) { if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype) { Motor_i = i; break; } } if (Motor_i == MAX_THREAD_MOTORS_NUM) return ERROR; if (request->has_derivativetime == true) { MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0); } if (request->has_proportionalgain == true) { MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0); } if (request->has_integraltime == true) { MotorControlConfig[Motor_i].m_params.Ki = request->integraltime; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0); } if (request->has_epsilon == true) { MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon; ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0); } if (request->has_controloutputtype == true) { MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype; temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; MotorTiming[Motor_i] = (int)temp_dt; if (MotorTiming[Motor_i]) { MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; } ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0); } ////////////////////////////////////////////////// return OK; } void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) // double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround * MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius); double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround) / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius); //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; InitialDryerSpeed = 0.0; CurrentControlledSpeed[Motor_i] = (int) motor_speed; if (process_speed > 1) ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)motor_speed,RpWarning,process_speed,0); // for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) // MotorSpeedSamples[Motor_i][i] = motor_speed; } } void ThreadPreSegmentEnded(void) { InitialProcess = false; REPORT_MSG (0,"First ThreadPreSegmentEnded"); PreSegmentReady(Module_Thread,ModuleDone); } int DrierDivider = 20; uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag) { InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider); if (InitialDryerSpeed >= OriginalMotorSpd_2PPS[DRYER_MOTOR]) { InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]; SafeRemoveControlCallback(ControlIdtoMotorId[DRYER_MOTOR], ThreadDryerRampUp ); ControlIdtoMotorId[DRYER_MOTOR] = 0xFF; //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp end",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); } if (InitialDryerSpeed == 0) { //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp Stopped",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); return OK; } MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0); return OK; } bool Set_Thread_Rockers_Bypass (int value) { if (value == 0) Thread_Rockers_Bypass = false; else Thread_Rockers_Bypass = true; return Thread_Rockers_Bypass; } //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { //set the speed only before the first segment, speed is constant across all job segments and intersegments //JobSegment* Segment = SegmentDetails; float process_speed = dyeingspeed; if (dyeingspeed == 0) { ReportWithPackageFilter(ThreadFilter,"job speed zero.",__FILE__,__LINE__,(int)dyeingspeed,RpError,(int)SegmentId,0); return ERROR; } ReportWithPackageFilter(ThreadFilter,"ThreadPreSegmentState",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)SegmentId,0); if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job) { SetOriginMotorSpeed(process_speed); ThreadControlActive = true; if (PrepareWaitCount == 0) PrepareState = false; PullerSpeedIndex = 0; FeederSpeedIndex = 0; #ifndef TEST_PID_THREAD // set the new speed in the dryer motor to the speed of the new segment if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { DrierDivider = dyeingspeed/3; //ramp up drier in 5 cm/sec steps } else { DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps } ReportWithPackageFilter(ThreadFilter,"Dryer ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider; MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0); #endif #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogInit(); #endif if (Thread_Rockers_Bypass == false) { if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0) { MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1); } if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].maxfrequency > 0) { MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1); } } if (EnableLubrication == true) { IDS_StartLubrication(); } } // activate control fr all motors //set speed for both rocker motors //wait for all motors to get to the required speed (set the target speed for the control to check) //call the job state machine when the thread system is ready if ((InitialProcess==false) && (EnableIntersegment == true)) //&& (IntersegmentLength >= 1.0)) //fix - avoid intersegment length 0 {//add initial presegment and cleaning before first segment ThreadUpdateProcessLength (IntersegmentLength,(void *)ThreadInterSegmentEnded); REPORT_MSG (IntersegmentLength," ThreadPreSegmentState IntersegmentLength"); SegmentState = false; PreSegmentState = true; DTSState = false; } /*else if (InitialProcess==true) { ThreadUpdateProcessLength (dryerbufferMeters,(void *)ThreadPreSegmentEnded); REPORT_MSG (dryerbufferCentimeters," ThreadPreSegmentState DTS length (sample)"); SegmentState = false; PreSegmentState = true; DTSState = false; }*/ else { ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); JobCounter = 0; InitialProcess = false; } return OK; } int REPSegmentId = 0; void SendSegmentFail(void) { if (SegmentState == true) SegmentReady(Module_Thread,ModuleFail); else if (PreSegmentState == true) PreSegmentReady(Module_Thread,ModuleFail); else if (DTSState == true) DistanceToSpoolReady(Module_Thread,ModuleFail); } void ThreadInterSegmentEnded(void) { REPORT_MSG (REPSegmentId,"ThreadInterSegmentEnded"); //ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); } void ThreadSegmentEnded(void) { REPORT_MSG (REPSegmentId," ThreadSegmentEnded"); SegmentReady(Module_Thread,ModuleDone); } void ThreadDistanceToSpoolEnded(void) { REPORT_MSG (REPSegmentId," ThreadDistanceToSpoolEnded"); DistanceToSpoolReady(Module_Thread,ModuleDone); } double seglength = 0.0; //******************************************************************************************************************** uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId) { JobSegment* Segment = SegmentDetails; REPSegmentId = SegmentId; seglength = Segment->length; CurrentSegmentId = SegmentId; REPORT_MSG (seglength," ThreadSegmentState"); ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded); SegmentState = true; PreSegmentState = false; DTSState = false; return OK; } //******************************************************************************************************************** uint32_t ThreadDistanceToSpoolState(void ) { seglength = dryerbufferMeters; REPORT_MSG (seglength,"ThreadDistanceToSpoolState"); //#ifdef FEEDER_LENGTH_CALCULATION ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded); /*#else ThreadUpdateProcessLength (0,(void *)NULL); //move DTS to job start DistanceToSpoolReady(Module_Thread,ModuleDone); #endif*/ SegmentState = false; PreSegmentState = false; DTSState = true; return OK; } char Endstr[150]; //******************************************************************************************************************** uint32_t ThreadEndState(void ) { int Motor_i; ThreadControlActive = false; uint32_t status = OK; usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); ThreadUpdateProcessLength (0.0,(void *)NULL); //TotalProcessedLength = 0.0; SetOriginMotorSpeed(0); #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogClose(); #endif if (SpeedControlId != 0xFF) { if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)ThreadLengthCBFunction,RpError,(int)SpeedControlId,0); SpeedControlId = 0xFF; } if (PoolerSpeedControlId != 0xFF) { if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)PoolerThreadLengthCBFunction,RpError,(int)PoolerSpeedControlId,0); PoolerSpeedControlId = 0xFF; } for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); if(status == OK) ControlIdtoMotorId[Motor_i] = 0xFF; else ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0); } } Task_sleep(100); for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); Release_Right_TFU_Tension(); IDS_StopLubrication(); return OK; } void ThreadCheckArcHeadCovers(void) { if ((Head_Type == HEAD_TYPE_ARC) && (JoggingJobActive == false)) { if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) { ReportWithPackageFilter(ThreadFilter,"Dyeing head arc tunnel is open!!!",__FILE__,__LINE__,0,RpError,LIMIT,0); } } } //******************************************************************************************************************** void ThreadStartPrinting(void) { //PrintingIterate(); } //******************************************************************************************************************** //******************************************************************************************************************** void ThreadStopPrinting(void) { //PrintingIterate(); }