Winder bugs. improved trace, improved control (callback invoked after time requested elabsed)

5 files changed, 83 insertions, 46 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index cfba461ff..8a54526a5 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -392,7 +392,7 @@ uint32_t MotorMoveWithCallback (TimerMotors_t _motorId,bool direction, uint32_t
 
     MotorStop(_motorId,Hard_Hiz );
     MotorMove(_motorId,direction,Steps );
-    MotorMoveControlId[_motorId] = AddControlCallback( MotorMoveCallBackFunction,  eTenMillisecond, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
+    MotorMoveControlId[_motorId] = AddControlCallback( MotorMoveCallBackFunction,  /*eTenMillisecond*/20, MotorControlGetnBusyState,(IfTypeMotors*0x100+_motorId), _motorId, 0 );
     return MotorMoveControlId[_motorId];
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 0e9ed5e8e..35f12dd50 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -372,38 +372,44 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
                 }//if dispensers
             }//for brush
         }//for segments
-    for (Dispenser_i=0;Dispenser_i<MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+    if (Configured[Module_IDS])
     {
-        if (DispenserInUse[Dispenser_i] == true)
+        for (Dispenser_i=0;Dispenser_i<MAX_SYSTEM_DISPENSERS;Dispenser_i++)
         {
-            for (Alarm_i = 0;Alarm_i<MAX_SYSTEM_ALARMS;Alarm_i++)
+            if (DispenserInUse[Dispenser_i] == true)
             {
-                if (AlarmItem[Alarm_i].EventType == (EVENT_TYPE__Dispenser1Empty+Dispenser_i))
+                for (Alarm_i = 0;Alarm_i<MAX_SYSTEM_ALARMS;Alarm_i++)
                 {
-                    if (AlarmState[Alarm_i].Status == true)
+                    if (AlarmItem[Alarm_i].EventType == (EVENT_TYPE__Dispenser1Empty+Dispenser_i))
                     {
-                        status = ERROR;
-                        AlarmId = Alarm_i;
-                        break;
+                        if (AlarmState[Alarm_i].Status == true)
+                        {
+                            status = ERROR;
+                            AlarmId = Alarm_i;
+                            break;
+                        }
                     }
                 }
             }
         }
     }
-    for (Heater_i = 0;Heater_i<MAX_HEATERS_NUM;Heater_i++)
+    if (Configured[Module_Heaters])
     {
-        LoadHeaterState((HeaterType)Heater_i,&HeaterState);
-        if (HeaterState.setpoint)//temperature is set - heater active
+        for (Heater_i = 0;Heater_i<MAX_HEATERS_NUM;Heater_i++)
         {
-            for (Alarm_i = 0;Alarm_i<MAX_SYSTEM_ALARMS;Alarm_i++)
+            LoadHeaterState((HeaterType)Heater_i,&HeaterState);
+            if (HeaterState.setpoint)//temperature is set - heater active
             {
-                if (AlarmItem[Alarm_i].EventType == HeaterEventType[Heater_i])
+                for (Alarm_i = 0;Alarm_i<MAX_SYSTEM_ALARMS;Alarm_i++)
                 {
-                    if (AlarmState[Alarm_i].Status == true)
+                    if (AlarmItem[Alarm_i].EventType == HeaterEventType[Heater_i])
                     {
-                        status = ERROR;
-                        AlarmId = Alarm_i;
-                        break;
+                        if (AlarmState[Alarm_i].Status == true)
+                        {
+                            status = ERROR;
+                            AlarmId = Alarm_i;
+                            break;
+                        }
                     }
                 }
             }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index ed1e73bef..001cafeb3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -65,10 +65,11 @@ typedef struct
   uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
   bool ControlActive;
   uint32_t Parameter1;
-  uint32_t IfIndex;
+  uint16_t IfIndex;
+  uint32_t StartTick;
   DataReadCBFunction ControlDataReadPtr;
   ControlCBFunction ControlCallbackPtr;
-  CTRL_TIMING_ENUM ControlTiming;
+  uint16_t ControlTiming;
 }ControlDeviceStruc;
 
 typedef enum
@@ -115,8 +116,8 @@ void ControlInit(void)
     Error_Block eb;
 
     //Mailbox_Params_init(&ControlMsgQ);
-    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
-    TenControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 20, NULL,NULL);
+    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 1, NULL,NULL);
+    TenControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 1, NULL,NULL);
 
     ControlRestart = false;
 
@@ -197,7 +198,7 @@ void ControlStart(void)
  * both these callbacks can be removed. if a new call is arriving, it invalidates the previous one (no dual control or data)
  *
  ***************************************************************************************************************************************************/
-uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
+uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
 {
     assert(Callback);
     assert(DriverfPtr);
@@ -222,6 +223,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     ControlArray[deviceId].ControlDataReadPtr = DriverfPtr;
     ControlArray[deviceId].Parameter1 = Parameter1;
     ControlArray[deviceId].IfIndex = IfIndex;
+    ControlArray[deviceId].StartTick = millisecondCounter;
     GateMutex_leave(gateControlDB, key);
     //LOG_ERROR(deviceId, "Add Callback");
 
@@ -364,6 +366,24 @@ uint32_t ControlLowLoop(uint32_t tick)
     {
         if (ControlArray[Device_i].ControlActive)
         {
+            if (tick == ControlArray[Device_i].StartTick)
+                continue;
+            if (((tick - ControlArray[Device_i].StartTick)%ControlArray[Device_i].ControlTiming)==0) // run the control on exact intervals
+            {
+                ControlBacklog[backlogindex]=Device_i;
+                if ( ++backlogindex >= 999)
+                    backlogindex = 0;
+
+                if(ControlArray[Device_i].ControlDataReadPtr)
+                    ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
+                else
+                    LOG_ERROR (Device_i, "Invalid callback ptr");
+                if(ControlArray[Device_i].ControlCallbackPtr)
+                    ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                else
+                    LOG_ERROR (Device_i, "Invalid callback ptr");
+            }
+            /*
             ControlBacklog[backlogindex]=Device_i;
             if ( ++backlogindex >= 999)
                 backlogindex = 0;
@@ -422,8 +442,10 @@ uint32_t ControlLowLoop(uint32_t tick)
                     memset (&ControlArray[Device_i],0,sizeof(ControlDeviceStruc) );
                     break;
                 default:
+                    //LOG_ERROR(tick, "Default Timing checked");
                     if (tick%ControlArray[Device_i].ControlTiming == 0)
                     {
+                        Report("Default Timing checked",__FILE__,__LINE__,tick,RpWarning,ControlArray[Device_i].ControlTiming,0);
                         if(ControlArray[Device_i].ControlDataReadPtr)
                             ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
                         else
@@ -434,7 +456,7 @@ uint32_t ControlLowLoop(uint32_t tick)
                             LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
-            } //switch
+            } //switch*/
         } //if control active
     }  //for
     //ROM_IntMasterEnable();
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 0aba4b4ba..b3e574d2a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -37,10 +37,11 @@ typedef enum
 void        ControlInit(void);
 void        ControlStop(void);
 void        ControlStart(void);
-uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
+uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
 int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1);
 
 extern Task_Handle Control_Task_Handle;
+extern uint32_t millisecondCounter;
 
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index d92e6a330..2b6d3aaea 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -124,7 +124,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
-    REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency, "Winder_PrepareStage2");
+    REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
 
     status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback);
      //set motor location 0 here
@@ -141,7 +141,7 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewSpeed = 0;
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
-    REPORT_MSG(BusyFlag, "Winder_ScrewAtOffsetCallback");
+    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;
@@ -175,15 +175,20 @@ InternalWinderCfg.segmentoffsetpulses
 numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 */
 char ScrewStr[100];
+//char TempScrewStr[100];
+double WinderReferenceSpeed=0;
+int32_t  TotalWinderSpeed=0;
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
-    uint32_t Steps,i,winderspeed=0;
+    uint32_t Steps,i;
     double temp;
-    uint32_t WinderReferenceSpeed = OriginalMotorSpd_2PPS[WINDER_MOTOR];
-    float screw_horizontal_speed = 0;
-    float RotationsPerSecond;
+    double screw_horizontal_speed = 0;
+    double RotationsPerSecond;
+    int32_t Averagewinderspeed = 0;
 
+    TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
     WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
+    TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
     if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
     {
         WinderMotorSpeedCounter=0;
@@ -215,18 +220,18 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     }
     if (WinderMotorSpeedRollOver)
     {
-        for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++)
+        /*for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++)
         {
-            winderspeed+=WinderMotorSpeed[i];
-        }
-        winderspeed/=MAX_WINDER_SPEED_CALCULATION;
+            TotalWinderSpeed+=WinderMotorSpeed[i];
+        }*/
+        Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
         //REPORT_MSG(winderspeed, "WinderSpeedUpdated");
-        Report("WinderSpeedUpdated",__FILE__,__LINE__,winderspeed,RpWarning,ScrewNumberOfSteps,0);
+        Report("WinderSpeedUpdated",__FILE__,__LINE__,TotalWinderSpeed,RpWarning,Averagewinderspeed,0);
 
-        WinderReferenceSpeed = winderspeed;
+        WinderReferenceSpeed = Averagewinderspeed;
     }
     screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
-    RotationsPerSecond =  WinderReferenceSpeed / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
+    RotationsPerSecond =  WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
     ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
     CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
     temp = SYS_CLK_FREQ;
@@ -234,9 +239,11 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     temp /= ScrewSpeed;
     if (ScrewRunningTime != temp)
     {
-        usnprintf(ScrewStr, 254, "Winder: Steps,Speed, Time, WinderSpeed{ %d, %d ,%d, %d} ",ScrewNumberOfSteps,ScrewSpeed,temp,WinderReferenceSpeed);
+        //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);
+        usnprintf(ScrewStr, 100, "Winder: Steps,Speed, Time, WinderSpeed{ %d, %d ,%d, %d} ",(int)ScrewNumberOfSteps,(int)ScrewSpeed,(int)temp,(int)WinderReferenceSpeed);
         //        Report(logmsg[index],__FILE__,__LINE__,index,RpWarning,index,  Counter[index]);
         //        #warning PID is now only proportional (above)
+        //Report(TempScrewStr,__FILE__,__LINE__,0,RpWarning,0, 0);
         Report(ScrewStr,__FILE__,__LINE__,ScrewCurrentDirection,RpWarning,CalculationDirectionChangeCounter, 0);
         //REPORT_MSG(temp , "new winder speed");
         //Report("new winder speed",__FILE__,__LINE__,temp,RpWarning,ScrewSpeed,0);
@@ -256,9 +263,8 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
 uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
 {
     //JobTicket* JobTicket = JobDetails;
-    float screw_horizontal_speed = 0;
-    float RotationsPerSecond;
-    char Message[80];
+    double screw_horizontal_speed = 0;
+    double RotationsPerSecond;
 	double temp = 0;
     if (dyeingspeed == 0)
     {
@@ -282,13 +288,14 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
 
         //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
-        usnprintf(Message, 80, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
-        //REPORT_MSG(segmentfirst_speed,Message);
-        //Report(Message,__FILE__,__LINE__,Dispenser_i,RpWarning,segmentfirst_speed,0);
-        SendJobProgress(0.0,0,false, Message);
+        usnprintf(ScrewStr, 100, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
+        //REPORT_MSG(segmentfirst_speed,ScrewStr);
+        Report(ScrewStr,__FILE__,__LINE__,RotationsPerSecond,RpWarning,ScrewSpeed,0);
+        SendJobProgress(0.0,0,false, ScrewStr);
 
         CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
         OriginalMotorSpd_2PPS[SCREW_MOTOR] = ScrewSpeed;
+        WinderReferenceSpeed = OriginalMotorSpd_2PPS[WINDER_MOTOR];
         //screw_horizontal_speed = InternalWinderCfg.milimetersperrotation
     //     * 2. determine optimal micro-step setting
     //     * 3. calculate cart travel length from winding parameters
@@ -311,6 +318,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         CalculationDirectionChangeCounter = 1;
         memset (WinderMotorSpeed,0,sizeof(WinderMotorSpeed) );
         WinderMotorSpeedCounter=0;
+        TotalWinderSpeed = 0;
         WinderMotorSpeedRollOver=false;
    }
     PreSegmentReady(Module_Winder,ModuleDone);