From 23abfd4bf042906abe5de4a46a15c529fceb54d7 Mon Sep 17 00:00:00 2001
From: Shlomo Hecht <shlomo@twine-s.com>
Date: Wed, 18 Dec 2019 19:19:23 +0200
Subject: some fixes in thread loading

---
 .../Embedded/Modules/Thread/ThreadLoad.c           | 94 +++++++++++++++-------
 1 file changed, 65 insertions(+), 29 deletions(-)

(limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index aeacde455..373e4bbd9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -79,7 +79,7 @@
     uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF;
     uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF;
     uint32_t LoadCounter = 0;
-    double TotalProcessedLen = 0.0;
+    double TotalLoadedLen = 0.0;
     double LengthCalculationMultiplier;
     uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
     uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
@@ -188,18 +188,18 @@
         }
         return OK;
     }
-    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
        // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
 
-        /*NumberOfDrierLoaderCycles--;
+        /`*NumberOfDrierLoaderCycles--;
         if (NumberOfDrierLoaderCycles)
         {
             Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
             status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                             MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
         }
-        else*/
+        else*`/
         {
             Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
             //storeLoadArmParameters();
@@ -208,7 +208,7 @@
             ThreadLoadStateMachine(LoadStages);
         }
         return OK;
-    }
+    }*/
     uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
 
@@ -224,6 +224,8 @@
             RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
             WinderControlId = 0xFF;
         }
+
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
         /*Task_sleep(5)
         MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
         MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/
@@ -279,7 +281,7 @@
 
     uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
     {
-        Report("Thread_Load_HomingCallback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+        Report("Thread_Load_HomingCallback",__FILE__,MotorId,LoadStages,RpMessage,CallbackCounter,0);
 //        REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
         if ((LoadStages == THREAD_LOAD_CLOSE_ROCKERS)||(LoadStages == THREAD_LOAD_LIFT_ROCKERS))
         {
@@ -360,7 +362,7 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
         //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
-        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000);
+        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
         return OK;
     }
     uint32_t Thread_Load_Lift_Dancers(void)
@@ -440,7 +442,10 @@
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500);
+
+//        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, (2000*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
 
         return OK;
     }
@@ -454,7 +459,7 @@
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
         CallbackCounter++;
 //        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
-        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000);
+        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
 
         return OK;
     }
@@ -499,7 +504,7 @@
            RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
            SpeedTControlId = 0xFF;
         }
-        Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalProcessedLen,0);
+        Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0);
 
         if (CallbackCounter)
         {
@@ -518,17 +523,32 @@
 
         return OK;
     }
+    float numberOfSteps = 0;
+    float numberOfCycles = 0;
     uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
-        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
-        Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,__LINE__,MotorId,RpMessage,CallbackCounter,0);
+        numberOfCycles++;
+        //Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+        //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
+        if (LoadStages != THREAD_LOAD_DRYER_LOADING)
+        {
+            return OK;
+        }
+        if (numberOfCycles<LoadArmInfo.LoadArmRounds)
+        {
+            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
+                                   numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
+        }
+        else
+        {
+            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
+        }
         /*if(LoadingControlId != 0xFF)
         {
             MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
             RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
             LoadingControlId = 0xFF;
         }*/
-        MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000);
         return OK;
     }
 
@@ -540,16 +560,15 @@
             LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
         if (LoadArmInfo.LoadArmRounds <= 2)
             LoadArmInfo.LoadArmRounds = 30;
-        float numberOfSteps = 0;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
         SetOriginMotorSpeed(19);
 //        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
 //        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
 
         MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds);
-
-        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-        numberOfSteps -= 100;
+        numberOfCycles = 0;
+        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmInfo.LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
+//        numberOfSteps -= 100;
         //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
         Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0);
         MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1;
@@ -582,6 +601,7 @@
         }
         //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
         LengthCalculationMultiplier = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulleyradius*2*PI)/(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep);
+        TotalLoadedLen = 0;
         SpeedTControlId = AddControlCallback(NULL,ThreadLoadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+HARDWARE_MOTOR_TYPE__MOTO_RDRIVING),HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,0);
         Tinitialpos = 0xFFFF;previousPosition = 0; currentPosition = 0;
         CallbackCounter++;
@@ -610,7 +630,7 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         ThreadJoggingFunc(20);
-        LoadingControlId = AddControlCallback(NULL,Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
+        LoadingControlId = AddControlCallback(NULL,Thread_Load_Jog_ThreadStop, eOneSecond*25,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
         return OK;
     }
     uint32_t Thread_Load_End(void)
@@ -652,18 +672,31 @@
         load.color = colorOFF;
         return OK;
     }
+    FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT};
+    int Screw_Dir = false;
+
+    uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag)
+    {
+        if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
+        {
+            return OK;
+        }
+        Screw_Dir = 1-Screw_Dir;
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,15000);
+
+    }
     uint32_t Thread_Load_Dryer_UnLoading(void)
     {
         REPORT_MSG(LoadStages, "Thread UnLoad State Machine step");
         LoadArmInfo.LoadArmRounds = 0;
         //uint32_t numberOfSteps = 0;
         //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
-        SetOriginMotorSpeed(30);
+        SetOriginMotorSpeed(22);
 //        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
 //        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;
 
         //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
-        MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1;
+        /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1;
         MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1;
         MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime;
         MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain;
@@ -683,7 +716,7 @@
         MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);
 
         PullerControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR);
-        Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);
+        Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);*/
 ////////////////////////
         MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
         MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
@@ -717,7 +750,10 @@
         UnloadingStart = msec_millisecondCounter;
         Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
         status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
-                                        MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+                                        MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
+
+        Screw_Dir = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,15000);
 
         //Keep Notation How Many Rotations In The Dryer
         LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
@@ -834,7 +870,7 @@ uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     uint32_t positionDiff = 0,prevprev;
     double length = 0.0;
     char lenstr[150];
-
+    static int counter = 0;
     int index = MAX_THREAD_MOTORS_NUM;
     if (IfIndex>>8 != IfTypeThread)
     {
@@ -868,17 +904,17 @@ uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
         length = (double)(positionDiff)*LengthCalculationMultiplier;
 
-        //if (length > 1000)
+        if (counter++ % 50 == 0)
         {
-            usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,previousPosition,prevprev);
-            SendJobProgress(0.0,0,false, lenstr);
-            ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalProcessedLen,RpWarning,(int)TotalProcessedLen,0);
-            length = 0;
+            usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x TotalLoadedLen %d",(int)length*100,(int)positionDiff,previousPosition,prevprev, (int)TotalLoadedLen);
+//            SendJobProgress(0.0,0,false, lenstr);
+            ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalLoadedLen,RpWarning,(int)TotalLoadedLen,0);
+//            length = 0;
 
         }
 
     }
-    TotalProcessedLen += length;
+    TotalLoadedLen += length;
 
     return OK;
 }
-- 
cgit v1.3.1