Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango

1 files changed, 51 insertions, 12 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 6a8474438..4131fbca5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -30,6 +30,7 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 #include "modules/General/process.h"
+#include "modules/ids/ids_ex.h"
 #include "Modules/AlarmHandling/AlarmHandling.h"
 #include "Control/MillisecTask.h"
 
@@ -67,6 +68,12 @@ double TempPoolerTotalProcessedLength = 0.0;
 double TempTotalProcessedLength = 0.0;
 
 bool PrepareState = false;
+// job parameters
+bool EnableLubrication = false;
+bool EnableIntersegment = false;
+double IntersegmentLength = 0;
+
+
 int CurrentSegmentId = 0;
 typedef  void (* ProcessedLengthFunc)(void);
 ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
@@ -353,7 +360,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     int DancerId;
-    int32_t TranslatedReadValue, avreageSampleValue = 0;
+    int32_t TranslatedReadValue, avreageSampleValue = 0,avreageMotorSampleValue = 0;
     //double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
@@ -372,6 +379,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     if(MotorControlConfig[index].m_isEnabled )
     {
+        //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true)
+        //    return OK;
         DancerId = ThreadMotorIdToDancerId[index];
         if (ReadValue < 10)
         {
@@ -486,7 +495,23 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             //KeepNormalizedError = NormalizedError;
         }
+        if ((JobCounter % 1000) == 0)
+        {
+            if (JobCounter >= 20000)
+            {
+                MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue);
+                MotorSpeedSamplePointer[index]++;
+                if (MotorSpeedSamplePointer[index] >= MAX_CONTROL_SAMPLES)
+                    MotorSpeedSamplePointer[index] = 0;
+                for (i=0;i<MAX_CONTROL_SAMPLES;i++)
+                    avreageMotorSampleValue += MotorSpeedSamples[index][i];
+                avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES;
+                //Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0);
+                OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue;
+            }
+        }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
         {
             /*if (keepdata == true)
@@ -551,6 +576,7 @@ bool InitialProcess = false;
  uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i, HW_Motor_Id, Pid_Id;
+    JobTicket* JobTicket = JobDetails;
     CurrentSegmentId = 0;
 
     JobCounter = 0;
@@ -563,6 +589,11 @@ bool InitialProcess = false;
     AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,false);
     AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false);
 
+    EnableLubrication = JobTicket->enablelubrication;
+    EnableIntersegment = JobTicket->enableintersegment;
+    IntersegmentLength = JobTicket->intersegmentlength;
+
+
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
@@ -659,7 +690,7 @@ bool InitialProcess = false;
 
 void SetOriginMotorSpeed(float process_speed)
 {
-    int Motor_i, HW_Motor_Id;
+    int i,Motor_i, HW_Motor_Id;
     for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
@@ -671,14 +702,16 @@ void SetOriginMotorSpeed(float process_speed)
         //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
         CurrentControlledSpeed[Motor_i] = (int) motor_speed;
+        for (i = 0; i <= MAX_CONTROL_SAMPLES; i++)
+            MotorSpeedSamples[Motor_i][i] = motor_speed;
     }
 }
 
 //********************************************************************************************************************
-uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
+uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
 {
 //set the speed only before the first segment, speed is constant across all job segments and intersegments
-    JobTicket* JobTicket = JobDetails;
+    JobSegment* Segment = SegmentDetails;
 
     float process_speed = dyeingspeed;
     if (dyeingspeed == 0)
@@ -708,14 +741,19 @@ uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
             MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
             MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
         }
+        if (EnableLubrication == true)
+        {
+            IDS_StartLubrication();
+        }
     }
     // activate control fr all motors
     //set speed for both rocker motors
     //wait for all motors to get to the required speed (set the target speed for the control to check)
     //call the job state machine when the thread system is ready
-    if ((InitialProcess==false) && JobTicket->enableintersegment == true)
+    if ((InitialProcess==false) && (EnableIntersegment == true) && (IntersegmentLength >= 1.0)) //fix - avoid intersegment length 0
     {
-        ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded);
+        ThreadUpdateProcessLength (IntersegmentLength,(void *)ThreadInterSegmentEnded);
+        REPORT_MSG (IntersegmentLength," ThreadPreSegmentState IntersegmentLength");
     }
     else
     {
@@ -731,11 +769,12 @@ int REPSegmentId = 0;
 void ThreadInterSegmentEnded(void)
 {
     REPORT_MSG (REPSegmentId,"ThreadInterSegmentEnded");
+    //ThreadUpdateProcessLength (0,(void *)NULL);
     PreSegmentReady(Module_Thread,ModuleDone);
 }
 void ThreadSegmentEnded(void)
 {
-    REPORT_MSG (REPSegmentId," ThreadSegmentState");
+    REPORT_MSG (REPSegmentId," ThreadSegmentEnded");
     SegmentReady(Module_Thread,ModuleDone);
 }
 void ThreadDistanceToSpoolEnded(void)
@@ -745,11 +784,11 @@ void ThreadDistanceToSpoolEnded(void)
 }
 double seglength = 0.0;
 //********************************************************************************************************************
-uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
+uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId)
 {
-    JobTicket* JobTicket = JobDetails;
+    JobSegment* Segment = SegmentDetails;
     REPSegmentId = SegmentId;
-    seglength = JobTicket->segments[SegmentId]->length;
+    seglength = Segment->length;
     CurrentSegmentId = SegmentId;
     REPORT_MSG (seglength," ThreadSegmentState");
     ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded);
@@ -767,7 +806,7 @@ uint32_t ThreadDistanceToSpoolState(void )
 
 char Endstr[150];
 //********************************************************************************************************************
- uint32_t ThreadEndState(void *JobDetails)
+ uint32_t ThreadEndState(void )
 {
      int Motor_i;
      ThreadControlActive = false;
@@ -807,7 +846,7 @@ char Endstr[150];
      }
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
      MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
-
+     IDS_StopLubrication();
     return OK;
 }