aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Visual_Studio/SideChains/RealTimeGraphEx/FastGraphs/RealTimeGraphExMultiLineScroll.cs
blob: 7cd7f74f1b6559c5fb56279fc3e0fa06ab912728 (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
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using RealTimeGraphEx.Models;
using RealTimeGraphEx.DataSeries;
using RealTimeGraphEx.Controllers;
using System.Runtime.ExceptionServices;

namespace RealTimeGraphEx.FastGraphs
{
    public class RealTimeGraphExMultiLineScroll : RealTimeGraphExMultiBase
    {
        private GraphMultiController _lastController;

        #region Cross Thread Fields

        protected List<ConcurrentpointsList> _graphPolygons;
        protected Color _stroke;
        protected Color _fill;
        protected bool _fillGraph;
        protected List<double> _markers;

        #endregion

        #region Constructors

        public RealTimeGraphExMultiLineScroll()
            : base()
        {
            _graphPolygons = new List<ConcurrentpointsList>();
            _graphController = new GraphMultiController();
            _markers = new List<double>();
        }

        #endregion

        #region Properties

        /// <summary>
        /// Gets or sets the graph strokes color.
        /// </summary>
        public Color Stroke
        {
            get { return (Color)GetValue(StrokeProperty); }
            set { SetValue(StrokeProperty, value); }
        }
        public static readonly DependencyProperty StrokeProperty =
            DependencyProperty.Register("Stroke", typeof(Color), typeof(RealTimeGraphExMultiLineScroll), new PropertyMetadata(Colors.Black, new PropertyChangedCallback(CrossModelChanged)));

        /// <summary>
        /// Gets or sets the graph fill color.
        /// </summary>
        public Color Fill
        {
            get { return (Color)GetValue(FillProperty); }
            set { SetValue(FillProperty, value); }
        }
        public static readonly DependencyProperty FillProperty =
            DependencyProperty.Register("Fill", typeof(Color), typeof(RealTimeGraphExMultiLineScroll), new PropertyMetadata(Colors.Transparent, new PropertyChangedCallback(CrossModelChanged)));

        /// <summary>
        /// Gets or sets whether the graph will be rendered using the Fill color property.
        /// </summary>
        public bool FillGraph
        {
            get { return (bool)GetValue(FillGraphProperty); }
            set { SetValue(FillGraphProperty, value); }
        }
        public static readonly DependencyProperty FillGraphProperty =
            DependencyProperty.Register("FillGraph", typeof(bool), typeof(RealTimeGraphExMultiLineScroll), new PropertyMetadata(false, new PropertyChangedCallback(CrossModelChanged)));

        #endregion

        #region Override Methods

        protected override void OnControllerChanged()
        {
            if (_graphController != null)
            {
                if (_lastController != _graphController)
                {
                    _graphPolygons.Clear();

                    _graphController.RegisterMethods(ClearGraph, StartPushThread, SetPaused, ChangeRenderMode, OnPushMarker);

                    foreach (var dataSeries in _graphController.DataSeriesCollection)
                    {
                        _graphPolygons.Add(new ConcurrentpointsList());
                    }


                    _lastController = _graphController;
                }
            }
        }

        protected override void OnSizeChanged(object sender, SizeChangedEventArgs e)
        {
            OnSetCrossThreadFields();
        }

        protected override void OnSetCrossThreadFields()
        {
            base.OnSetCrossThreadFields();

            this.Dispatcher.Invoke(() =>
            {
                _maxPoints = MaxPoints;
                _scaleFactor = _width / _maxPoints;

            }, System.Windows.Threading.DispatcherPriority.Send);
        }

        protected override void OnClearGraph()
        {
            _graphPolygons.ForEach(x => x.Clear());
            _graphController.ClearPoints();

            base.OnClearGraph();
        }

        protected internal override void OnRenderGraph()
        {
            if (_graphController != null && _graphController.dataSeriesCollection.Count > 0 && _graphController.TotalPoints > 0 && _width > 1 && _height > 1)
            {
                var pointsCollection = _graphController.GetAndClearAllPoints();

                if (_useAutoRange && pointsCollection.Count > 0)
                {
                    Dispatcher.Invoke(() =>
                    {
                        Maximum = pointsCollection.SelectMany(x => x).Concat(_graphPolygons.SelectMany(x => x.GetPoints())).Max();
                        Minimum = pointsCollection.SelectMany(x => x).Concat(_graphPolygons.SelectMany(x => x.GetPoints())).Min();
                    });
                }

                if (!_isPaused)
                {

                    Parallel.For(0, pointsCollection.Count, (i) =>
                    {
                        double seriesCounter = xValueCounter;

                        for (int j = 0; j < pointsCollection[(int)i].Count; j++)
                        {
                            double value = pointsCollection[(int)i][j];
                            NormalizeValue(ref value);

                            _graphPolygons[i].Add(value);
                            seriesCounter += _scaleFactor;
                        }

                    });

                    xValueCounter += (pointsCollection[0].Count * _scaleFactor);

                    if (_graphPolygons[0].Count > _maxPoints + _graphController.dataSeriesCollection[0].Points.Count)
                    {
                        xValueCounter -= (_scaleFactor * (_graphPolygons[0].Count - (_maxPoints + _graphController.dataSeriesCollection[0].Points.Count)));


                        Parallel.ForEach(_graphPolygons, (polygon) =>
                        {
                            polygon.RemoveFromStart(polygon.Count - (_maxPoints + _graphController.dataSeriesCollection[0].Points.Count));
                        });
                    }
                }

                updateCounter++;

                if (updateCounter >= 1)
                {
                    updateCounter = 0;

                    if (!_disableRendering)
                    {
                        WriteableBitmap bmp = BitmapFactory.New((int)_mainWidth, (int)_mainHeight);
                        bmp.Clear(Colors.Transparent);

                        OnDrawVisuals(bmp);

                        bmp.Freeze();

                        img.Dispatcher.BeginInvoke(new Action(() =>
                        {
                            img.Source = bmp;
                        }), System.Windows.Threading.DispatcherPriority.Send);
                    }
                }
            }
        }

        protected override void OnZoomingComplete(Point transformOrigin, double scaleX, double scaleY)
        {
            base.OnZoomingComplete(transformOrigin, scaleX, scaleY);

            if (IsPaused)
            {
                WriteableBitmap bmp = BitmapFactory.New((int)_width, (int)_height);
                bmp.Clear(Colors.Transparent);

                OnDrawVisuals(bmp);

                bmp.Freeze();

                img.Dispatcher.BeginInvoke(new Action(() =>
                {
                    img.Source = bmp;
                }), System.Windows.Threading.DispatcherPriority.Send);
            }
        }

        protected override void OnDragging(System.Windows.Controls.Primitives.DragDeltaEventArgs e)
        {
            base.OnDragging(e);
        }

        #endregion

        #region Virtual Methods

        [HandleProcessCorruptedStateExceptions]
        protected virtual void OnDrawVisuals(WriteableBitmap bmp)
        {
            for (int i = 0; i < _graphPolygons.Count; i++)
            {
                Color stroke = _graphController.dataSeriesCollection[i].GetStrokeColor() != null ? _graphController.dataSeriesCollection[i].GetStrokeColor().Value : _stroke;
                Color? fill = _graphController.dataSeriesCollection[i].GetFillColor();
                _fillGraph = fill != Colors.Transparent && fill != null;

                var polygon = _graphPolygons[i];
                double scale = _width / (polygon.Count - 1);

                if (polygon.Count > 0)
                {
                    if (_fillGraph && _graphController.dataSeriesCollection[i].isVisible) //Fill Graph
                    {
                        bmp.FillPolygon(polygon.ToPolygonPointsFill(_offSetX, _offSetY, _width, _height, scale, ConvertYToImageYFliped), fill.Value);
                    }

                    if (_graphController.dataSeriesCollection[i].isVisible)
                    {
                        DrawPolyline(bmp, polygon.ToPolygonPoints(_offSetX, _offSetY, scale, ConvertYToImageYFliped), stroke);
                    }
                }


                for (int j = 0; j < _markers.Count; j++)
                {
                    int x = (int)((_markers[j] * scale) + _offSetX);
                    bmp.DrawLine(x, 0, x, (int)_mainHeight, Colors.Silver);
                }
            }
        }

        public virtual List<double> GetCurrentPointsOnGraph(int seriesIndex)
        {
            return _graphPolygons[seriesIndex].GetPoints();
        }

        protected virtual void OnPushMarker()
        {

        }

        #endregion

    }
}
nclude <DataDef.h> #include "include.h" #include "thread_ex.h" #include "Common/SWUpdate/FileSystem.h" #include "StateMachines/Printing/PrintingSTM.h" #include "Modules/thread/thread.h" #include "Modules/Control/control.h" #include "Modules/General/process.h" #include "Modules/control/pidalgo.h" #include "Modules/AlarmHandling/AlarmHandling.h" #include "Modules/heaters/heaters_ex.h" #include "Modules/General/buttons.h" #include "StateMachines/Initialization/InitSequence.h" #include "PMR/Hardware/HardwareMotor.pb-c.h" #include "PMR/Hardware/HardwareMotorType.pb-c.h" #include "PMR/Hardware/HardwareDancerType.pb-c.h" #include "PMR/Printing/ProcessParameters.pb-c.h" #include "drivers/Flash_Memory/fatfs/ff.h" #include "drivers/I2C_Communication/DAC/Blower.h" #include "drivers/SSI_Comm/Dancer/Dancer.h" #include "drivers/motors/motor.h" #include "drivers/Flash_ram/MCU_E2Prom.h" #include "Drivers/I2C_Communication/I2C.h" #include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h> #include <Drivers/I2C_Communication/I2C_Task.h> #include "drivers/FPGA/FPGA_SPI_Comm.h" #include "drivers/FPGA/Full_Vme/FPGA_Programming_Up.h" #include "PMR/Stubs/StubHeatingTestRequest.pb-c.h" #include "PMR/Stubs/StubHeatingTestResponse.pb-c.h" #include "PMR/Stubs/StubHeatingTestPollRequest.pb-c.h" #include "PMR/Stubs/StubHeatingTestPollResponse.pb-c.h" #include "PMR/ThreadLoading/StartThreadLoadingRequest.pb-c.h" #include "PMR/ThreadLoading/StartThreadLoadingResponse.pb-c.h" #include "PMR/ThreadLoading/ContinueThreadLoadingRequest.pb-c.h" #include "PMR/ThreadLoading/ContinueThreadLoadingResponse.pb-c.h" #include "PMR/ThreadLoading/TryThreadLoadingRequest.pb-c.h" #include "PMR/ThreadLoading/TryThreadLoadingResponse.pb-c.h" #include "PMR/ThreadLoading/StopThreadLoadingRequest.pb-c.h" #include "PMR/ThreadLoading/StopThreadLoadingResponse.pb-c.h" #include "PMR/ThreadLoading/AttemptThreadJoggingResponse.pb-c.h" //#include <stdint.h> //#include <stdbool.h> static char* LoadStagesStr[15] = { /*00*/ "Initialization", /*01*/ "Heating stop", /*02*/ "Rockers opening", /*03*/ "Dryer unloading", /*04*/ "Lids opening", /*05*/ "Dancers opening", /*06*/ "Initial tension building", /*07*/ "Rockers closing", /*08*/ "Dancers closing", /*09*/ "Lids closing", /*10*/ "Feeder jogging", /*11*/ "Dryer loading", /*12*/ "Heating resuming", /*13*/ "Thread jogging" , /*14*/ "END"}; /* typedef enum { 00 THREAD_LOAD_INIT, 01 THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, 02 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE 03 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY 04 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID 05 THREAD_LOAD_LIFT_DANCERS, 06 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND 07 THREAD_LOAD_CLOSE_ROCKERS, 08 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD 09 THREAD_LOAD_CLOSE_LIDS, 10 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION 11 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER 12 THREAD_LOAD_RESUME_HEATING, 13 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING 14 THREAD_LOAD_END }THREAD_LOAD_STAGES_ENUM;*/ THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT; ProcessParameters ProcessParametersClear,ProcessParametersRecover; uint32_t LoadStatus = OK; char LoadErrorMsg[100]; int32_t keepmicrostep; int32_t keepkvalrun,keepkvalhold,kval_upperValue; uint8_t CallbackCounter = 0; uint8_t TimeoutsCounter = 0; bool SecondTry = false,TryAgain = false; int MessageState = 0; //0 - none, 1 = start, 2 = continue uint32_t UnloadingStart = 0; uint8_t NumberOfDrierLoaderCycles = 0; uint32_t status = OK; uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF,ThreadLoadControlId = 0xFF; uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF; uint32_t LoadCounter = 0; double TotalLoadedLen = 0.0; double LengthCalculationMultiplier; float numberOfSteps = 0; float numberOfCycles = 0; double DrierPrevLocation = 0; int DrierCenterLocation = 0; FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_RIGHT,GPI_LS_SCREW_LEFT}; int Screw_Dir = false; bool InitCalled = false; uint32_t LoadArmRounds; int CurrentlyLoaded = 0; uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages); uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue); uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue); uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t ReadValue); uint32_t Thread_Load_Dryer_UnLoading(void); uint32_t ThreadLoadingReport(void); uint32_t ThreadLoadingRestartReport(void); //sending after a failure in the finalizing stage bool ThreadLoaded(void); bool ThreadLoadingActive(void) { if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END)) return true; else return false; } uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage) { //void* buffer = NULL; //uint32_t Bytes = 0; //FRESULT Fresult = FR_OK; REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Init"); load.color = BLINK; LoadStatus = OK; //LoadArmRounds = 0xFF; //EEPROM_STORAGE_DRYER_CYCLES MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,LoadArmRounds,RpWarning,Read_Dryer_ENC_Position(),0); StopInitSequence(); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); if (Stage == THREAD_LOAD_INIT) { LoadStages++; ThreadLoadStateMachine(LoadStages); } else { Report("Thread_Load_Init another state ",__FILE__,__LINE__,Stage,RpMessage,LoadArmRounds,0); } InitCalled = true; return OK; } uint32_t Thread_Load_Reduce_Heat(void) { if (Is_PP_Machine()) kval_upperValue = 28; else kval_upperValue = 70; //Heaters Off, Dryer Blower Off, Blower Low, REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Reduce_Heat"); memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters)); ProcessParametersClear.dryerzone1temp = 0; ProcessParametersClear.dryerzone2temp = 0; ProcessParametersClear.dryerzone3temp = 0; ProcessParametersClear.mixertemp = 0; ProcessParametersClear.headzone1temp = 0; ProcessParametersClear.headzone2temp = 0; ProcessParametersClear.headzone3temp = 0; ProcessParametersClear.headzone4temp = 0; ProcessParametersClear.headzone5temp = 0; ProcessParametersClear.headzone6temp = 0; ProcessParametersClear.headzone7temp = 0; ProcessParametersClear.headzone8temp = 0; ProcessParametersClear.headzone9temp = 0; ProcessParametersClear.headzone10temp = 0; ProcessParametersClear.headzone11temp = 0; ProcessParametersClear.headzone12temp = 0; ProcessParametersClear.rblowertemp = 0; ProcessParametersClear.lblowertemp = 0; ProcessParametersClear.rblowerflow = 0; ProcessParametersClear.lblowerflow = 0; ProcessParametersClear.dyeingspeed = 40; ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength; ProcessParametersClear.pullertension = ProcessParametersKeep.pullertension; ProcessParametersClear.windertension = ProcessParametersKeep.windertension; if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Reduce_Heat failed"); strcpy(LoadErrorMsg,"Thread_Load_Reduce_Heat failed"); load.color = fastBILNK; LoadStatus = ERROR; TryAgain = true; //ThreadLoadButton(LoadStages); } else { if(Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD) { WHS_Set_SetPoint_Q_value(headairflow); } else { Turn_the_Blower_On();//Turn on with the Default_Voltage if (BlowerCfg.heatingvoltage) Control_Voltage_To_Blower(BlowerCfg.heatingvoltage); else Control_Voltage_To_Blower(BlowerCfg.voltage-500); } Control_Dryer_Fan(STOP,75);//use START or STOP, 0 - 100% LoadStages++; ThreadLoadStateMachine(LoadStages); } return OK; } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback(uint32_t deviceID, uint32_t BusyFlag) { Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); Report("Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); NumberOfDrierLoaderCycles=0; //storeLoadArmParameters(); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); return OK; } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { uint32_t temp = Read_Dryer_ENC_Position(); int angle; ReportWithPackageFilter(DiagnosticsFilter,"Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,(int)numberOfCycles,(int)DrierPrevLocation,RpMessage,temp,0); angle = Calculate_Arm_Distance(DrierPrevLocation,temp); DrierPrevLocation = temp; if (CallbackCounter) { CallbackCounter--; } if (abs(angle)<10 ) // OK { Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); NumberOfDrierLoaderCycles=0; MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); //storeLoadArmParameters(); LoadStages++; ThreadLoadStateMachine(LoadStages); } else { load.color = fastBILNK; usnprintf(LoadErrorMsg, 100, "^Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); LoadStatus = ERROR; ThreadLoadingReport(); TryAgain = true; //ThreadLoadButton(LoadStages); } //move to exact location? // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); return OK; } uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) { Report("Thread_Load_HomingCallback",__FILE__,MotorId,LoadStages,RpMessage,CallbackCounter,0); // REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id"); if ((LoadStages == THREAD_LOAD_CLOSE_ROCKERS)||(LoadStages == THREAD_LOAD_LIFT_ROCKERS)) { if((MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)||(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)) { if(LoadStages == THREAD_LOAD_CLOSE_ROCKERS) { MotorMove(MotorId,MotorsCfg[MotorId].directionthreadwize,400); Report("Move rockers further down",__FILE__,__LINE__,MotorId,RpMessage,400,0); Task_sleep(200); } MotorSetMicroStep(MotorId, keepmicrostep); MotorSetKvalRun(MotorId, keepkvalrun); } } if (CallbackCounter) { CallbackCounter--; if (ReadValue != LIMIT) { //returned with a timeout TimeoutsCounter++; } if (CallbackCounter == 0) { if (TimeoutsCounter) { TimeoutsCounter = 0; load.color = fastBILNK; if (LoadStages == THREAD_LOAD_INITIAL_TENSION) { usnprintf(LoadErrorMsg, 100, "/Stage %s - no cone in winder!!",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); } else if (LoadStages == THREAD_LOAD_JOG_THREAD) { usnprintf(LoadErrorMsg, 100, "/Stage %s - Dryer door is open!!",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); } else { usnprintf(LoadErrorMsg, 100, "/Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[MotorId]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); } LoadStatus = ERROR; ThreadLoadingReport(); TryAgain = true; //ThreadLoadButton(LoadStages); } else { LoadStages++; LoadStatus = OK; SecondTry = false; TryAgain = false; if (LoadStages == THREAD_LOAD_CLOSE_DANCERS) { MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold); } if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call { ThreadLoadStateMachine(LoadStages); } else { load.color = colorON; Pannel_Leds(THREAD_LOAD, MODE_ON); ThreadLoadingReport(); } } } } else LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); return OK; } uint32_t Thread_Load_Open_Covers(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Open_Covers"); //Open Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, //DeActivateHeadMagnet(); CallbackCounter++; //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); int direction; if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep { direction = DRIER_LID_CLOSE; } else { direction = DRIER_LID_OPEN; } MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); if (Head_Type != HEAD_TYPE_ARC) { CallbackCounter++; if (MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 300, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,20000)==ERROR) { Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_DH_LID,NO_LIMIT); } } if (Head_Type == HEAD_TYPE_FLAT) { Report("Thread_Load_Lift right actuator",__FILE__,__LINE__,LOW,RpMessage,false,0); Trigger_Head_Actuators_Control(ACTIN, LOW,true);//UP } return OK; } uint32_t Thread_Load_Lift_Dancers(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Dancers"); /*keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold; MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);*/ CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,1); if (Is_PP_Machine() == true) //PP machine - new LTFU { CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,25000); status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,1); } if (Head_Type == HEAD_TYPE_FLAT) { Report("Thread_Load_Lift left actuator",__FILE__,__LINE__,LOW,RpMessage,false,0); Trigger_Head_Actuators_Stub(ACTOT, ENABLE, UP); } return OK; } uint32_t Thread_Load_Lift_Rockers(void) //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; /*if (keepkvalrun>=25) keepkvalrun = 25;*/ Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0); Report("Thread_Load_Lift_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0); Report("Thread_Load_Lift_right dancer",__FILE__,__LINE__,0,RpMessage,0,0); keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold; MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 22, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000); REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Rockers"); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue); Task_sleep(10); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,35000); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,35000); return OK; } uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Initial_Tension"); load.color = BLINK; CallbackCounter++; if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT) { REPORT_MSG(LIMIT, "No cone in winder"); Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_WINDER,NO_LIMIT); //return ERROR; } MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500, Thread_Load_HomingCallback,10000); return OK; } uint32_t Thread_Load_Close_Rockers(void) { keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep; keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun; /*if (keepkvalrun>=25) keepkvalrun = 25;*/ Report("Thread_Load_Close_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8); MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue); MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue); REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Rockers"); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,25000); if (Head_Type == HEAD_TYPE_FLAT) { Report("Thread_Load_Close left actuator",__FILE__,__LINE__,LOW,RpMessage,true,0); Trigger_Head_Actuators_Stub(ACTOT, ENABLE, DOWN); } return OK; } uint32_t Thread_Load_Close_Dancers(void) //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread { uint32_t current = 1; REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Dancers"); CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); Report("Thread_Load_Close dancers tension",__FILE__,__LINE__,(int)windertension,RpMessage,(int)pullertension,0); // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,&current); ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); /* if (current!=windertension) { CallbackCounter++; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000); status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension); }*/ if (Is_PP_Machine() == true) //PP machine - new LTFU { ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); /*status |= MCU_E2PromRead(EEPROM_PULLER_TENSION_POSITION,&current); if (current!=pullertension) { CallbackCounter++; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000); status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension); }*/ } if (Head_Type == HEAD_TYPE_FLAT) { Report("Thread_Load_Close right actuators",__FILE__,__LINE__,LOW,RpMessage,true,0); HeadCard_Actuators_Relocate(); } return OK; } uint32_t Thread_Load_Close_Lids(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids"); //Close Dyeing Head Cover And Dryer Lid //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2, //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4, CallbackCounter++; // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); int direction; if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep { direction = DRIER_LID_OPEN; } else { direction = DRIER_LID_CLOSE; } MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); if (Head_Type != HEAD_TYPE_ARC) { CallbackCounter++; if(MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 300, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,20000)==ERROR) { Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_DH_LID,NO_LIMIT); } } return OK; } uint32_t Thread_Load_Resume_Heating(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating"); ActivateHeadMagnet(); HeadCard_Actuators_Relocate(); if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK) { usnprintf(LoadErrorMsg, 100, "Stage %s error",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); } else { Control_Dryer_Fan(START,75);//use START or STOP, 0 - 100% LoadStages++; ThreadLoadStateMachine(LoadStages); } return OK; } uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition) { if (Condition == true) { CallbackCounter--; Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); //storeLoadArmParameters(); LoadStages++; ThreadLoadStateMachine(LoadStages); } else { CallbackCounter--; Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); //storeLoadArmParameters(); if (SecondTry == true) { LoadStages++; ThreadLoadStateMachine(LoadStages); LoadStatus = OK; SecondTry = false; TryAgain = false; } else { TimeoutsCounter = 0; load.color = fastBILNK; usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!"); Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); LoadStatus = ERROR; ThreadLoadingReport(); TryAgain = true; //ThreadLoadButton(LoadStages); } } return OK; } uint32_t Thread_Load_Jog_Feeder_To_Middle_Point(void) //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Feeder_To_Middle_Point"); CallbackCounter++; MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 400, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,6000); return OK; } uint32_t Thread_Load_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue) { Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); if(LoadingControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); LoadingControlId = 0xFF; } if (SpeedTControlId != 0xFF) { RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); SpeedTControlId = 0xFF; } Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0); if (CallbackCounter) { CallbackCounter--; if (CallbackCounter == 0) { LoadStatus = OK; SecondTry = false; TryAgain = false; LoadStages++; if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call { ThreadLoadStateMachine(LoadStages); } } } else LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); return OK; } uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { numberOfCycles++; Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,numberOfCycles,0); if ((fabs (TotalLoadedLen -DrierPrevLocation)>50 )&&(ReadValue == NOTBUSY)) { DrierPrevLocation = TotalLoadedLen ; //Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0); if (LoadStages != THREAD_LOAD_DRYER_LOADING) { return OK; } MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded); if (numberOfCycles<LoadArmRounds) { MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, numberOfSteps, Thread_Load_Dryer_Loading_Callback, 10000); } else { MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); Report("Store Number of cycles in dryer",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } } else { TryAgain = true; SecondTry = false; Report("Loading dryer - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles+CurrentlyLoaded); TimeoutsCounter = 0; CallbackCounter = 0; load.color = fastBILNK; usnprintf(LoadErrorMsg, 100, "$Stage %s - Dryer load arm timeout",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); LoadStatus = ERROR; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); if(LoadingControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); LoadingControlId = 0xFF; } if (SpeedTControlId != 0xFF) { RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); SpeedTControlId = 0xFF; } Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0); ThreadLoadingReport(); //ThreadLoadButton(LoadStages); } return OK; } uint32_t Thread_Load_Dryer_Loading(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Dryer_Loading"); if (dryerbufferlength) LoadArmRounds = (int)dryerbufferlength; if (LoadArmRounds <= 2) LoadArmRounds = 30; //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 SetOriginMotorSpeed(19); // OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; // CurrentControlledSpeed[FEEDER_MOTOR] = 1000; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&CurrentlyLoaded); Report("Thread_Load_Dryer_Loading cycles",__FILE__,LoadArmRounds,CurrentlyLoaded,RpMessage,LoadArmRounds-CurrentlyLoaded,0); if (CurrentlyLoaded) { LoadArmRounds -= CurrentlyLoaded; } if (LoadArmRounds == CurrentlyLoaded) { Report("Thread_Load_Dryer_Loading no need for cycles",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); LoadStages++; ThreadLoadStateMachine(LoadStages); return OK; } ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ; ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierPrevLocation); numberOfCycles = 0; numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; // numberOfSteps -= 100; //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0); MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1; MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1; MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime; MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain; MotorControlConfig[FEEDER_MOTOR].m_params.Ki = 0;//MotorsControl[FEEDER_MOTOR].integraltime; MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp; MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue; MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon; MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype; MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue = MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0; MotorControlConfig[FEEDER_MOTOR].m_integral = 0; MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true; MotorControlConfig[FEEDER_MOTOR].m_isReady = true; MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0; MotorControlConfig[FEEDER_MOTOR].m_preError = 0; MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); //6 seconds per round LoadingControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); Report("AddControlCallback feeder",__FILE__,__LINE__,LoadingControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0); if (SpeedTControlId != 0xFF) { RemoveControlCallback(SpeedTControlId,ThreadLoadLengthCBFunction); SpeedTControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); LengthCalculationMultiplier = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulleyradius*2*PI)/(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep); TotalLoadedLen = 0; DrierPrevLocation = 200; //initial safe value SpeedTControlId = AddControlCallback("Thread length",ThreadLoadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+HARDWARE_MOTOR_TYPE__MOTO_RDRIVING),HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,0); Tinitialpos = 0xFFFF;previousPosition = 0; currentPosition = 0; CallbackCounter++; Report("Thread_Load_Dryer_Loading",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, numberOfSteps-800, Thread_Load_Dryer_Loading_Callback, 10000); return OK; } double keepfeedertension = 0; uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue) { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_ThreadStop"); SafeRemoveControlCallback(LoadingControlId, Thread_Load_Jog_ThreadStop ); LoadingControlId = 0xFF; ThreadAbortJoggingFunc(); LoadStages++; ProcessParametersKeep.feedertension = keepfeedertension; ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_Jog_Thread(void) //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading { REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Thread"); if (Get_COVER_1_State(DryerDoor)) { CallbackCounter++; REPORT_MSG(LIMIT, "Dryer door open"); Thread_Load_HomingCallback(HARDWARE_MOTOR_TYPE__MOTO_WINDER,NO_LIMIT); return ERROR; } keepfeedertension = feedertension; ProcessParametersKeep.feedertension = 0.0; ThreadJoggingFunc(20); LoadingControlId = AddControlCallback("Load jog",Thread_Load_Jog_ThreadStop, eOneSecond*25,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); return OK; } uint32_t Thread_Load_End(void) { REPORT_MSG(LoadStages,"Loading Ended"); ThreadLoadingReport(); if(LoadingControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); LoadingControlId = 0xFF; } if(PullerControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); PullerControlId = 0xFF; } if(WinderControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } if (SpeedTControlId != 0xFF) { RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); SpeedTControlId = 0xFF; } if (ThreadLoadControlId != 0xFF) { RemoveControlCallback(ThreadLoadControlId, ThreadUpdateCallBackFunction ); ThreadLoadControlId = 0xFF; } LoadingStopArmReset(); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RDANCER); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LDANCER1); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Stop); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_WINDER,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,Hard_Hiz); if (LoadStages!=THREAD_LOAD_END) { usnprintf(LoadErrorMsg, 100, "Load sequence stopped by user on stage %d",LoadStages); LoadStatus = ERROR; } LoadStages = THREAD_LOAD_INIT; load.color = colorON; Pannel_Leds(THREAD_LOAD, MODE_ON); return OK; } uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag) { if ((LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)||(LoadStatus == ERROR)) { return OK; } Screw_Dir = 1-Screw_Dir; // movement of the screw while unloading the thread from the dryer to the spool AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,OFF); //handle alarm detection and operation status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000); return OK; } /*uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) { bool direction; numberOfCycles++; uint32_t temp = Read_Dryer_ENC_Position(); Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,AccumulatedArmMovement,temp,RpMessage,DrierPrevLocation,0); //Report("Thread_Load_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0); if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION) { Report("Thread_Load_Dryer_UnLoading_Callback wrong state",__FILE__,__LINE__,LoadStages,RpMessage,DrierPrevLocation,0); return OK; } if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round { DrierPrevLocation = temp; Report("Thread_Load_Dryer_UnLoading cycles",__FILE__,numberOfCycles,LoadArmRounds,RpMessage,0,0); if (numberOfCycles<LoadArmRounds) { MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, numberOfSteps, Thread_Load_Dryer_UnLoading_Callback, 10000); } else //done enough cycles, go to the center point { MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); Report("Store Number of cycles in dryer",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } } else //timeout or no movement { Report("Unloading dryer - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); if (SecondTry == true) MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); else MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles); if (abs(temp -DrierCenterLocation)<200) { if (temp<DrierCenterLocation) direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; else direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction,10); } else MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); TimeoutsCounter = 0; CallbackCounter = 0; if(WinderControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); load.color = fastBILNK; usnprintf(LoadErrorMsg, 100, "&Stage %s - Dryer load arm timeout",LoadStagesStr[LoadStages]); Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); Report("Dryer unloading timeout(1) or no movement",__FILE__,temp,DrierPrevLocation,RpWarning,ReadValue,0); LoadStatus = ERROR; TryAgain = true; ThreadLoadingReport(); //ThreadLoadButton(LoadStages); } return OK; }*/ uint32_t Thread_Load_Dryer_UnLoading(void) { uint32_t temp; REPORT_MSG(LoadStages, "Thread Load State Machine step Dryer Unloading"); //LoadArmRounds = 0; //uint32_t numberOfSteps = 0; CallbackCounter++; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); DrierCenterLocation = DrierPrevLocation; DrierPrevLocation = temp; UnloadingStart = msec_millisecondCounter; //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; //numberOfCycles = 0; LoadingArmReset(Thread_Load_Set_Load_Arm_To_Stopper_Callback,300000); /*if (SecondTry == true) { MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); if (LoadArmRounds == 0) //prev trial stopped { LoadArmRounds = (int)dryerbufferlength; } //LoadArmRounds = 30; } else { MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); if (LoadArmRounds == 0) //prev trial stopped { LoadArmRounds = (int)dryerbufferlength; } }*/ /*MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); DrierCenterLocation = DrierPrevLocation; DrierPrevLocation = temp; Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmRounds,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); // status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), // MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,0,1000); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, numberOfSteps-800, Thread_Load_Dryer_UnLoading_Callback, 10000); Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize; // movement of the screw while unloading the thread from the dryer to the spool status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000); //Keep Notation How Many Rotations In The Dryer //LoadArmRounds = (int)dryerbufferlength; * */ load.color = BLINK; return OK; } double percentage; uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue) { REPORT_MSG(ReadValue,"ThreadLoadStateMachine"); //ThreadLoaded(); percentage = ReadValue*7; ThreadLoadingReport(); if (LoadStages!=ReadValue) LoadStages = ReadValue; switch (ReadValue) { case THREAD_LOAD_INIT: //LoadStages++; MessageState = 1; Thread_Load_Init(ReadValue); break; case THREAD_LOAD_REDUCE_HEAT: //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, //LoadStages++; MessageState = 1; if (InitCalled == false) { REPORT_MSG(ReadValue,"Thread_Load_Init called from 1"); Thread_Load_Init(ReadValue); } else { REPORT_MSG(ReadValue,"Thread_Load_Init called from 0"); } Thread_Load_Reduce_Heat(); InitCalled = false; break; case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY //LoadStages++; //Thread_Load_Set_Load_Arm_To_Start_Position(); Thread_Load_Dryer_UnLoading(); break; /*case THREAD_LOAD_CENTER_HEAD_ROCKERS: //LoadStages++; Thread_Load_Center_Head_Rockers(); break;*/ case THREAD_LOAD_OPEN_COVERS: //OPEN DYEING HEAD COVER AND DRYER LID //LoadStages++; Thread_Load_Open_Covers(); break; case THREAD_LOAD_LIFT_DANCERS: //LoadStages++; Thread_Load_Lift_Dancers(); break; case THREAD_LOAD_LIFT_ROCKERS: //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE //LoadStages++; Thread_Load_Lift_Rockers(); break; case THREAD_LOAD_INITIAL_TENSION://CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND //LoadStages++; Thread_Load_Initial_Tension(); break; case THREAD_LOAD_CLOSE_ROCKERS: //LoadStages++; Thread_Load_Close_Rockers(); break; case THREAD_LOAD_CLOSE_DANCERS: //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD //LoadStages++; Thread_Load_Close_Dancers(); break; case THREAD_LOAD_CLOSE_LIDS: //LoadStages++; Thread_Load_Close_Lids(); break; case THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT://JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION //LoadStages++; Thread_Load_Jog_Feeder_To_Middle_Point(); break; case THREAD_LOAD_DRYER_LOADING: //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER //LoadStages++; Thread_Load_Dryer_Loading(); break; case THREAD_LOAD_RESUME_HEATING: //LoadStages++; Thread_Load_Resume_Heating(); break; case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING //LoadStages++; Thread_Load_Jog_Thread(); ThreadLoadingReport(); break; case THREAD_LOAD_END: LoadStages = THREAD_LOAD_END; percentage = 100.0; Thread_Load_End(); break; default: LOG_ERROR(LoadStages,"ERROR IN LOAD STATE MACHINE"); break; } return OK; } uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) { if (JobIsActive()||(MachineReadyForHeating == false)||(FPGABurningActive==true)) { Report("ThreadLoadButton called on job",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); return ERROR; } if (CallbackCounter) { Report("ThreadLoadButton called when the loading is active",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); return ERROR; } Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); Report("ThreadLoadButton params",__FILE__,LoadStatus,TryAgain,RpMessage,SecondTry,0); if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call MessageState = 2; ThreadLoadingReport(); if (LoadStatus == ERROR) { if ((SecondTry == false)&&(TryAgain == true)) { LoadStatus = OK; SecondTry = true; load.color = BLINK; //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) // ThreadLoadingRestartReport(); Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); ThreadLoadStateMachine(LoadStages); } else if (SecondTry == true) { SecondTry = false; Report("Calling State machine 2nd try",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); load.color = BLINK; LoadStatus = OK; if (LoadStages == THREAD_LOAD_LIFT_DANCERS) { LoadStages++; } else ThreadLoadStateMachine(LoadStages+1); } else //((SecondTry == false)&&(TryAgain == false))??????? { Report("State ERROR with wrong parameters",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); SecondTry = false; } } else { Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0); //if (LoadStages > THREAD_LOAD_INITIAL_TENSION) // ThreadLoadingRestartReport(); ThreadLoadStateMachine(LoadStages); SecondTry = false; } return OK; } uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0,prevprev; double length = 0.0; char lenstr[150]; static int counter = 0; int index = MAX_THREAD_MOTORS_NUM; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; // if (CurrentRequestedLength == 0.0) // return OK; if (index != HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) { LOG_ERROR (IfIndex, "Wrong Motor"); return 0xFFFFFFFF; } currentPosition = MotorGetPosition(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); if (currentPosition != 0) { if (Tinitialpos == 0xFFFF) { previousPosition = currentPosition; Tinitialpos = 0; } prevprev = previousPosition; if (Extended_Motor_Param[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING] == true) //powerstep driver reverses the direction positionDiff = Control_Delta_Position_Pass(previousPosition,currentPosition); else positionDiff = Control_Delta_Position_Pass(currentPosition,previousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; previousPosition = currentPosition; // total length = (position diff / full cycle) * pulley perimeter //(positionDiff/pulseperround)*((2*PI*motor_Radius) //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; length = (double)(positionDiff)*LengthCalculationMultiplier; if (counter++ % 50 == 0) { usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x TotalLoadedLen %d",(int)length*100,(int)positionDiff,previousPosition,prevprev, (int)TotalLoadedLen); // SendJobProgress(0.0,0,false, lenstr); ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalLoadedLen,RpWarning,(int)TotalLoadedLen,0); // length = 0; } } TotalLoadedLen += length; return OK; } uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle int i; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0; int index=MAX_THREAD_MOTORS_NUM; //double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; DancerId = ThreadMotorIdToDancerId[index]; /*if (ReadValue < 10) { Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); return OK; }*/ if (ReadValue == 0x3FFF) { return OK; } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); } if (abs(TranslatedReadValue) > 0x2000) { TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode; NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = 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); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit { MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); } calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } /* if (index == POOLER_MOTOR) { if ((TranslatedReadValue)<(-1200)) { if(PullerControlId != 0xFF) { Report("Puller stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0); MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); PullerControlId = 0xFF; } } } if (index == WINDER_MOTOR) { if ((TranslatedReadValue)<(-1200)) { if(WinderControlId != 0xFF) { Report("Winder stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0); MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } } }*/ /* LoadCounter++; if ((LoadCounter % 5001) == 0) { Report("MotorSpeedUpdated",__FILE__,index,abs(TranslatedReadValue),RpWarning,(int)CurrentControlledSpeed[index],0); }*/ return OK; } bool ThreadLoaded(void) { /* uint32_t ReadValue; int32_t PoolerValue,WinderValue,FeederValue; //this function tests if there is a thread in the machine. all dancers are in lowermost position then there is no thread. ReadValue = Control_Read_Dancer_Position(FEEDER_DANCER,0); FeederValue = ReadValue - DancersCfg[FEEDER_DANCER].zeropoint; Report("Feeder out",__FILE__,abs(FeederValue),ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0); ReadValue = Control_Read_Dancer_Position(WINDER_DANCER,0); WinderValue = ReadValue - DancersCfg[WINDER_DANCER].zeropoint; Report("Winder out",__FILE__,abs(WinderValue),ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0); ReadValue = Control_Read_Dancer_Position(POOLER_DANCER,0); PoolerValue = ReadValue - DancersCfg[POOLER_DANCER].zeropoint; //pooler dancer is right sided: data is opposite PoolerValue = (-1*PoolerValue); Report("Puller out",__FILE__,abs(PoolerValue),ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0); if (((PoolerValue)<(-1200))&&((WinderValue)<(-1200))&&((FeederValue)<(-1200))) { Report("Thread is out",__FILE__,(-1*PoolerValue),(-1*WinderValue),RpWarning,(-1*FeederValue),0); return true; } else*/ return false; } void ThreadLoadRequest(MessageContainer* requestContainer) { //#ifdef DEBUG_TEST_FUNCTIONS MessageContainer responseContainer; uint8_t* container_buffer; //uint32_t status = OK; StubHeatingTestRequest* request = stub_heating_test_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); StubHeatingTestResponse response = STUB_HEATING_TEST_RESPONSE__INIT; responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size); container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); int LoadStage = (int)request->dryerzone1temp; if (request->hardwarepidcontrol1!=NULL) { UpdatePidDuringRun(request->hardwarepidcontrol1); } else { LoadStages = LoadStage; ThreadLoadStateMachine(LoadStage); } /*if (status) { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR; }*/ size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); //USBCDCD_sendData(container_buffer, container_size,10); SendChars(container_buffer, container_size); stub_heating_test_request__free_unpacked(request,NULL); //#else // LOG_ERROR (-1, "Heating Control not on debug"); // return ERROR; //#endif } void ThreadLoadPollRequest(MessageContainer* requestContainer) { uint8_t* container_buffer; StubHeatingTestPollRequest* request = stub_heating_test_poll_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); StubHeatingTestPollResponse response = STUB_HEATING_TEST_POLL_RESPONSE__INIT; MessageContainer responseContainer; response.has_heater1percentage = true; response.heater1percentage = LoadStages; responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestPollResponse, requestContainer->token, false, &response, &stub_heating_test_poll_response__pack, &stub_heating_test_poll_response__get_packed_size); container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); /* { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR; }*/ size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars(container_buffer, container_size); stub_heating_test_poll_request__free_unpacked(request,NULL); } char ThreadLoadingToken[36+1] = {0}; char DefaultErrSrt[] = "OK"; uint32_t ThreadLoadingReport(void) { MessageContainer responseContainer; bool last = false; StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; if (ThreadLoadingToken[0] == 0) return OK; if (MessageState == 0) { response.has_state = true; response.state = THREAD_LOADING_STATE__None; response.errorreason = DefaultErrSrt; } else if (MessageState == 1) { response.has_state = true; if (LoadStatus == OK) { response.state = THREAD_LOADING_STATE__Preparing; if (LoadStages == THREAD_LOAD_INITIAL_TENSION) { response.state = THREAD_LOADING_STATE__ReadyForLoading; response.errorreason = DefaultErrSrt; } } else { response.state = THREAD_LOADING_STATE__PreparationError; response.errorreason = LoadErrorMsg; } } else if (MessageState == 2) { response.has_state = true; if (LoadStatus == OK) { response.state = THREAD_LOADING_STATE__Finalizing; if (LoadStages > THREAD_LOAD_JOG_THREAD) { response.state = THREAD_LOADING_STATE__Completed; response.errorreason = DefaultErrSrt; //last = true; } } else { response.state = THREAD_LOADING_STATE__FinalizationError; response.errorreason = LoadErrorMsg; } } else { Report("ThreadLoadingReport ERROR",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); return ERROR; } Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); response.has_progresspercentage = true; response.progresspercentage = percentage; //------------------------------------------------------------------------------------------- responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); responseContainer.has_continuous = true; responseContainer.continuous = true; uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); return OK; } bool ThreadLoadingRestartFlag = false; uint32_t ThreadLoadingRestartReport(void) //sending after a failure in the finalizing stage { MessageContainer responseContainer; StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; if (ThreadLoadingToken[0] == 0) return OK; if (LoadStages == THREAD_LOAD_INIT) return OK; ThreadLoadingRestartFlag = true; response.has_state = true; response.state = THREAD_LOADING_STATE__ReadyForLoading; response.errorreason = DefaultErrSrt; Report("ThreadLoadingRestartReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); //------------------------------------------------------------------------------------------- responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, false, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); responseContainer.has_continuous = true; responseContainer.continuous = true; uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); return OK; } uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer) { TryThreadLoadingResponse Cresponse = TRY_THREAD_LOADING_RESPONSE__INIT; MessageContainer responseContainer; //MessageState = 2; ThreadLoadingReport(); Report("TryThreadLoadingFunc",__FILE__,__LINE__,MESSAGE_TYPE__TryThreadLoadingResponse,RpWarning,(int)LoadStages,0); if (requestContainer) { responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size); responseContainer.continuous = false; if (JobIsActive()||(MachineReadyForHeating == false)||(FPGABurningActive==true)) { responseContainer.has_error = true; responseContainer.error = ERROR_CODE__GENERAL_ERROR; responseContainer.errormessage = "The machine is not ready for loading"; } uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); } ThreadLoadButton(LoadStages); return OK; } uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) { if(ThreadLoadingActive()) ThreadLoadingReport(); return OK; } uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer) { StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); ustrncpy (ThreadLoadingToken, requestContainer->token,36); //MessageState = 1; ThreadLoadControlId = AddControlCallback("ThreadLoad", ThreadUpdateCallBackFunction, 4* eOneSecond, TemplateDataReadCBFunction,0,0, 0 ); Report("StartThreadLoadingFunc",__FILE__,__LINE__,0,RpWarning,(int)LoadStages,0); ThreadLoadingReport(); return OK; } uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) { ContinueThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); ContinueThreadLoadingResponse Cresponse = CONTINUE_THREAD_LOADING_RESPONSE__INIT; MessageContainer responseContainer; MessageState = 2; ThreadLoadingReport(); if (ThreadLoadingRestartFlag == false) { if (request->processparameters) { dryerbufferlength = request->processparameters->dryerbufferlength; LoadArmRounds = (int)(request->processparameters->dryerbufferlength); windertension = request->processparameters->windertension; pullertension = request->processparameters->pullertension; memcpy (&ProcessParametersRecover,request->processparameters,sizeof(ProcessParameters)); } Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); } responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size); responseContainer.continuous = false; uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); //if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call //commented out - error retry is only with this message { ThreadLoadStateMachine(LoadStages); } return OK; } uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer) { StopThreadLoadingRequest *request = stop_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); StopThreadLoadingResponse Cresponse = STOP_THREAD_LOADING_RESPONSE__INIT; MessageContainer responseContainer; MessageState = 2; Thread_Load_End(); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); Report("StopThreadLoadingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)LoadStages,0); responseContainer = createContainer(MESSAGE_TYPE__StopThreadLoadingResponse, requestContainer->token, true, &Cresponse, &stop_thread_loading_response__pack, &stop_thread_loading_response__get_packed_size); responseContainer.continuous = false; uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); return OK; } uint8_t attemptJogCounter = 0; #define ATTEMPT_JOG_TIMEOUT 18 char AttemptJoggingToken[36+1] = {0}; uint32_t AttemptThreadJoggingCallback(uint32_t index, uint32_t ReadValue) { AttemptThreadJoggingResponse response = ATTEMPT_THREAD_JOGGING_RESPONSE__INIT; MessageContainer responseContainer; attemptJogCounter++; Report("AttemptThreadJoggingCallback",__FILE__,attemptJogCounter,JobEndReason,RpWarning,(int)JobIsActive(),0); if (attemptJogCounter<ATTEMPT_JOG_TIMEOUT) { if (JobIsActive() == true)//jog still preparing/running return OK; } if (JobEndReason == JOB_OK) ThreadAbortJoggingFunc(); else { responseContainer.has_error = true; responseContainer.error = getJobError_to_ErrorCode(JobEndReason); } SafeRemoveControlCallback(LoadingControlId, AttemptThreadJoggingCallback ); LoadingControlId = 0xFF; if (AttemptJoggingToken[0]) { responseContainer = createContainer(MESSAGE_TYPE__AttemptThreadJoggingResponse, AttemptJoggingToken, true, &response, &attempt_thread_jogging_response__pack, &attempt_thread_jogging_response__get_packed_size); responseContainer.continuous = false; uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); size_t container_size = message_container__pack(&responseContainer, container_buffer); my_free(responseContainer.data.data); SendChars((char*)container_buffer, container_size); } return OK; } uint32_t AttemptThreadJoggingFunc(MessageContainer* requestContainer) { attemptJogCounter = 0; if(requestContainer) ustrncpy (AttemptJoggingToken, requestContainer->token,36); Report("AttemptThreadJoggingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)ATTEMPT_JOG_TIMEOUT,0); LoadingControlId = AddControlCallback("Load jog",AttemptThreadJoggingCallback, eOneSecond,TemplateDataReadCBFunction,0,0,0); ThreadJoggingFunc(30); return OK; }