MERGE

4 files changed, 105 insertions, 62 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index ec4e336cd..9ce05b485 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -380,7 +380,7 @@ uint32_t MillisecLoop(uint32_t tick)
                     ScrewSetMaxSpeedPending.Active = false;
                     if (ScrewSetMaxSpeedPending.Callback)
                         ScrewSetMaxSpeedPending.Callback(Motor_i,0);
-
+                    continue;
                 }
                 else if (ScrewMovePending.Active == true)
                 {
@@ -391,7 +391,7 @@ uint32_t MillisecLoop(uint32_t tick)
                     ScrewMovePending.Active = false;
                     if (ScrewMovePending.Callback)
                         ScrewMovePending.Callback(Motor_i,0);
-
+                    continue;
                 }
             }
             if (SpeedSetPending[Motor_i].Active == true)
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index b4e9b24af..1ec32a4ae 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -95,7 +95,7 @@ DoubleArray DispenserFreq[MAX_SYSTEM_DISPENSERS];
 DoubleArray DiagnosticsDispenserPressure[MAX_SYSTEM_DISPENSERS];
 
 HeaterState **heatersstates;
-HeaterState  HeaterInfo[HEATER_TYPE__HeaterZone6+1];
+HeaterState  HeaterInfo[HEATER_TYPE__MixerHeater+1];
 /*double dispenser1motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser2motorfrequency[DIAGNOSTICS_LIMIT];
 double dispenser3motorfrequency[DIAGNOSTICS_LIMIT];
@@ -147,8 +147,8 @@ void DiagnosticsInit(void)
         double_array__init(&DispenserFreq[i]);
         double_array__init(&DiagnosticsDispenserPressure[i]);
     }
-    heatersstates = (HeaterState**)malloc(sizeof(HeaterState *)*(HEATER_TYPE__HeaterZone6+1));
-    for(i=0;i<=HEATER_TYPE__HeaterZone6;i++)
+    heatersstates = (HeaterState**)malloc(sizeof(HeaterState *)*(HEATER_TYPE__MixerHeater+1));
+    for(i=0;i<=HEATER_TYPE__MixerHeater;i++)
     {
         heater_state__init(&HeaterInfo[i]);
         heatersstates[i] = &HeaterInfo[i];
@@ -531,12 +531,12 @@ void SendDiagnostics(void)
         }
     }
 */
-    for (i=HEATER_TYPE__HeaterZone1;i<=HEATER_TYPE__HeaterZone6;i++)
+    for (i=HEATER_TYPE__DryerAirHeater;i<=HEATER_TYPE__MixerHeater;i++)
     {
         LoadHeaterState((HeaterType)i,&HeaterInfo[i]);
         //heatersstates[i] = &HeaterInfo[i];
     }
-    response.n_heatersstates = HEATER_TYPE__HeaterZone6+1;
+    response.n_heatersstates = HEATER_TYPE__MixerHeater+1;
     response.heatersstates = heatersstates;
 
     diagnosticsresponseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse,  DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index e2baf6d53..2acd8c56f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -511,22 +511,10 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
     if(HeaterPIDConfig[index].m_isEnabled && (HeaterPIDConfig[index].m_SetParam != 0))
     {
         HeaterPIDConfig[index].m_mesuredParam = readValue;
-        /*if (readValue < ((HeaterCmd[index].targettemperatue * (100-HeaterControl[index].outputproportionalband))/100)) //below proportional band
-        {
-            HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, 100);
-            //REPORT_MSG (readValue,"AC HEATER Over temperature");
-        }
-        else if (readValue > ((HeaterCmd[index].targettemperatue * (100+HeaterControl[index].outputproportionalband))/100))
-        {
-            HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, 0);
-            //REPORT_MSG (readValue,"AC HEATER Under temperature");
-        }*/
 //check only for the proportional band limits
-        //temperror = (double)readValue/10000;
-        //HeaterPIDConfig[index].m_mesuredParam = temperror;
         HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
                                                                            &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-        correction = HeaterPIDConfig[index].m_calculatedError;
+        //correction = HeaterPIDConfig[index].m_calculatedError;
         HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, (int)(HeaterPIDConfig[index].m_calculatedError));
     }
 
@@ -547,7 +535,6 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
-    double temperror = 0.0;
     int len;
     /*char str[100];
     uint8_t len = 0;*/
@@ -612,8 +599,6 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 
 //check only for the proportional band limits
         HeaterPIDConfig[index].m_mesuredParam = readValue;
-        //temperror = (double)readValue/10000;
-        //HeaterPIDConfig[index].m_mesuredParam = temperror;
         HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
                                                                               &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
 
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)