1 files changed, 65 insertions, 17 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 1c3326336..2484b9444 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -446,6 +446,7 @@ char TMessage[150];
 //char time[150];
 uint16_t BreakSensorCounter = 0;
 uint16_t BreakSensorLatchCounter = 0;
+bool FirstCalcInJob = true;
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -453,7 +454,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 
     //read value is the dancer angle
-    int i,index=MAX_THREAD_MOTORS_NUM;
+    int i,index=MAX_THREAD_MOTORS_NUM,len;
     int DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0;
     //double tempcalcspeed = 0;
@@ -604,14 +605,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             return OK;
         }
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
+        if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit
+        {
+            NormalizedError = (-1*NormalizedError);
+        }
+
         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)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit
-        {
-            MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
-        }
         /*else
         {
             //KeepNormalizedError = NormalizedError;
@@ -637,14 +639,37 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
+        if (FirstCalcInJob == true)
+        {
+            if (index == FEEDER_MOTOR)
+            {
+                FirstCalcInJob = false;
+                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
+                                ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
+                                (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
+                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed);
+                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
+
+            }
+        }
+
 #ifndef TEST_PID_THREAD
         if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
 #else
         if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
 #endif
         {
-            CurrentControlledSpeed[index] = calculated_speed;
-            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+            if (calculated_speed>5.0)
+            {
+                CurrentControlledSpeed[index] = calculated_speed;
+                MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+            }
+            else
+            {
+                if (calculated_speed<0)
+                ReportWithPackageFilter(ThreadFilter,"Negative speed",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
+            }
+
             /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
             {
                 ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
@@ -719,7 +744,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
-    int Motor_i, HW_Motor_Id, Pid_Id;
+    int Motor_i,i, HW_Motor_Id, Pid_Id;
     JobTicket* JobTicket = JobDetails;
     CurrentSegmentId = 0;
 
@@ -742,6 +767,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     EnableIntersegment = JobTicket->enableintersegment;
     IntersegmentLength = JobTicket->intersegmentlength;
 
+    FirstCalcInJob = true;
     if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
     {
         ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;
@@ -791,7 +817,17 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
             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
-
+//////////////////////////////////////////////////
+            for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+            {
+                if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                    MotorSamples[Motor_i][i] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0);  //reset the samples value for control beginning
+                else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
+                    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
+                //MotorSpeedSamples[Motor_i][i] = 0;
+            }
+            MotorSamplePointer[Motor_i] = 0;
+/////////////////////////////////////////////////////
             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
@@ -888,10 +924,11 @@ void SetOriginMotorSpeed(float process_speed)
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
         InitialDryerSpeed = 0.0;
         CurrentControlledSpeed[Motor_i] = (int) motor_speed;
-        //ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0);
+        if (process_speed > 1)
+            ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)motor_speed,RpWarning,process_speed,0);
 
-        for (i = 0; i <= MAX_CONTROL_SAMPLES; i++)
-            MotorSpeedSamples[Motor_i][i] = motor_speed;
+       // for (i = 0; i <= MAX_CONTROL_SAMPLES; i++)
+       //     MotorSpeedSamples[Motor_i][i] = motor_speed;
     }
 }
 void ThreadPreSegmentEnded(void)
@@ -900,7 +937,7 @@ void ThreadPreSegmentEnded(void)
     REPORT_MSG (0,"First ThreadPreSegmentEnded");
     PreSegmentReady(Module_Thread,ModuleDone);
 }
-int DrierDivider = 10;
+int DrierDivider = 20;
 uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag)
 {
     InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider);
@@ -943,7 +980,14 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
         PrepareState = false;
 #ifndef TEST_PID_THREAD
         // set the new speed in the dryer motor to the speed of the new segment
-        DrierDivider = dyeingspeed/5;  //ramp up drier in 5 cm/sec steps
+        if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
+        {
+            DrierDivider = dyeingspeed/3;  //ramp up drier in 5 cm/sec steps
+        }
+        else
+        {
+            DrierDivider = dyeingspeed/5;  //ramp up drier in 5 cm/sec steps
+        }
         ReportWithPackageFilter(ThreadFilter,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0);
         InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider;
         MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed );
@@ -1065,7 +1109,7 @@ char Endstr[150];
 {
      int Motor_i;
      ThreadControlActive = false;
-
+     uint32_t status = OK,tempCtl;
      usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
      SendJobProgress(0.0,0,false, Endstr);
      ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
@@ -1091,15 +1135,19 @@ char Endstr[150];
 
      for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
      {
+         tempCtl = ControlIdtoMotorId[Motor_i];
          if (ControlIdtoMotorId[Motor_i] != 0xFF)
          {
-             if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK)
+             status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+             if(status == OK)
                  ControlIdtoMotorId[Motor_i] = 0xFF;
              else
                  ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0);
          }
+         //ReportWithPackageFilter(ThreadFilter,"Remove Control",__FILE__,Motor_i,(int)status,RpError,(int)tempCtl,0);
+
      }
-	 Task_sleep(10);
+	 Task_sleep(100);
      for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
      {
         MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);