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

4 files changed, 80 insertions, 17 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index c5cdb00d6..856812c12 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -18,6 +18,7 @@
 #include "Modules/control/pidalgo.h"
 
 #include "Modules/heaters/heaters_ex.h"
+#include "Modules/General/buttons.h"
 #include "StateMachines/Initialization/InitSequence.h"
 
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
@@ -151,7 +152,7 @@
         ProcessParametersClear.headzone6temp  = 0;
         ProcessParametersClear.dyeingspeed = 40;
         ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength;
-        if (HandleProcessParameters(&ProcessParametersClear)!= OK)
+        if (HandleProcessParameters(&ProcessParametersClear,false)!= OK)
         {
             LOG_ERROR (LoadStages, "Thread_Load_Init failed");
         }
@@ -223,7 +224,7 @@
         ThreadLoadStateMachine(LoadStages);
         return OK;
     }
-    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
+    /*uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
     {
         uint32_t numberOfSteps = 0;
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
@@ -238,14 +239,14 @@
         }
         else //number of circles is not known - compare to position of the motor to verify location at the stopper
         {
-            Report("Thread_Load_Set_Load_Arm_To_Start_Position unknown cycles",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,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*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
 
         }
         //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
         return OK;
-    }
+    }*/
     uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t BusyFlag)
     {
         CallbackCounter--;
@@ -265,6 +266,12 @@
         {
             if((MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)||(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING))
             {
+                if(LoadStages == THREAD_LOAD_CLOSE_ROCKERS)
+                {
+                    MotorMove(MotorId,MotorsCfg[MotorId].directionthreadwize,400);
+			        Report("Move rockers further down",__FILE__,__LINE__,MotorId,RpMessage,400,0);
+                    Task_sleep(200);
+                }
                 MotorSetMicroStep(MotorId, keepmicrostep);
                 MotorSetKvalRun(MotorId, keepkvalrun);
             }
@@ -273,6 +280,7 @@
         if (CallbackCounter)
         {
             CallbackCounter--;
+            load.color = BLINK;
             if (ReadValue != LIMIT)
             {
                 //returned with a timeout
@@ -284,10 +292,12 @@
                 {
                     LOG_ERROR(LoadStages,"Load sequence timeout");
                     TimeoutsCounter = 0;
+                    load.color = fastBILNK;
                 }
                 else
                 {
                     LoadStages++;
+                    load.color = colorON;
                     if (LoadStages == THREAD_LOAD_CLOSE_DANCERS)
                     {
                         MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold);
@@ -296,6 +306,10 @@
                     {
                         ThreadLoadStateMachine(LoadStages);
                     }
+                    else
+                    {
+                        load.color = fastBILNK;
+                    }
                 }
             }
         }
@@ -334,7 +348,7 @@
         CallbackCounter++;
         MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);
         CallbackCounter++;
-        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 600,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
 
         return OK;
     }
@@ -403,7 +417,7 @@
         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, 600,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
+        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);
 
         return OK;
     }
@@ -426,7 +440,7 @@
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         ActivateHeadMagnet();
 
-        if (HandleProcessParameters(&ProcessParametersRecover)!= OK)
+        if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK)
         {
             LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed");
         }
@@ -494,8 +508,8 @@
 
         if (dryerbufferlength)
             LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
-        //if (LoadArmInfo.LoadArmRounds <= 2)
-        //    LoadArmInfo.LoadArmRounds = 20;
+        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);
@@ -591,6 +605,7 @@
         MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID);
         MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
         LoadStages = THREAD_LOAD_INIT;
+        load.color = colorOFF;
         return OK;
     }
     uint32_t Thread_Load_Dryer_UnLoading(void)
@@ -664,7 +679,7 @@
         //Keep Notation How Many Rotations In The Dryer
         LoadArmInfo.LoadArmBackLash = 5;
         LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
-        //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
+        load.color = BLINK;
         return OK;
     }
 
@@ -825,6 +840,35 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
     }
+    if (index == POOLER_MOTOR)
+    {
+        if ((TranslatedReadValue)<(-1200))
+        {
+            if(PullerControlId != 0xFF)
+            {
+                Report("Puller stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0);
+                MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
+                PullerControlId = 0xFF;
+            }
+        }
+
+    }
+    if (index == WINDER_MOTOR)
+    {
+        if ((TranslatedReadValue)<(-1200))
+        {
+            if(WinderControlId != 0xFF)
+            {
+                Report("Winder stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0);
+                MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
+                RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
+                WinderControlId = 0xFF;
+            }
+
+        }
+
+    }
 /*    LoadCounter++;
     if ((LoadCounter % 5001) == 0)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index a93387e0a..741fed7af 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -66,6 +66,7 @@ InternalWinderConfigStruc InternalWinderCfg = {0};
 #ifdef READ_SCREW_ENCODER
 uint32_t ScrewLocationLimitSwitch = 0,ScrewLocationStart = 0;
 uint32_t ScrewLocationRun[3];
+uint32_t ScrewLocationPrev[3];
 float WinderRunAverage = 0.0,WinderRunSum = 0.0;
 int WinderRunSamples = 0;
 #endif
@@ -305,7 +306,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 //            WinderMotorSpeedCounter=0;
         }
 //    }
-    if (flipflop == 0)
+    /*if (flipflop == 0)
     {
         MotorGetSpeedFromFPGA1(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
@@ -314,11 +315,11 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         speedf = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
     flipflop ++;
-
+    */
     if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
         return OK;
     //deley TODO
-    flipflop = 0;
+    //flipflop = 0;
 
     ScrewCurrentDirection = 1-ScrewCurrentDirection;
     CalculationDirectionChangeCounter++;
@@ -342,6 +343,16 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
              Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0);
          }
      }
+     if (abs(ScrewLocationPrev[1]-ScrewLocationRun[1])>21)
+     {
+         Report("Winder difference",__FILE__,CalculationDirectionChangeCounter,ScrewLocationPrev[1],RpWarning,ScrewLocationRun[1], 0);
+     }
+     ScrewLocationPrev[1] = ScrewLocationRun[1];
+     if (abs(ScrewLocationPrev[0]-ScrewLocationRun[0])>21)
+     {
+         Report("Winder difference",__FILE__,CalculationDirectionChangeCounter,ScrewLocationPrev[0],RpWarning,ScrewLocationRun[0], 0);
+     }
+     ScrewLocationPrev[0] = ScrewLocationRun[0];
      //else
      //    Report("winder run error value" ,__FILE__,WinderRun,ScrewLocationRun[0],RpWarning,ScrewLocationRun[1], 0);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 416b53494..226bb2559 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -23,6 +23,7 @@
 #include "drivers/Flash_ram/FlashProgram.h"
 
 #include "drivers/Flash_ram/MCU_E2Prom.h"
+#include "modules/ids/ids_ex.h"
 
 #include "thread.h"
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
@@ -105,6 +106,10 @@ uint32_t MotorsConfigMessage(HardwareConfiguration * HWrequest)
         MotorsCfg[Motor_i].toffmin  = request->toffmin ;
 
             status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
+            if ((Motor_i >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)&&(Motor_i <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8))
+            {
+                IDS_Dispenser_Init(Motor_i-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1);
+            }
 //        if (Motor_i == MOTOR_RDRIVING)
 //            ThreadInitialTestStub(request);
     }
@@ -173,7 +178,7 @@ uint32_t StoreDancerConfigMessage()
     status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
     status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
     status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint);
-    Report("Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
+    Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
 /*
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 41d98e892..9d408a4a6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -883,9 +883,10 @@ void ThreadPreSegmentEnded(void)
 }
 #define DRYER_RAMPUP 1
 #ifdef DRYER_RAMPUP
+int DrierDivider = 10;
 uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag)
 {
-    InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/10);
+    InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider);
     if (InitialDryerSpeed >= OriginalMotorSpd_2PPS[DRYER_MOTOR])
     {
         InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR];
@@ -924,7 +925,9 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
 #ifndef DRYER_RAMPUP
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
 #else
-        InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/10;
+        DrierDivider = dyeingspeed/5;  //ramp up drier in 5 cm/sec steps
+        Report("Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0);
+        InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider;
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,InitialDryerSpeed );
         ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0);
 #endif