Version 1.4.0.2 improve winding (simplify). incorporate screw encoder reading. fix length/time problem between IDS and Thread.

1 files changed, 71 insertions, 45 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 66c2e1659..9ebc32f57 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -43,10 +43,11 @@ uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of
 //uint32_t ScrewChangeCounter = 0;
 //uint32_t ScrewChangeLimit = 0;
 uint32_t CalculationDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
-uint16_t WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
+double WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
 uint16_t WinderMotorSpeedCounter = 0;
 bool     WinderMotorSpeedRollOver = false;
-double   ScrewSpeed = 0;
+#define DEFAULT_SCREW_SPEED 1400
+double   ScrewSpeed = DEFAULT_SCREW_SPEED;
 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;
@@ -96,7 +97,7 @@ uint32_t Winder_Prepare(void)
     uint32_t status = 0;
     //JobTicket* JobTicket = JobDetails;
     //float process_speed = JobTicket->processparameters->dyeingspeed;
-    double ScrewSpeed = 1500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
+    double ScrewSpeed = DEFAULT_SCREW_SPEED;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
     /*
@@ -143,9 +144,9 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
-    //Read_Screw_Encoder();
-    //ScrewLocationLimitSwitch = Screw_RotEnc.Position;
-    //REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
+    Read_Screw_Encoder();
+    ScrewLocationLimitSwitch = Screw_RotEnc.Position;
+    REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
 
 
     REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
@@ -169,8 +170,13 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
 
-    //Read_Screw_Encoder();
-    //ScrewLocationStart = Screw_RotEnc.Position;
+    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
+    Task_sleep(5);
+    Reset_Screw_Encoder();
+    Task_sleep(5);
+    Read_Screw_Encoder();
+    Task_sleep(5);
+    ScrewLocationStart = Screw_RotEnc.Position;
 
     REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location");
 
@@ -180,7 +186,6 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
     REPORT_MSG(millisecondCounter, "Winder_ScrewAtOffsetCallback");
-    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
     PrepareReady(Module_Winder, ModuleDone);
     return OK;
 }
@@ -212,26 +217,28 @@ InternalWinderCfg.segmentoffsetpulses
   
 numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 */
-char ScrewStr[100];
+char ScrewStr[150];
 //char TempScrewStr[100];
 double WinderReferenceSpeed=0;
-int32_t  TotalWinderSpeed=0;
+double  TotalWinderSpeed=0;
 bool Add100 = false;
 double Rotations = 6.0;
+bool flipflop = false;
+uint32_t motspeed;
+float speedf;
+int WinderCalculation = 0;
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     //uint32_t Steps;
     double temp,tempScrewSpeed;
     double screw_horizontal_speed = 0;
     double RotationsPerSecond;
-    int32_t Averagewinderspeed = 0;
+    double Averagewinderspeed = 0;
 
-    //ScrewChangeCounter++;
-    //if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
-    {
-        TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
-        WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
-        TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
+//    {
+//        TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
+//        WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
+//        TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
         if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
         {
             if (WinderMotorSpeedRollOver == false)
@@ -239,20 +246,34 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
                 Add100 = true;
                 LOG_ERROR(Add100, "Add100 = true");
             }
-            WinderMotorSpeedCounter=0;
+//            WinderMotorSpeedCounter=0;
             WinderMotorSpeedRollOver=true;
         }
+//    }
+    if (flipflop)
+    {
+        speedf = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
+    else
+    {
+        MotorGetSpeedFromFPGA1(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    }
+    flipflop = 1-flipflop;
 
     if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
         return OK;
+    //deley TODO
+
 
     ScrewCurrentDirection = 1-ScrewCurrentDirection;
     CalculationDirectionChangeCounter++;
-    //REPORT_MSG(ScrewLocationRun[1] - ScrewLocationRun[0], "Screw Run NumberOfSteps");
-    //usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]));
+    //double calcsteps = (ScrewRunningTime/SYS_CLK_FREQ)*ScrewSpeed;
+    //REPORT_MSG((abs(ScrewLocationRun[1] - ScrewLocationRun[0]), "Screw Run NumberOfSteps");
+//    usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d intent %d rot %d",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,Rotations*10);
+    usnprintf(ScrewStr, 150, "Winder Encoder:id, diff, intended,  winderspeed,  rotation, speed, time, mot speed {, %d, %d, %d, %d, %d, %d, %d, %d, }",CalculationDirectionChangeCounter,
+              abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,(int)(WinderReferenceSpeed),(int)(Rotations*10),(int)ScrewSpeed,(int)ScrewRunningTime,(int)speedf);
     //usnprintf(ScrewStr, 100, "Winder Encoder: 0 %d 1 %d diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],ScrewLocationRun[1] - ScrewLocationRun[0]);
-    //Report(ScrewStr,__FILE__,__LINE__,ScrewLocationLimitSwitch,RpWarning,ScrewLocationStart, 0);
+    Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0);
 
     if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out
     {
@@ -265,8 +286,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
             ScrewNumberOfSteps--;
-            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
-
+            Report("Head Backing",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0);
         }
     }
     else //next time going back
@@ -274,34 +294,38 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0)
         {
             ScrewNumberOfSteps++;
-            REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
+            Report("Bottom Backing ",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0);
         }
     }
-    if (WinderMotorSpeedRollOver)
+   /* if (WinderMotorSpeedRollOver)
     {
-        /*for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++)
+        if (WinderCalculation%60000 == 0)//100 minutes
         {
-            TotalWinderSpeed+=WinderMotorSpeed[i];
-        }*/
-        Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
-        //REPORT_MSG(winderspeed, "WinderSpeedUpdated");
-        //Report("WinderSpeedUpdated",__FILE__,__LINE__,TotalWinderSpeed,RpWarning,Averagewinderspeed,0);
-
-        WinderReferenceSpeed = Averagewinderspeed;
-    }
+            Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
+            //Report("WinderSpeedUpdated",__FILE__,__LINE__,(int)TotalWinderSpeed,RpWarning,(int)Averagewinderspeed,0);
+            WinderReferenceSpeed = Averagewinderspeed;
+        }
+        WinderCalculation++;
+    }*/
+    //WinderReferenceSpeed = 1000;
+    //ScrewNumberOfSteps = 1000;
     screw_horizontal_speed = ScrewNumberOfSteps /  Rotations;//InternalWinderCfg.NumberOfRotationPerPassage;
-    if (Rotations > 6.6)//7.0)
-        Rotations = 6.0;
+  //  if (Rotations > 6.6)//7.0)
+  //      Rotations = 6.0;
     RotationsPerSecond =  WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
     tempScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
     //ROM_IntMasterDisable();
+    tempScrewSpeed = DEFAULT_SCREW_SPEED;
+
     CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+
     temp = SYS_CLK_FREQ;
     temp *= ScrewNumberOfSteps;
     temp /= tempScrewSpeed;
     if ((ScrewRunningTime != temp)||(ScrewSpeed != tempScrewSpeed))
     {
         ScrewSpeed = tempScrewSpeed;
+        ScrewSpeed = DEFAULT_SCREW_SPEED;
         ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
         //ROM_IntMasterEnable();
         //usnprintf(TempScrewStr, 100, "Winder: Horizon,Rotation, PPR, RPP{ %d, %d ,%d, %d} ",(int)screw_horizontal_speed,(int)RotationsPerSecond,(int)InternalWinderCfg.NumberOfRotationPerPassage,(int)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround);
@@ -353,6 +377,7 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId)
         RotationsPerSecond =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
         // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
         ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
+        ScrewSpeed = DEFAULT_SCREW_SPEED;
 
         //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         usnprintf(ScrewStr, 100, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
@@ -372,12 +397,12 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId)
         temp *= ScrewNumberOfSteps;
         temp /= ScrewSpeed;
         ScrewRunningTime = temp;//(SYS_CLK_FREQ*InternalWinderCfg.segmentoffsetpulses)/ScrewSpeed;
-        REPORT_MSG((int)ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps");
-        REPORT_MSG((int)ScrewRunningTime,"Winder pre segment - ScrewRunningTime");
+        REPORT_MSG(ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps");
+        REPORT_MSG(ScrewRunningTime,"Winder pre segment - ScrewRunningTime");
 //        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
         //ScrewDirection = 1-ScrewDirection;
         REPORT_MSG(SegmentId,"Winder pre segment - SegmentId");
-        REPORT_MSG((int)ScrewSpeed,"Winder pre segment - ScrewSpeed");
+        REPORT_MSG(ScrewSpeed,"Winder pre segment - ScrewSpeed");
         //MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         ScrewsStartControlTimer ();
         ScrewControlId = AddControlCallback(Screw100msecDirectionChange, eHundredMillisecond,TemplateDataReadCBFunction,0,0,0);
@@ -458,20 +483,20 @@ void ScrewsStartControlTimer (void)
     return;
 }
 
-int random = 0;
+
 void ScrewTimerInterrupt(int ARG0)
 {
     ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     ROM_IntMasterDisable();
 
-    //Read_Screw_Encoder();
-    //ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
 
     if (SCREW_TimerActivated == true)
     {
+        Read_Screw_Encoder();
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
-        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed+random);
+        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
       //  ScrewChangeCounter = 0;
       //  ScrewChangeLimit = ScrewRunningTime/12000000;
         ScrewDirectionChangeCounter++;
@@ -481,7 +506,8 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
-    Rotations+=0.03;
+    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+    //Rotations+=0.03;
 
     return ;