aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
diff options
context:
space:
mode:
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
-rw-r--r--Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c136
1 files changed, 97 insertions, 39 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 77e1939f9..05c6c9003 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -43,6 +43,7 @@ TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TY
HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
uint32_t SpeedControlId=0xFF;
+uint32_t PoolerSpeedControlId=0xFF;
double DancerError[NUM_OF_DANCERS] = {0.0};
int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
@@ -63,11 +64,16 @@ MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
uint32_t PreviousPosition = 0, CurrentPosition = 0;
-double totalLength = 0.0;
double CurrentRequestedLength = 0.0;
double CurrentProcessedLength = 0.0;
double TotalProcessedLength = 0.0;
double LengthCalculationMultiplier;
+
+uint32_t PoolerPreviousPosition = 0, PoolerCurrentPosition = 0;
+double PoolerCurrentProcessedLength = 0.0;
+double PoolerTotalProcessedLength = 0.0;
+double PoolerLengthCalculationMultiplier;
+
bool PrepareState = false;
int CurrentSegmentId = 0;
typedef void (* ProcessedLengthFunc)(void);
@@ -120,22 +126,17 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
*
* **************************************************************************************/
uint32_t initialpos = 0xFFFF;
+uint32_t Poolerinitialpos = 0xFFFF;
void ThreadUpdateProcessLength (double length, void *Funcptr)
{
CurrentRequestedLength = length*100;//Centimetres
CurrentProcessedLength = 0;
- //PreviousPosition = 0;
- //CurrentPosition = 0;
- totalLength = 0;
+ PoolerCurrentProcessedLength = 0;
ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
initialpos = 0xFFFF;
+ Poolerinitialpos = 0xFFFF;
}
-double MotorSentData[100] = {0};
-uint32_t PosDif[100] = {0};
-
-
-int MotorDataIndex = 0;
uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
@@ -143,6 +144,10 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
double length = 0.0;
char str[150];
int index = MAX_THREAD_MOTORS_NUM;
+ if (ThreadControlActive == false)
+ return OK;
+ if (PrepareState == true)
+ return OK;
if (IfIndex>>8 != IfTypeThread)
{
LOG_ERROR (IfIndex, "Wrong Interface type");
@@ -173,19 +178,9 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
//positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
length = (double)(positionDiff)*LengthCalculationMultiplier;
- if (length > 0.1)
- {
- totalLength+=length;
- }
-//#warning control disabled
- CurrentProcessedLength+=length;
+ CurrentProcessedLength+=length;
- PosDif[MotorDataIndex] = CurrentPosition;
- //PosDif[MotorDataIndex] = positionDiff;
- MotorSentData[MotorDataIndex] = length;
- MotorDataIndex+=1;
- if (MotorDataIndex == 99) MotorDataIndex = 0;
static int pooler_counter = 0;
pooler_counter++;
TotalProcessedLength+= (length/100);
@@ -206,12 +201,59 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
}
if (CurrentProcessedLength>=CurrentRequestedLength )
{
+ usnprintf(str, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+ SendJobProgress(0.0,0,false, str);
+ Report(str,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
// segment/intersegment/distance to spool finished
if (ProcessedLengthFuncPtr)
ProcessedLengthFuncPtr();
+
}
return OK;
}
+
+uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+ uint32_t positionDiff = 0;
+ double length = 0.0;
+ int index = MAX_THREAD_MOTORS_NUM;
+ 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 (CurrentRequestedLength == 0.0)
+// return OK;
+ if (index != POOLER_MOTOR)
+ {
+ LOG_ERROR (IfIndex, "Wrong Motor");
+ return 0xFFFFFFFF;
+ }
+ PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
+// if (CurrentPosition == 0)
+// return OK; //unusable data
+ if (Poolerinitialpos == 0xFFFF)
+ {
+ PoolerPreviousPosition = PoolerCurrentPosition;
+ Poolerinitialpos = 0;
+ }
+ positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition);
+ //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+ PoolerPreviousPosition = PoolerCurrentPosition;
+
+ length = (double)(positionDiff)*PoolerLengthCalculationMultiplier;
+ PoolerCurrentProcessedLength+=length;
+
+return OK;
+}
+
+
+
float SpeedSamples[MAX_CONTROL_SAMPLES] = {0};
uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
@@ -268,14 +310,15 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
}
return OK;
}
-/*double calculatedError[100];
-double eNormalizedError[100];
-int readValue[100];
-int TranslatedreadValue[100];
-int AveragereadValue[100];
-int calculatedspeed[100];
+double calculatedError[200];
+//double eNormalizedError[100];
+int MotorId[200];
+int readValue[200];
+//int TranslatedreadValue[100];
+int AveragereadValue[200];
+int calculatedspeed[200];
int controlIndex = 0;
-int32_t KeepReadValue = 0;
+/*int32_t KeepReadValue = 0;
void testDancersControl()
{
int mm20,mm10,mm5,mm2,mm1;
@@ -327,18 +370,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
}
index = IfIndex&0xFF;
- /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
- if (ControlIdtoMotorId[i] == MotorId)
- {
- index = i;
- break;
- }
- if (index==MAX_THREAD_MOTORS_NUM)
- {
- LOG_ERROR (MotorId, "No motor for device");
- return 0xFFFFFFFF;
- }*/
-
if(MotorControlConfig[index].m_isEnabled )
{
DancerId = ThreadMotorIdToDancerId[index];
@@ -440,6 +471,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
}*/
if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
{
+ calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
+ //double eNormalizedError[100];
+ MotorId[controlIndex] = index;
+ readValue[controlIndex] = ReadValue;
+ //int TranslatedreadValue[100];
+ AveragereadValue[controlIndex] = avreageSampleValue;
+ calculatedspeed[controlIndex] = calculated_speed;
+ if (controlIndex++>=199)
+ controlIndex = 0;
CurrentControlledSpeed[index] = calculated_speed;
MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
}
@@ -478,6 +518,7 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
JobCounter = 0;
TotalProcessedLength = 0.0;
+ PoolerTotalProcessedLength = 0.0;
PrepareState = true;
AlarmHandlingSetAlarm(EVENT_TYPE__ThreadBreak,false);
AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,false);
@@ -515,6 +556,17 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
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)
+ {
+ RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction);
+ PoolerSpeedControlId = 0xFF;
+ }
+ //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+ LengthCalculationMultiplier = (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)
@@ -676,6 +728,12 @@ uint32_t ThreadDistanceToSpoolState(void )
RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
SpeedControlId = 0xFF;
}
+ if (PoolerSpeedControlId != 0xFF)
+ {
+ RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction);
+ PoolerSpeedControlId = 0xFF;
+ }
+
for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
{
if (ControlIdtoMotorId[Motor_i] != 0xFF)
generated by cgit v1.3.1 (git 2.54.0) at 2026-06-19 18:15:06 +0000