Version 1.3.8.12 printing function calls with segments info. ids - no valves. improved winding. dispensers pressure build on startup. try not to touch valves during job

5 files changed, 85 insertions, 26 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 638611c8b..3e2a6aa40 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -64,6 +64,8 @@ extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
 extern    MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
+extern int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index d8a96a4d9..faee55608 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -40,8 +40,8 @@ void ScrewsStartControlTimer (void);
 bool     Winder_ScrewHoming = false;
 bool     ScrewCurrentDirection = false;  //holds current screw direction
 uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
-uint32_t ScrewChangeCounter = 0;
-uint32_t ScrewChangeLimit = 0;
+//uint32_t ScrewChangeCounter = 0;
+//uint32_t ScrewChangeLimit = 0;
 uint32_t CalculationDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
 uint16_t WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
 uint16_t WinderMotorSpeedCounter = 0;
@@ -77,7 +77,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request)
     InternalWinderCfg.spoolbackingrate = request->backingrate;
     InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
     InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
-    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
+    InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*2;//request->rotationsperpassage;
     InternalWinderCfg.diameter = request->diameter;
 
     return status;
@@ -198,6 +198,8 @@ char ScrewStr[100];
 //char TempScrewStr[100];
 double WinderReferenceSpeed=0;
 int32_t  TotalWinderSpeed=0;
+bool Add100 = false;
+double Rotations = 6.0;
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     //uint32_t Steps;
@@ -206,14 +208,18 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     double RotationsPerSecond;
     int32_t Averagewinderspeed = 0;
 
-    ScrewChangeCounter++;
-    if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
+    //ScrewChangeCounter++;
+    //if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
     {
         TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
         WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
         TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
         if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
         {
+            if (WinderMotorSpeedRollOver == false)
+            {
+                Add100 = true;
+            }
             WinderMotorSpeedCounter=0;
             WinderMotorSpeedRollOver=true;
         }
@@ -227,6 +233,12 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 
     if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out
     {
+        if (Add100 == true) //once per job
+        {
+            ScrewNumberOfSteps += 100;
+            Add100 = false;
+        }
+
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
             ScrewNumberOfSteps--;
@@ -254,7 +266,9 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 
         WinderReferenceSpeed = Averagewinderspeed;
     }
-    screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
+    screw_horizontal_speed = ScrewNumberOfSteps /  Rotations;//InternalWinderCfg.NumberOfRotationPerPassage;
+    if (Rotations > 7.0)
+        Rotations = 6.0;
     RotationsPerSecond =  WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
     tempScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
     //ROM_IntMasterDisable();
@@ -288,7 +302,7 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
     return PreSegmentReady(Module_Winder,ModuleDone);
 }
 
-uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
+uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId)
 {
     //JobTicket* JobTicket = JobDetails;
     double screw_horizontal_speed = 0;
@@ -330,6 +344,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
     //     * 4. start move of travel length
     //     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
         ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+        ScrewNumberOfSteps -= 100;
         temp = SYS_CLK_FREQ;
         temp *= InternalWinderCfg.segmentoffsetpulses;
         temp /= ScrewSpeed;
@@ -349,6 +364,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         WinderMotorSpeedCounter=0;
         TotalWinderSpeed = 0;
         WinderMotorSpeedRollOver=false;
+        Add100 = false;
    }
     PreSegmentReady(Module_Winder,ModuleDone);
 
@@ -429,8 +445,8 @@ void ScrewTimerInterrupt(int ARG0)
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
         MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed+random);
-        ScrewChangeCounter = 0;
-        ScrewChangeLimit = ScrewRunningTime/12000000;
+      //  ScrewChangeCounter = 0;
+      //  ScrewChangeLimit = ScrewRunningTime/12000000;
         ScrewDirectionChangeCounter++;
     }
     else
@@ -438,9 +454,10 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
-    random++;
+    Rotations+=0.1;
+    /*random++;
     if (random >= 2)
-        random = -1;
+        random = -1;*/
     //Report("ScrewTimerInterrupt dir, duration, speed", __FILE__,ScrewCurrentDirection,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
     //
     // Enable all interrupts.
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index cd71c73b1..69ac4b6ad 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -31,10 +31,10 @@ uint32_t ThreadConfigBreakSensor(void *request);
 uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
 double ThreadGetMotorCalculatedError(int DancerId);
 uint32_t ThreadPrepareState(void *JobDetails);
-uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId);
-uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
+uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId);
+uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId);
 uint32_t ThreadDistanceToSpoolState(void);
-uint32_t ThreadEndState(void *JobDetails);
+uint32_t ThreadEndState();
 uint32_t ThreadInitialTestStub();
 
 uint32_t StoreDancerConfigMessage(void);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index c277dd5df..4454565c1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -22,6 +22,9 @@
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
+int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
+
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
@@ -104,7 +107,10 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
 #endif
     for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+    {
         MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
+        MotorSpeedSamples[Motor_i][i] = 0;
+    }
     NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
     temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index a6ca46246..4131fbca5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -68,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;
@@ -354,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;
@@ -373,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)
         {
@@ -487,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)
@@ -552,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;
@@ -564,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++)
     {
@@ -660,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];
@@ -672,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)
@@ -709,7 +741,7 @@ 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 (JobTicket->enablelubrication == true)
+        if (EnableLubrication == true)
         {
             IDS_StartLubrication();
         }
@@ -718,9 +750,10 @@ uint32_t ThreadPreSegmentState(void *JobDetails, uint32_t SegmentId)
     //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
     {
@@ -736,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)
@@ -750,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);
@@ -772,7 +806,7 @@ uint32_t ThreadDistanceToSpoolState(void )
 
 char Endstr[150];
 //********************************************************************************************************************
- uint32_t ThreadEndState(void *JobDetails)
+ uint32_t ThreadEndState(void )
 {
      int Motor_i;
      ThreadControlActive = false;