aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Visual_Studio/Scripting/Tango.Scripting.Editors/Document/OffsetChangeMap.cs
blob: 9494af56b060892e9cd9a73fa95498b2768e355e (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
// Copyright (c) AlphaSierraPapa for the SharpDevelop Team (for details please see \doc\copyright.txt)
// This code is distributed under the GNU LGPL (for details please see \doc\license.txt)

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Diagnostics.CodeAnalysis;

using Tango.Scripting.Editors.Utils;

namespace Tango.Scripting.Editors.Document
{
	/// <summary>
	/// Contains predefined offset change mapping types.
	/// </summary>
	public enum OffsetChangeMappingType
	{
		/// <summary>
		/// Normal replace.
		/// Anchors in front of the replaced region will stay in front, anchors after the replaced region will stay after.
		/// Anchors in the middle of the removed region will be deleted. If they survive deletion,
		/// they move depending on their AnchorMovementType.
		/// </summary>
		/// <remarks>
		/// This is the default implementation of DocumentChangeEventArgs when OffsetChangeMap is null,
		/// so using this option usually works without creating an OffsetChangeMap instance.
		/// This is equivalent to an OffsetChangeMap with a single entry describing the replace operation.
		/// </remarks>
		Normal,
		/// <summary>
		/// First the old text is removed, then the new text is inserted.
		/// Anchors immediately in front (or after) the replaced region may move to the other side of the insertion,
		/// depending on the AnchorMovementType.
		/// </summary>
		/// <remarks>
		/// This is implemented as an OffsetChangeMap with two entries: the removal, and the insertion.
		/// </remarks>
		RemoveAndInsert,
		/// <summary>
		/// The text is replaced character-by-character.
		/// Anchors keep their position inside the replaced text.
		/// Anchors after the replaced region will move accordingly if the replacement text has a different length than the replaced text.
		/// If the new text is shorter than the old text, anchors inside the old text that would end up behind the replacement text
		/// will be moved so that they point to the end of the replacement text.
		/// </summary>
		/// <remarks>
		/// On the OffsetChangeMap level, growing text is implemented by replacing the last character in the replaced text
		/// with itself and the additional text segment. A simple insertion of the additional text would have the undesired
		/// effect of moving anchors immediately after the replaced text into the replacement text if they used
		/// AnchorMovementStyle.BeforeInsertion.
		/// Shrinking text is implemented by removing the text segment that's too long; but in a special mode that
		/// causes anchors to always survive irrespective of their <see cref="TextAnchor.SurviveDeletion"/> setting.
		/// If the text keeps its old size, this is implemented as OffsetChangeMap.Empty.
		/// </remarks>
		CharacterReplace,
		/// <summary>
		/// Like 'Normal', but anchors with <see cref="TextAnchor.MovementType"/> = Default will stay in front of the
		/// insertion instead of being moved behind it.
		/// </summary>
		KeepAnchorBeforeInsertion
	}
	
	/// <summary>
	/// Describes a series of offset changes.
	/// </summary>
	[Serializable]
	[SuppressMessage("Microsoft.Naming", "CA1710:IdentifiersShouldHaveCorrectSuffix",
	                 Justification="It's a mapping old offsets -> new offsets")]
	public sealed class OffsetChangeMap : Collection<OffsetChangeMapEntry>
	{
		/// <summary>
		/// Immutable OffsetChangeMap that is empty.
		/// </summary>
		[SuppressMessage("Microsoft.Security", "CA2104:DoNotDeclareReadOnlyMutableReferenceTypes",
		                 Justification="The Empty instance is immutable")]
		public static readonly OffsetChangeMap Empty = new OffsetChangeMap(Empty<OffsetChangeMapEntry>.Array, true);
		
		/// <summary>
		/// Creates a new OffsetChangeMap with a single element.
		/// </summary>
		/// <param name="entry">The entry.</param>
		/// <returns>Returns a frozen OffsetChangeMap with a single entry.</returns>
		public static OffsetChangeMap FromSingleElement(OffsetChangeMapEntry entry)
		{
			return new OffsetChangeMap(new OffsetChangeMapEntry[] { entry }, true);
		}
		
		bool isFrozen;
		
		/// <summary>
		/// Creates a new OffsetChangeMap instance.
		/// </summary>
		public OffsetChangeMap()
		{
		}
		
		internal OffsetChangeMap(int capacity)
			: base(new List<OffsetChangeMapEntry>(capacity))
		{
		}
		
		private OffsetChangeMap(IList<OffsetChangeMapEntry> entries, bool isFrozen)
			: base(entries)
		{
			this.isFrozen = isFrozen;
		}
		
		/// <summary>
		/// Gets the new offset where the specified offset moves after this document change.
		/// </summary>
		public int GetNewOffset(int offset, AnchorMovementType movementType)
		{
			IList<OffsetChangeMapEntry> items = this.Items;
			int count = items.Count;
			for (int i = 0; i < count; i++) {
				offset = items[i].GetNewOffset(offset, movementType);
			}
			return offset;
		}
		
		/// <summary>
		/// Gets whether this OffsetChangeMap is a valid explanation for the specified document change.
		/// </summary>
		public bool IsValidForDocumentChange(int offset, int removalLength, int insertionLength)
		{
			int endOffset = offset + removalLength;
			foreach (OffsetChangeMapEntry entry in this) {
				// check that ChangeMapEntry is in valid range for this document change
				if (entry.Offset < offset || entry.Offset + entry.RemovalLength > endOffset)
					return false;
				endOffset += entry.InsertionLength - entry.RemovalLength;
			}
			// check that the total delta matches
			return endOffset == offset + insertionLength;
		}
		
		/// <summary>
		/// Calculates the inverted OffsetChangeMap (used for the undo operation).
		/// </summary>
		public OffsetChangeMap Invert()
		{
			if (this == Empty)
				return this;
			OffsetChangeMap newMap = new OffsetChangeMap(this.Count);
			for (int i = this.Count - 1; i >= 0; i--) {
				OffsetChangeMapEntry entry = this[i];
				// swap InsertionLength and RemovalLength
				newMap.Add(new OffsetChangeMapEntry(entry.Offset, entry.InsertionLength, entry.RemovalLength));
			}
			return newMap;
		}
		
		/// <inheritdoc/>
		protected override void ClearItems()
		{
			CheckFrozen();
			base.ClearItems();
		}
		
		/// <inheritdoc/>
		protected override void InsertItem(int index, OffsetChangeMapEntry item)
		{
			CheckFrozen();
			base.InsertItem(index, item);
		}
		
		/// <inheritdoc/>
		protected override void RemoveItem(int index)
		{
			CheckFrozen();
			base.RemoveItem(index);
		}
		
		/// <inheritdoc/>
		protected override void SetItem(int index, OffsetChangeMapEntry item)
		{
			CheckFrozen();
			base.SetItem(index, item);
		}
		
		void CheckFrozen()
		{
			if (isFrozen)
				throw new InvalidOperationException("This instance is frozen and cannot be modified.");
		}
		
		/// <summary>
		/// Gets if this instance is frozen. Frozen instances are immutable and thus thread-safe.
		/// </summary>
		public bool IsFrozen {
			get { return isFrozen; }
		}
		
		/// <summary>
		/// Freezes this instance.
		/// </summary>
		public void Freeze()
		{
			isFrozen = true;
		}
	}
	
	/// <summary>
	/// An entry in the OffsetChangeMap.
	/// This represents the offset of a document change (either insertion or removal, not both at once).
	/// </summary>
	[Serializable]
	public struct OffsetChangeMapEntry : IEquatable<OffsetChangeMapEntry>
	{
		readonly int offset;
		
		// MSB: DefaultAnchorMovementIsBeforeInsertion
		readonly uint insertionLengthWithMovementFlag;
		
		// MSB: RemovalNeverCausesAnchorDeletion; other 31 bits: RemovalLength
		readonly uint removalLengthWithDeletionFlag;
		
		/// <summary>
		/// The offset at which the change occurs.
		/// </summary>
		public int Offset {
			get { return offset; }
		}
		
		/// <summary>
		/// The number of characters inserted.
		/// Returns 0 if this entry represents a removal.
		/// </summary>
		public int InsertionLength {
			get { return (int)(insertionLengthWithMovementFlag & 0x7fffffff); }
		}
		
		/// <summary>
		/// The number of characters removed.
		/// Returns 0 if this entry represents an insertion.
		/// </summary>
		public int RemovalLength {
			get { return (int)(removalLengthWithDeletionFlag & 0x7fffffff); }
		}
		
		/// <summary>
		/// Gets whether the removal should not cause any anchor deletions.
		/// </summary>
		public bool RemovalNeverCausesAnchorDeletion {
			get { return (removalLengthWithDeletionFlag & 0x80000000) != 0; }
		}
		
		/// <summary>
		/// Gets whether default anchor movement causes the anchor to stay in front of the caret.
		/// </summary>
		public bool DefaultAnchorMovementIsBeforeInsertion {
			get { return (insertionLengthWithMovementFlag & 0x80000000) != 0; }
		}
		
		/// <summary>
		/// Gets the new offset where the specified offset moves after this document change.
		/// </summary>
		public int GetNewOffset(int oldOffset, AnchorMovementType movementType)
		{
			int insertionLength = this.InsertionLength;
			int removalLength = this.RemovalLength;
			if (!(removalLength == 0 && oldOffset == offset)) {
				// we're getting trouble (both if statements in here would apply)
				// if there's no removal and we insert at the offset
				// -> we'd need to disambiguate by movementType, which is handled after the if
				
				// offset is before start of change: no movement
				if (oldOffset <= offset)
					return oldOffset;
				// offset is after end of change: movement by normal delta
				if (oldOffset >= offset + removalLength)
					return oldOffset + insertionLength - removalLength;
			}
			// we reach this point if
			// a) the oldOffset is inside the deleted segment
			// b) there was no removal and we insert at the caret position
			if (movementType == AnchorMovementType.AfterInsertion)
				return offset + insertionLength;
			else if (movementType == AnchorMovementType.BeforeInsertion)
				return offset;
			else
				return this.DefaultAnchorMovementIsBeforeInsertion ? offset : offset + insertionLength;
		}
		
		/// <summary>
		/// Creates a new OffsetChangeMapEntry instance.
		/// </summary>
		public OffsetChangeMapEntry(int offset, int removalLength, int insertionLength)
		{
			ThrowUtil.CheckNotNegative(offset, "offset");
			ThrowUtil.CheckNotNegative(removalLength, "removalLength");
			ThrowUtil.CheckNotNegative(insertionLength, "insertionLength");
			
			this.offset = offset;
			this.removalLengthWithDeletionFlag = (uint)removalLength;
			this.insertionLengthWithMovementFlag = (uint)insertionLength;
		}
		
		/// <summary>
		/// Creates a new OffsetChangeMapEntry instance.
		/// </summary>
		public OffsetChangeMapEntry(int offset, int removalLength, int insertionLength, bool removalNeverCausesAnchorDeletion, bool defaultAnchorMovementIsBeforeInsertion)
			: this(offset, removalLength, insertionLength)
		{
			if (removalNeverCausesAnchorDeletion)
				this.removalLengthWithDeletionFlag |= 0x80000000;
			if (defaultAnchorMovementIsBeforeInsertion)
				this.insertionLengthWithMovementFlag |= 0x80000000;
		}
		
		/// <inheritdoc/>
		public override int GetHashCode()
		{
			unchecked {
				return offset + 3559 * (int)insertionLengthWithMovementFlag + 3571 * (int)removalLengthWithDeletionFlag;
			}
		}
		
		/// <inheritdoc/>
		public override bool Equals(object obj)
		{
			return obj is OffsetChangeMapEntry && this.Equals((OffsetChangeMapEntry)obj);
		}
		
		/// <inheritdoc/>
		public bool Equals(OffsetChangeMapEntry other)
		{
			return offset == other.offset && insertionLengthWithMovementFlag == other.insertionLengthWithMovementFlag && removalLengthWithDeletionFlag == other.removalLengthWithDeletionFlag;
		}
		
		/// <summary>
		/// Tests the two entries for equality.
		/// </summary>
		public static bool operator ==(OffsetChangeMapEntry left, OffsetChangeMapEntry right)
		{
			return left.Equals(right);
		}
		
		/// <summary>
		/// Tests the two entries for inequality.
		/// </summary>
		public static bool operator !=(OffsetChangeMapEntry left, OffsetChangeMapEntry right)
		{
			return !left.Equals(right);
		}
	}
}
">(double)(positionDiff)*PoolerLengthCalculationMultiplier; PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; 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; uint32_t calculated_speed; float speed = getSensorSpeedData(); if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += SpeedSamples[i]; avreageSampleValue = avreageSampleValue / 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 (abs(calculated_speed-CurrentControlledSpeed[index])>2) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } } return OK; } uint32_t _speed; uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) { int index; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { int MotorId = ThreadMotorIdToMotorId[index]; _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId); } return OK; } //double eNormalizedError[100]; //int TranslatedreadValue[100]; #define MAX_THREAD_CONTROL_LOG 300 double calculatedError[MAX_THREAD_CONTROL_LOG+1]; double NormError[MAX_THREAD_CONTROL_LOG+1]; double Integral[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]; int controlIndex = 0; bool keepdata = true; /*int32_t KeepReadValue = 0; 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; ThreadControlActive = true; SetOriginMotorSpeed(30.0); 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; }*/ bool dancerinvalid = false; int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0}; char Message[60]; 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; //double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; char Message[60]; if (ThreadControlActive == false) return OK; if (PrepareState == true) return OK; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) { MotorFailedSample[index]++; REPORT_MSG(ReadValue, "Dancer value read too small."); return OK; } if (ReadValue == 0x3FFF) { MotorFailedSample[index]++; if (dancerinvalid == false) { dancerinvalid = true; LOG_ERROR(index, "Dancer value invalid."); } return OK; } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); JobCounter++; } //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; if (BreakSensorenabled == true) { if (index == POOLER_MOTOR) { if (JobCounter > eOneSecond) { if (ReadBreakSensor()==ERROR) { //consider applying the debouce parameters later //BreakSensordebouncetimemilli JobEndReason = JOB_THREAD_BREAK; ThreadControlActive = false; SendJobProgress(0.0,0,false, "ReadBreakSensor Error"); SegmentReady(Module_Thread,ModuleFail); AlarmHandlingSetAlarm(EVENT_TYPE__ThreadBreak,true); //EndState(CurrentJob,"ReadBreakSensor Error" ); LOG_ERROR(index, "ReadBreakSensor Error"); return OK; } } } } //Stop Execution if the dancer moves too much if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) { keepdata = false; usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint); //JobAbortedByUser = true; ThreadControlActive = false; MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]); JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId; SendJobProgress(0.0,0,false, Message); //EndState(CurrentJob,Message ); SegmentReady(Module_Thread,ModuleFail); AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,true); LOG_ERROR (index, "Dancer Failure"); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; 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); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit { MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); } else { //KeepNormalizedError = NormalizedError; } calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (abs(calculated_speed-CurrentControlledSpeed[index])>2) { if (keepdata == true) { calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; MotorId[controlIndex] = index; readValue[controlIndex] = ReadValue; AveragereadValue[controlIndex] = avreageSampleValue; calculatedspeed[controlIndex] = calculated_speed; timestamp[controlIndex] = msec_millisecondCounter; NormError[controlIndex]= NormalizedError; Integral[controlIndex] = MotorControlConfig[index].m_integral; if (controlIndex++>=MAX_THREAD_CONTROL_LOG) controlIndex = 0; } CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } else MotorFailedSample[index]++; } return OK; } //******************************************************************************************************************** uint32_t 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 ThreadInitialTestStub(HardwareMotor * request) { //MotorsConfigMessage(request); ThreadPrepareState(request); ThreadPreSegmentState(request); return OK; } bool InitialProcess = false; uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) { return OK; } //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i, HW_Motor_Id, Pid_Id; CurrentSegmentId = 0; JobCounter = 0; TotalProcessedLength = 0.0; PoolerTotalProcessedLength = 0.0; PrepareState = true; AlarmHandlingSetAlarm(EVENT_TYPE__ThreadBreak,false); AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,false); //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];*/ 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.epsilon = 0.1; MotorControlConfig[Motor_i].m_params.dt = 1000; 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 MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); 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 (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(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } 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 { if (PoolerSpeedControlId != 0xFF) { if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) LOG_ERROR(Motor_i,"Remove Control Failed"); 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(PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } 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) LOG_ERROR(Motor_i,"Remove Control Failed"); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } 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) LOG_ERROR(Motor_i,"Remove Control Failed"); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } 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 { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK) LOG_ERROR(Motor_i,"Remove Control Failed"); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } // if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled // AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,ThreadEmptyCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled continue; } //testDancersControl(); PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions InitialProcess = true; return OK; } void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; for (Motor_i = 0; Motor_i <= WINDER_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; CurrentControlledSpeed[Motor_i] = (int) motor_speed; } } //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *JobDetails) { //set the speed only before the first segment, speed is constant across all job segments and intersegments JobTicket* JobTicket = JobDetails; float process_speed = dyeingspeed; if (dyeingspeed == 0) { LOG_ERROR (dyeingspeed," job speed zero"); return ERROR; } LOG_ERROR (dyeingspeed," ThreadPreSegmentState"); SetOriginMotorSpeed(process_speed); ThreadControlActive = true; PrepareState = false; // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); //only for testing - when control works, these motors will take their speed from the dryer //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, OriginalMotorSpd_2PPS[POOLER_MOTOR]); //only for testing - when control works, these motors will take their speed from the dryer //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR]); //#warning rocker disabled 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); } // #warning rocker disabled // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO // 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) && JobTicket->enableintersegment == true) { ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded); } else { ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); JobCounter = 0; InitialProcess = false; } return OK; } int REPSegmentId = 0; void ThreadInterSegmentEnded(void) { LOG_ERROR (REPSegmentId,"ThreadInterSegmentEnded"); PreSegmentReady(Module_Thread,ModuleDone); } void ThreadSegmentEnded(void) { LOG_ERROR (REPSegmentId," ThreadSegmentState"); SegmentReady(Module_Thread,ModuleDone); } void ThreadDistanceToSpoolEnded(void) { LOG_ERROR (REPSegmentId," ThreadDistanceToSpoolEnded"); DistanceToSpoolReady(Module_Thread,ModuleDone); } double seglength = 0.0; //******************************************************************************************************************** uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) { JobTicket* JobTicket = JobDetails; REPSegmentId = SegmentId; seglength = JobTicket->segments[SegmentId]->length; CurrentSegmentId = SegmentId; LOG_ERROR (seglength," ThreadSegmentState"); ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded); return OK; } //******************************************************************************************************************** uint32_t ThreadDistanceToSpoolState(void ) { seglength = dryerbufferlength; LOG_ERROR (seglength,"ThreadDistanceToSpoolState"); ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded); return OK; } char Endstr[150]; //******************************************************************************************************************** uint32_t ThreadEndState(void *JobDetails) { int Motor_i; ThreadControlActive = false; usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); ThreadUpdateProcessLength (0.0,(void *)NULL); SetOriginMotorSpeed(0); if (SpeedControlId != 0xFF) { if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK) LOG_ERROR(Motor_i,"RemoveControl Failed"); SpeedControlId = 0xFF; } if (PoolerSpeedControlId != 0xFF) { if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK) LOG_ERROR(Motor_i,"RemoveControl Failed"); PoolerSpeedControlId = 0xFF; } for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK) ControlIdtoMotorId[Motor_i] == 0xFF; else LOG_ERROR (ControlIdtoMotorId[Motor_i],"Remove Control failed"); } MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); return OK; } //******************************************************************************************************************** void ThreadStartPrinting(void) { //PrintingIterate(); } //******************************************************************************************************************** //******************************************************************************************************************** void ThreadStopPrinting(void) { //PrintingIterate(); }