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

1 files changed, 116 insertions, 20 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 41ef9e480..a6cb25c45 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -17,7 +17,7 @@
 #include "Common/report/report.h"
 
 #include "drivers/FPGA/FPGA.h"
-
+#include "drivers/Flash_ram/MCU_E2Prom.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
@@ -54,7 +54,7 @@ double   ScrewRunningTime = 0;
 uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
 bool     SCREW_TimerActivated = false;
 uint32_t ScrewControlId = 0xFF;
-
+bool IgnoreConeMissing = false;
 
 
 static bool KeepWindingCone = false;
@@ -89,26 +89,38 @@ void SetWinderBackToBaseTime(uint32_t value)
     ReportWithPackageFilter(ThreadFilter,"Set WinderBackToBaseTime",__FILE__,__LINE__,WinderBackToBaseTime,RpWarning,KeepWindingCone, 0);
 }
 
-uint32_t InternalWinderConfigMessage(HardwareWinder* request)
+/*uint32_t InternalWinderConfigMessage(HardwareWinder* request)
 {
     uint32_t status = PASSED;
 
     InternalWinderCfg.milimetersperrotation = request->millimeterperrotation;
 
     return status;
-}
+}*/
 char ScrewStr[150];
 
 uint32_t InternalWindingConfigMessage(JobSpool* request)
 {
     uint32_t status = PASSED;
+    int limitswitchstartpointoffset = 0;
 
     InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
     InternalWinderCfg.spoolbackingrate = request->backingrate;
     InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
+    if (request->has_limitswitchstartpointoffset)
+    {
+        //InternalWinderCfg.startoffsetpulses += request->limitswitchstartpointoffset;
+        Report("limit switch start point offset",__FILE__,__LINE__,(int)(request->startoffsetpulses),RpWarning,(int)(request->limitswitchstartpointoffset), 0);
+    }
+    status |= MCU_E2PromRead(EEPROM_STORAGE_WINDER_CALIBRATION,&limitswitchstartpointoffset);
+    if ((status!= OK )||(limitswitchstartpointoffset == 0xFFFF))
+        limitswitchstartpointoffset = 0;
+    Report("limit switch start point offset",__FILE__,(int)(request->startoffsetpulses),(int)(limitswitchstartpointoffset),RpWarning,status, 0);
+    InternalWinderCfg.startoffsetpulses += limitswitchstartpointoffset;
+
     InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
     InternalWinderCfg.NumberOfRotationPerPassage = 3.1415926*3;//request->rotationsperpassage;
-    if (request->rotationsperpassage > 6.1)
+    if (request->rotationsperpassage > 5.1)
         InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
     if (NumberOfRotationPerPassage > 1)
     {
@@ -118,7 +130,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request)
     InternalWinderCfg.diameter = request->diameter;
     usnprintf(ScrewStr, 150, "WindingConfig start,offset,head,tail {, %d, %d, %d, %d, %d}",InternalWinderCfg.startoffsetpulses,(int)InternalWinderCfg.segmentoffsetpulses,
               (int)InternalWinderCfg.spoolbackingrate,(int)InternalWinderCfg.SpoolBottomBackingRate,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000));
-    Report(ScrewStr,__FILE__,__LINE__,(int)InternalWinderCfg.diameter,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0);
+    Report(ScrewStr,__FILE__,__LINE__,(int)InternalWinderCfg.startoffsetpulses,RpWarning,(int)(InternalWinderCfg.NumberOfRotationPerPassage*1000), 0);
 
     return status;
 }
@@ -128,6 +140,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request)
  * 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.
  * report ready to the job STM
  */
+int Screw_wait_counter;
 uint32_t Winder_Prepare(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
@@ -149,9 +162,17 @@ uint32_t Winder_Prepare(void *JobDetails)
     if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
     {
         REPORT_MSG(LIMIT, "No cone in winder");
-        //PrepareReady(Module_Winder,ModuleFail);
-        //AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true);
-        //return ERROR;
+        if (Is_PP_Machine())
+        {
+            if (IgnoreConeMissing == false)
+            {
+                JobEndReason = JOB_THREAD_BREAK;
+                usnprintf(AlarmReasonStr, 100, "No cone in winder");
+                PrepareReady(Module_Winder,ModuleFail);
+                AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,true);
+                return ERROR;
+            }
+        }
     }
 
 #ifdef READ_SCREW_ENCODER
@@ -187,7 +208,16 @@ uint32_t Winder_Prepare(void *JobDetails)
         //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
         Winder_ScrewHoming = true;
         //REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, "Winder_Prepare move to limit");
+        Screw_wait_counter=1;
         status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2,2000);
+#ifdef FOUR_WINDERS
+        Screw_wait_counter++;
+        status = MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, Winder_PrepareStage2,2000);
+        Screw_wait_counter++;
+        status = MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, Winder_PrepareStage2,2000);
+        Screw_wait_counter++;
+        status = MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, Winder_PrepareStage2,2000);
+#endif
     }
     return status;
 }
@@ -202,7 +232,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
     uint32_t status=OK;
     uint32_t numOfSteps = WindingConeLocation*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 
-    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
+        //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
 #ifdef READ_SCREW_ENCODER
     Read_Screw_Encoder();
@@ -211,13 +241,15 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 #endif
 
     REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
+    Screw_wait_counter--;
     if (ReadValue != LIMIT)
     {
         if (JobIsActive()==true)
         {
-            LOG_ERROR(ReadValue,"Screw failed to reach the limit switch!!");
+            LOG_ERROR(deviceID,"Screw failed to reach the limit switch!!");
             //returned with a timeout
             Winder_ScrewAtOffsetCallback(0,0);
+            PrepareReady(Module_Winder, ModuleFail);
         }
         else //short jog, etc
         {
@@ -226,18 +258,39 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
     }
     else
     {
+        if (Screw_wait_counter!=0)
+        {
+            ReportWithPackageFilter(ThreadFilter,"waiting for winders",__FILE__,__LINE__,Screw_wait_counter,RpWarning,deviceID, 0);
+            return status;
+        }
+        Screw_wait_counter++;
         status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback,1000);
+#ifdef FOUR_WINDERS
+        Screw_wait_counter++;
+        status |= MotorMoveWithCallback (SCREW_2_Motor,(1-MotorsCfg[SCREW_2_Motor].directionthreadwize), numOfSteps,  Winder_ScrewAtOffsetCallback,1000);
+        Screw_wait_counter++;
+        status |= MotorMoveWithCallback (SCREW_3_Motor,(1-MotorsCfg[SCREW_3_Motor].directionthreadwize), numOfSteps,  Winder_ScrewAtOffsetCallback,1000);
+        Screw_wait_counter++;
+        status |= MotorMoveWithCallback (SCREW_4_Motor,(1-MotorsCfg[SCREW_4_Motor].directionthreadwize), numOfSteps,  Winder_ScrewAtOffsetCallback,1000);
+#endif
     }
      //set motor location 0 here
         return status;
 
 }
 
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t MotorId, uint32_t BusyFlag)
 {
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
+    if (Screw_wait_counter)
+        Screw_wait_counter--;
 
-    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
+    MotorStop (MotorId,Soft_Hiz); //per L6470 errata between mov and run commands
+/*#ifdef FOUR_WINDERS
+    MotorStop(SCREW_2_Motor, Hard_Hiz);
+    MotorStop(SCREW_3_Motor, Hard_Hiz);
+    MotorStop(SCREW_4_Motor, Hard_Hiz);
+#endif*/
     Task_sleep(5);
 #ifdef READ_SCREW_ENCODER
     Reset_Screw_Encoder();
@@ -248,7 +301,12 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
 
     REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location");
 #endif
-    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+    SetMotHome(MotorId);  //set this point as the spool home
+/*#ifdef FOUR_WINDERS
+    SetMotHome(SCREW_2_Motor);
+    SetMotHome(SCREW_3_Motor);
+    SetMotHome(SCREW_4_Motor);
+#endif*/
 
     ScrewCurrentDirection = false;
 #ifdef READ_SCREW_ENCODER
@@ -257,8 +315,9 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewSpeed = 0;
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
-    REPORT_MSG(millisecondCounter, "Winder_ScrewAtOffsetCallback");
-    PrepareReady(Module_Winder, ModuleDone);
+    ReportWithPackageFilter(ThreadFilter,"Winder_ScrewAtOffsetCallback",__FILE__,millisecondCounter,Screw_wait_counter,RpWarning,MotorId, 0);
+    if (Screw_wait_counter == 0)
+        PrepareReady(Module_Winder, ModuleDone);
     return OK;
 }
 
@@ -298,10 +357,17 @@ int flipflop = 0;
 uint32_t motspeed;
 float speedf;
 int WinderCalculation = 0;
+#ifdef FOUR_WINDERS
+int MotorOrder[4] = {HARDWARE_MOTOR_TYPE__MOTO_SCREW,SCREW_2_Motor,SCREW_3_Motor,SCREW_4_Motor};
+#endif
+
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     //uint32_t Steps;
     double temp,tempScrewSpeed;
+#ifdef FOUR_WINDERS
+    int tempmot;
+#endif
     //double screw_horizontal_speed = 0;
     //double RotationsPerSecond;
         if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
@@ -320,6 +386,14 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 
     ScrewCurrentDirection = 1-ScrewCurrentDirection;
     CalculationDirectionChangeCounter++;
+#ifdef FOUR_WINDERS
+    tempmot = MotorOrder[0];
+    MotorOrder[0] = MotorOrder[1];
+    MotorOrder[1] = MotorOrder[2];
+    MotorOrder[2] = MotorOrder[3];
+    MotorOrder[3] = tempmot;
+    //Report("order change" ,__FILE__,MotorOrder[0],MotorOrder[1],RpWarning,MotorOrder[2], 0);
+#endif
     if (SampleWinding)
         return OK;
     //double calcsteps = (ScrewRunningTime/SYS_CLK_FREQ)*ScrewSpeed;
@@ -539,7 +613,7 @@ uint32_t WinderDistanceToSpoolEnded(uint32_t deviceID, uint32_t ReadValue)
 }
 uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag)
 {
-    REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback called");
+    Report("ScrewDTSCallback called", __FILE__,__LINE__,msec_millisecondCounter, RpMessage, ScrewSpeed, 0);
     SafeRemoveControlCallback(ScrewDTSControlId, ScrewDTSCallback);
 
     if (ScrewControlId != 0xFF)
@@ -553,6 +627,11 @@ uint32_t ScrewDTSCallback(uint32_t deviceID, uint32_t BusyFlag)
     //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0);
     //MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
     MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, WinderDistanceToSpoolEnded,2000);
+#ifdef FOUR_WINDERS
+    MotorMovetoLimitSwitch (SCREW_2_Motor,MotorsCfg[SCREW_2_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE2_2, NULL,2000);
+    MotorMovetoLimitSwitch (SCREW_3_Motor,MotorsCfg[SCREW_3_Motor].directionthreadwize, ScrewSpeed, GPI_LS_SPARE1_2, NULL,2000);
+    MotorMovetoLimitSwitch (SCREW_4_Motor,MotorsCfg[SCREW_4_Motor].directionthreadwize, ScrewSpeed, GPI_LS_RDANCER_UP, NULL,2000);
+#endif
 
     return OK;
 }
@@ -562,7 +641,7 @@ uint32_t WinderDistanceToSpoolState(void )
     REPORT_MSG ((int)DTS_Time,"WinderDistanceToSpoolState");
 
     ScrewDTSControlId = AddControlCallback("screw DTS",ScrewDTSCallback, DTS_Time-WinderBackToBaseTime,TemplateDataReadCBFunction,0,0,0);
-    REPORT_MSG ((int)msec_millisecondCounter,"ScrewDTSCallback start");
+    Report("ScrewDTSCallback start", __FILE__,DTS_Time,msec_millisecondCounter, RpMessage, WinderBackToBaseTime, 0);
 
     return OK;
 }
@@ -582,9 +661,14 @@ uint32_t Winder_End(void)
     //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL,0);
     //MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
     MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //bug #2709
+#ifdef FOUR_WINDERS
+    MotorAbortMovetoLimitSwitch (SCREW_2_Motor);
+    MotorAbortMovetoLimitSwitch (SCREW_3_Motor);
+    MotorAbortMovetoLimitSwitch (SCREW_4_Motor);
+#endif
     return OK;
 }
-void Winder_ScrewHomeLimitSwitchInterrupt(void)
+/*void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
     //uint32_t status;
     //handle glitch - send information to the next time that the motor stops
@@ -599,7 +683,7 @@ void Winder_ScrewOutLimitSwitchInterrupt(void)
     //handle glitch - send information to the next time that the motor stops
     //uint32_t status;
     MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
-}
+}*/
 
 uint32_t  Screw_timerBase = TIMER3_BASE;        //Timer handle
 
@@ -643,7 +727,19 @@ void ScrewTimerInterrupt(int ARG0)
     {
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
+#ifdef FOUR_WINDERS
+        MotorSetDirection (SCREW_2_Motor, ScrewCurrentDirection);
+        MotorSetDirection (SCREW_3_Motor, ScrewCurrentDirection);
+        MotorSetDirection (SCREW_4_Motor, ScrewCurrentDirection);
+#endif
+#ifdef FOUR_WINDERS
+        MotorSetSpeedDirect(MotorOrder[0],ScrewSpeed);
+        MotorSetSpeedDirect (MotorOrder[1], ScrewSpeed);
+        MotorSetSpeedDirect (MotorOrder[2], ScrewSpeed);
+        MotorSetSpeedDirect (MotorOrder[3], ScrewSpeed);
+#else
         MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+#endif
 #ifdef READ_SCREW_ENCODER
         Read_Screw_Encoder();
         ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;