loading improved, actoator relocated. waste alarms

7 files changed, 80 insertions, 37 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
index 691f1c455..1ac0e439b 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
@@ -547,6 +547,12 @@ uint32_t HeadCard_ActStubCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) /
 
     ACTUATORS Act_ID = (ACTUATORS)(IfIndex);
 
+    if (SafeRemoveControlCallback(ActControlId, HeadCard_ActStubCallBackFunction )==OK)
+        ActControlId = 0xFF;
+    else
+        Report("Remove control callback failed",__FILE__,__LINE__,(int)ActControlId,RpWarning,(int)HeadCard_ActStubCallBackFunction,0);
+
+    Report("HeadCard_ActStubCallBackFunction",__FILE__,__LINE__,(int)ActControlId,RpWarning,(int)Act_ID,0);
     if(Act_ID == ACTIN)//0
     {
         Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTIN_ON  = HIGH;
@@ -563,6 +569,7 @@ uint32_t HeadCard_ActStubCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) /
 uint32_t HeadCard_Actuators_Stub(ACTUATORS Act_ID, bool Enable, bool Direction)//ENABLE/DISABLE, DOWN/UP/DONTCARE
 {
     uint32_t status = OK;
+    Report("HeadCard_Actuators_Stub",__FILE__,Act_ID,(int)Enable,RpWarning,(int)Direction,0);
 
     if(Enable == true)
     {
@@ -580,7 +587,8 @@ uint32_t HeadCard_Actuators_Stub(ACTUATORS Act_ID, bool Enable, bool Direction)/
         }
 
         //100msec delay - (need short command - Active LOW )
-        ActControlId = AddControlCallback("ctuatorControl", HeadCard_ActStubCallBackFunction, (eHundredMillisecond), TemplateDataReadCBFunction,Act_ID,0, 0 );
+        if (ActControlId == 0xFF)
+            ActControlId = AddControlCallback("ctuatorControl", HeadCard_ActStubCallBackFunction, (eHundredMillisecond), TemplateDataReadCBFunction,Act_ID,0, 0 );
     }
     else
     {
@@ -594,6 +602,10 @@ uint32_t HeadCard_Actuators_Stub(ACTUATORS Act_ID, bool Enable, bool Direction)/
         {
             Head_I2C_EXP4_0x46.bits.OUTPUT_ACTLOW_ACTOT_ON  = HIGH;
         }
+        if (RemoveControlCallback(ActControlId, HeadCard_ActStubCallBackFunction )==OK)
+            ActControlId = 0xFF;
+        else
+            Report("Remove control callback failed",__FILE__,__LINE__,(int)ActControlId,RpWarning,(int)HeadCard_ActStubCallBackFunction,0);
     }
 
     status |= Head_Write_IO_Reg(0x42,HIGH);
@@ -693,7 +705,7 @@ uint8_t Head_Test_IO()
     return status;
 }
 
-#define ACTUATOR_RELOCATING_MOVEUP_TIME 160
+#define ACTUATOR_RELOCATING_MOVEUP_TIME 16//160
 
 typedef enum
 {
@@ -725,31 +737,38 @@ bool isActuatorOnLowestLocation(void) // Give indication if the actuator stopped
 uint32_t HeadCard_Actuators_Relocate_callback(uint32_t IfIndex, uint32_t BusyFlag) // use IfIndex for Act_ID
 {
     count_time++;
+    //ReportWithPackageFilter(ThreadFilter,"actuators relocation",__FILE__,msec_millisecondCounter,ActuatorLocating,RpMessage,count_time,0);
     switch (ActuatorLocating)
     {
         case ActuatorIdle:
             break;
         case ActuatorMoveDown:
-            Trigger_Head_Actuators_Stub(ACTIN, ENABLE, DOWN);
-            ActuatorLocating = ActuatorWait;
-            ReportWithPackageFilter(ThreadFilter,"actuators relocation",__FILE__,msec_millisecondCounter,ActuatorLocating,RpMessage,count_time,0);
+            if (count_time <=1)
+                Trigger_Head_Actuators_Stub(ACTIN, ENABLE, DOWN);
+            if (count_time > 10)
+            {
+                ReportWithPackageFilter(ThreadFilter,"actuators relocation",__FILE__,msec_millisecondCounter,ActuatorLocating,RpMessage,count_time,0);
+                ActuatorLocating = ActuatorWait;
+            }
             break;
         case ActuatorWait:
             if (isActuatorOnLowestLocation())//read the inputs in Hundred_msTick
             {
-                ActuatorLocating = ActuatorMoveUp;
                 Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP);
                 moveuptime = count_time;
                 ReportWithPackageFilter(ThreadFilter,"actuators relocation",__FILE__,msec_millisecondCounter,ActuatorLocating,RpMessage,count_time,0);
+                ActuatorLocating = ActuatorMoveUp;
             }
             break;
         case ActuatorMoveUp:
             if (count_time>=(moveuptime+MovingUpTime))
             {
                 Trigger_Head_Actuators_Stub(ACTIN, DISABLE, DONTCARE);
-                ActuatorLocating = ActuatorLocated;
                 ReportWithPackageFilter(ThreadFilter,"actuators relocation",__FILE__,msec_millisecondCounter,ActuatorLocating,RpMessage,count_time,0);
+                ActuatorLocating = ActuatorLocated;
             }
+            else
+                Trigger_Head_Actuators_Stub(ACTIN, ENABLE, UP);
             break;
         case ActuatorLocated:
             if (SafeRemoveControlCallback(ActuatorLocatingControl, HeadCard_Actuators_Relocate_callback )==OK)
@@ -770,7 +789,7 @@ uint32_t HeadCard_Actuators_Relocate(void)
         ActuatorLocating = ActuatorMoveDown;
         count_time = 0;
         ReportWithPackageFilter(ThreadFilter,"actuators relocation",__FILE__,msec_millisecondCounter,ActuatorLocating,RpMessage,count_time,0);
-        ActuatorLocatingControl =  AddControlCallback("Actuator Location", HeadCard_Actuators_Relocate_callback, eOneMillisecond, TemplateDataReadCBFunction,0,0, 0 );
+        ActuatorLocatingControl =  AddControlCallback("Actuator Location", HeadCard_Actuators_Relocate_callback, eTenMillisecond, TemplateDataReadCBFunction,0,0, 0 );
     }
     return ActuatorLocatingControl;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
index 7bda97734..9e7ab1e56 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
@@ -232,7 +232,10 @@ uint32_t SetDigitalOutRequestRequestFunc(MessageContainer* requestContainer)
                 Pumps_Control(WASTECH_PUMP2, request->value);
             break;
         case INTERFACE_IOS__GPO_LED3:
-            Trigger_Head_Actuators_Control(ACTIN, LOW,request->value);
+            if (request->value == true)
+                Trigger_Head_Actuators_Control(ACTIN, LOW,request->value);
+            else
+                HeadCard_Actuators_Relocate();
             break;
         case INTERFACE_IOS__GPO_LED4:
             Trigger_Head_Actuators_Control(ACTOT, LOW,request->value);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
index 0be1e7ebe..c9e9afdde 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
@@ -101,8 +101,9 @@ uint32_t IDS_Cleaning_Center_And_Stop_Rockers (int timeout,callback_fptr callbac
     uint32_t status = OK;
     //status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, SaveRightRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], callback,timeout);
     //status |= MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, SaveLeftRockerSpeed,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], callback,timeout);
-    Trigger_Head_Actuators_Disable();
-    Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+    HeadCard_Actuators_Relocate();
+    //Trigger_Head_Actuators_Disable();
+    //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
     CleaningStageCounter = 0;
     CleaningStage = CleaningStageIdle;
     Report("IDS_Cleaning_Center_And_Stop_Rockers actuator down", __FILE__, __LINE__, timeout, RpWarning,   123456, 0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 3a7045a6e..c912bca21 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -85,5 +85,8 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 
 void SetOriginMotorSpeed(float process_speed);
 
+uint32_t ThreadPrepare_Tension (int DancerId, double tension);
+
+
 #endif //MODULES_THREAD_THREAD_H_
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index 2c8db8b6c..f95420725 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -236,6 +236,7 @@
         //storeLoadArmParameters();
         MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
         SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+        MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
 
         return OK;
     }
@@ -461,23 +462,35 @@
     uint32_t Thread_Load_Close_Dancers(void)
     //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread
     {
+        uint32_t  current = 0;
         REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Dancers");
         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++;
-        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, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
-        status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension);
-        if (Is_PP_Machine() == true) //PP machine - new LTFU
+        status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,&current);
+
+        ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension);
+/*        if (current!=windertension)
         {
             CallbackCounter++;
-            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500);
-            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000);
-            status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension);
-        }
+            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500);
+            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
+            status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension);
+        }*/
+        if (Is_PP_Machine() == true) //PP machine - new LTFU
+        {
+            ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension);
+            /*status |= MCU_E2PromRead(EEPROM_PULLER_TENSION_POSITION,&current);
+            if (current!=pullertension)
+            {
+                CallbackCounter++;
+                MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500);
+                MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000);
+                status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension);
+            }*/
 
+        }
         return OK;
     }
     uint32_t Thread_Load_Close_Lids(void)
@@ -507,7 +520,8 @@
         {
             Report("Thread_Load_Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0);
             Trigger_Head_Actuators_Control(ACTOT, LOW,false);
-            Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+            //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+            HeadCard_Actuators_Relocate();
         }
         return OK;
     }
@@ -515,6 +529,7 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating");
         ActivateHeadMagnet();
+        HeadCard_Actuators_Relocate();
 
         if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK)
         {
@@ -624,6 +639,7 @@
             {
                 return OK;
             }
+            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
             if (numberOfCycles<LoadArmRounds)
             {
                 MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
@@ -632,7 +648,6 @@
             else
             {
                 MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize);
-                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                 Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
             }
         }
@@ -810,7 +825,8 @@
         }
         Screw_Dir = 1-Screw_Dir;
         // movement of the screw while unloading the thread from the dryer to the spool
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,13000);
+        AlarmHandlingSetAlarm (EVENT_TYPE__SCREW_MOTOR_LIMIT_TIMEOUT,OFF);  //handle alarm detection and operation
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000);
 
         return OK;
     }
@@ -832,6 +848,7 @@
             Report("Thread_Load_Dryer_UnLoading cycles",__FILE__,numberOfCycles,LoadArmRounds,RpMessage,0,0);
             if (numberOfCycles<LoadArmRounds)
             {
+                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
                 MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                        numberOfSteps,    Thread_Load_Dryer_UnLoading_Callback, 10000);
             }
@@ -852,9 +869,9 @@
             if (abs(temp -DrierPrevLocation)<200)
             {
                 if (temp<DrierPrevLocation)
-                    direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
-                else
                     direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
+                else
+                    direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
                 MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction);
 
             }
@@ -1239,15 +1256,15 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
 
     NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
-    if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
-    {
-        NormalizedError = (-1*NormalizedError);
-    }
     MotorControlConfig[index].m_mesuredParam = 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) //feeder unit handles errors opposite to left unit
+    {
+        MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+    }
     calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-    //if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
     {
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index e1f887303..39ff91558 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -145,7 +145,7 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
 //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
-    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2);
+    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);//*3/2);
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index 40e23e0b3..de95ddb03 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
@@ -397,12 +397,6 @@ void Waste_StateMachine(void)
                     break;
                 }
 
-                //check door
-                if (DoorState == OPEN) {
-                    Report("Close cartridge cover", __FILE__, __LINE__, DoorState, RpMessage, 0, 0);
-                    break;
-                }
-
                 // select cartridge
                 SelectedCard = cartSELECT_CART();
                 if (SelectedCard == MaxWasteCartridges) {
@@ -413,6 +407,12 @@ void Waste_StateMachine(void)
                     AlarmHandlingSetAlarm( EVENT_TYPE__NO_WASTE_CARTRIDGE_AVAILABLE,  false);
                 }
 
+                //check door
+                //moved door handling down - I want to see the alarm even if the door is open
+                if (DoorState == OPEN) {
+                    Report("Close cartridge cover", __FILE__, __LINE__, DoorState, RpMessage, 0, 0);
+                    break;
+                }
                 //check RFID? (TBD)
 
                 //move to next state