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

1 files changed, 45 insertions, 6 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 47fc37081..28ba11fcd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -49,6 +49,7 @@ uint32_t    PoolerSpeedControlId=0xFF;
 
 double DancerError[NUM_OF_DANCERS] = {0.0};
 double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
+double InitialDryerSpeed = 0.0;
 uint32_t JobCounter = 0;
 
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
@@ -731,6 +732,18 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
 
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop);
 
+    if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)
+    {
+        ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0);
+        PrepareReady(Module_Thread,ModuleFail);
+        return ERROR;
+    }
+    if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)
+    {
+        ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpFatalError,LIMIT,0);
+        PrepareReady(Module_Thread,ModuleFail);
+        return ERROR;
+    }
 
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
@@ -767,7 +780,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
                 }
                 //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
                 LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
             }
             if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
             {
@@ -780,7 +793,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
                 }
                 //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
                 PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                PoolerSpeedControlId = AddControlCallback(PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
             }
             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
             {
@@ -792,8 +805,8 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
                     CurrentControlledSpeed[Motor_i] = 0;
                 }
 #ifndef TEST_PID_THREAD
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
-                //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
 #endif
             }
             if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
@@ -806,7 +819,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
 #ifndef TEST_PID_THREAD
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
             if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
@@ -820,7 +833,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
 #ifndef TEST_PID_THREAD
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
 //            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
@@ -852,6 +865,7 @@ void SetOriginMotorSpeed(float process_speed)
                 / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius);
         //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
+        InitialDryerSpeed = 0.0;
         CurrentControlledSpeed[Motor_i] = (int) motor_speed;
         //Report("Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0);
 
@@ -865,6 +879,25 @@ void ThreadPreSegmentEnded(void)
     REPORT_MSG (0,"First ThreadPreSegmentEnded");
     PreSegmentReady(Module_Thread,ModuleDone);
 }
+#define DRYER_RAMPUP 1
+#ifdef DRYER_RAMPUP
+uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag)
+{
+    InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/10);
+    if (InitialDryerSpeed >= OriginalMotorSpd_2PPS[DRYER_MOTOR])
+    {
+        InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR];
+        SafeRemoveControlCallback(ControlIdtoMotorId[DRYER_MOTOR], ThreadDryerRampUp );
+        ControlIdtoMotorId[DRYER_MOTOR] = 0xFF;
+    }
+
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,InitialDryerSpeed );
+    Report("ThreadDryerRampUp",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0);
+
+
+    return OK;
+}
+#endif
 
 //********************************************************************************************************************
 uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
@@ -886,7 +919,13 @@ 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
+#ifndef DRYER_RAMPUP
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
+#else
+        InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/10;
+        MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,InitialDryerSpeed );
+        ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0);
+#endif
 #endif
 #ifdef HUNDRED_MICROSECONDS_DANCER_READ
      MillisecLogInit();