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

2 files changed, 39 insertions, 8 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index aa9e6ec94..a119b0b16 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -199,7 +199,7 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void)
     uint32_t status;
     if  (Winder_ScrewHoming)
     {
-        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); //stop ASAP
     }
     status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index cfff7773b..1ae1145fb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -72,6 +72,7 @@ void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
 
 double KeepNormalizedError = 0;
+bool ThreadControlActive = false;
 ////////////////////////Slow Motor State////////////////////////////////////
 //uint32_t ThreadPreSegmentState(void *JobDetails);
 
@@ -122,8 +123,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
     initialpos = 0xFFFF;
 }
-double MotorSentData[1000] = {0};
-uint32_t PosDif[1000] = {0};
+double MotorSentData[100] = {0};
+uint32_t PosDif[100] = {0};
 
 
 int MotorDataIndex = 0;
@@ -175,7 +176,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
     MotorDataIndex+=1;
-    if (MotorDataIndex == 999) MotorDataIndex = 0;
+    if (MotorDataIndex == 99) MotorDataIndex = 0;
     static int pooler_counter = 0;
     pooler_counter++;
     if (pooler_counter%10 == 0)
@@ -248,6 +249,12 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
+double calculatedError[1000];
+int    readValue[1000];
+int    calculatedspeed[1000];
+int controlIndex = 0;
+int32_t KeepReadValue = 0;
+
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -259,6 +266,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int DancerId;
     static int pooler_counter = 0;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
+    double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
     char Message[60];
@@ -324,10 +332,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         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) //feeder unit handles errors opposite to left unit
+        {
             MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+        }
         else
-            KeepNormalizedError = NormalizedError;
+        {
+            //KeepNormalizedError = NormalizedError;
+        }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (index == POOLER_MOTOR)
+        {
+            if (KeepReadValue != TranslatedReadValue)
+            {
+                calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
+                readValue[controlIndex] = TranslatedReadValue;
+                calculatedspeed[controlIndex] = calculated_speed;
+                controlIndex++;
+                if (controlIndex >= 999) controlIndex = 0;
+                KeepReadValue = TranslatedReadValue;
+            }
+        }
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
             CurrentControlledSpeed[index] = calculated_speed;
@@ -393,7 +417,7 @@ bool InitialProcess = false;
             MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
             MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
             MotorControlConfig[Motor_i].m_params.epsilon = 0.1;
-            MotorControlConfig[Motor_i].m_params.dt = 50;
+            MotorControlConfig[Motor_i].m_params.dt = 1000;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
             MotorControlConfig[Motor_i].m_isEnabled = true;
@@ -489,10 +513,16 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 
     float process_speed;
     if (JobTicket->processparameters)
-        process_speed= JobTicket->processparameters->dyeingspeed;
+        if (JobTicket->processparameters->dyeingspeed)
+            process_speed= JobTicket->processparameters->dyeingspeed;
+        else
+        {
+            LOG_ERROR (-1," unknown job speed");
+            return ERROR;
+        }
 
     SetOriginMotorSpeed(process_speed);
-    //ControlStart();
+    ThreadControlActive = true;
     // set the new speed in the dryer motor to the speed of the new segment
     MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
  //only for testing - when control works, these motors will take their speed from the dryer
@@ -556,6 +586,7 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
  uint32_t ThreadEndState(void *JobDetails)
 {
      int Motor_i;
+     ThreadControlActive = false;
      ThreadUpdateProcessLength (0.0,(void *)NULL);
     SetOriginMotorSpeed(0);