Winder changes, Job stop on dancer extreme move

4 files changed, 33 insertions, 17 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index ba6597449..57b7a9d99 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -55,6 +55,7 @@ extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
+extern double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 17d683444..164e5eb23 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -13,6 +13,7 @@
 #include "drivers/FPGA/FPGA.h"
 
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
 bool Winder_ScrewHoming = false;
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps);
@@ -40,8 +41,8 @@ uint32_t Winder_Test1(uint32_t deviceID, uint32_t ReadValue)
     SysCtlDelay(100000);
     position = Fpga_Spi[HARDWARE_MOTOR_TYPE__MOTO_SCREW].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id);
 
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI112_LS_SCREW_LEFT, Winder_Test2);
-        //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_LEFT, Winder_Test2);
+//        status |= MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
         //set motor location 0 here
         return status;
 
@@ -61,7 +62,7 @@ uint32_t Winder_Test2(uint32_t deviceID, uint32_t ReadValue)
     SysCtlDelay(100000);
     position = Fpga_Spi[HARDWARE_MOTOR_TYPE__MOTO_SCREW].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id);
 
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI113_LS_SCREW_RIGHT, Winder_Test1);
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1);
         //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
         //set motor location 0 here
         return status;
@@ -74,14 +75,14 @@ uint32_t Winder_Prepare(void *JobDetails)
 
     float process_speed = JobTicket->processparameters->dyeingspeed;
 //    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+    double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
 
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
      * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
      * report ready to the job STM
      */
-    if (PollGPIO(GPI113_LS_SCREW_RIGHT))
+    if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
     {
         //go to prepare stage 2
         Winder_PrepareStage2(0,0);
@@ -89,8 +90,8 @@ uint32_t Winder_Prepare(void *JobDetails)
     else
     {
         Winder_ScrewHoming = true;
-//        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI113_LS_SCREW_RIGHT, Winder_PrepareStage2);
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI113_LS_SCREW_RIGHT, Winder_Test1);
+//        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2);
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1);
     }
     return status;
 }
@@ -104,6 +105,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
      * report ready to the job STM
      */
         status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out
+//        status |= MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
         //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
         //set motor location 0 here
         return status;
@@ -122,7 +124,7 @@ uint32_t Winder_Presegment(void *JobDetails)
     float RotationsPerSecond;
 
 //    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
+    double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
 
 /*typedef struct
 {
@@ -167,12 +169,12 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void)
     {
         StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
     }
-    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
+    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
 void Winder_ScrewOutLimitSwitchInterrupt(void)
 {
     uint32_t status;
-    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out
+    status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
 {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index d27b4f94e..e2d492b47 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -17,6 +17,7 @@ HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
+double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
 InternalWinderConfigStruc InternalWinderCfg = {0};
 HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
 
@@ -100,7 +101,8 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
 //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
-
+    DancerStopActivityLimit[Motor_i] = ((1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits)*(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm)*2));
+    DancerStopActivityLimit[Motor_i] = DancerStopActivityLimit[Motor_i]/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
 uint32_t DancerConfigMessage(HardwareDancer * request)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 3ebb0304b..56e1bf844 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -38,7 +38,7 @@ uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 uint32_t    SpeedControlId=0xFF;
 
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
-
+uint32_t JobCounter = 0;
 typedef struct
 {
     bool                m_isEnabled;
@@ -168,7 +168,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
 #warning control disabled
         CurrentProcessedLength+=length;
-#warning control disabled
+
     PosDif[MotorDataIndex] = CurrentPosition;
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
@@ -192,8 +192,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     if (CurrentProcessedLength>=CurrentRequestedLength )
     {
-        SendJobProgress(100,0,true);
-		#warning handle job wit several segments!!!
         // segment/intersegment/distance to spool finished
         if (ProcessedLengthFuncPtr)
             ProcessedLengthFuncPtr();
@@ -287,8 +285,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         if (ReadValue < 10)
             return OK;
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
-        if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite
+        if (index == POOLER_MOTOR)
+        {
+			//pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
+            JobCounter++;
+        }
         //TranslatedReadValue = 0;//test
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
@@ -296,6 +298,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
         avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+        //Stop Execution if the dancer moves too much
+#warning need to disable this in the first second of the job execution
+        if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
+        {
+            EndState(CurrentJob);
+        }
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
@@ -316,7 +324,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         pooler_counter++;
         if (pooler_counter>=1000)
         {
-            float error_integered = MotorControlConfig[index].m_calculatedError*1000;
+            //float error_integered = MotorControlConfig[index].m_calculatedError*1000;
             /*{
   "HeaterGroupId": 0,
   "Zone1Temp": 80,
@@ -357,6 +365,8 @@ bool InitialProcess = false;
 {
     int Motor_i, HW_Motor_Id, Pid_Id;
     CurrentSegmentId = 0;
+
+    JobCounter = 0;
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
@@ -494,6 +504,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     }
     else
     {
+        ThreadUpdateProcessLength (0,(void *)NULL);
         PreSegmentReady(Module_Thread,ModuleDone);
         InitialProcess = false;
     }