Calculate Job Process, report job progress, Abort Job (quick and Dirty)

1 files changed, 22 insertions, 34 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 71a5e0dac..92bfa9c92 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -59,6 +59,7 @@ double CurrentRequestedLength = 0.0;
 double CurrentProcessedLength = 0.0;
 double LengthCalculationMultiplier;
 
+int CurrentSegmentId = 0;
 typedef  void (* ProcessedLengthFunc)(void);
 ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
 // segment/intersegment/distance to spool finished
@@ -103,6 +104,7 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
  *
  *
  * **************************************************************************************/
+uint32_t initialpos = 0xFFFF;
 
 void ThreadUpdateProcessLength (double length, void *Funcptr)
 {
@@ -112,11 +114,11 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     //CurrentPosition = 0;
     totalLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
+    initialpos = 0xFFFF;
 }
 double MotorSentData[1000] = {0};
 uint32_t PosDif[1000] = {0};
 uint32_t tick[1000] = {0};
-uint32_t initialpos = 0xFFFF;
 
 
 int MotorDataIndex = 0;
@@ -143,7 +145,10 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     if (CurrentPosition == 0)
         return OK; //unusable data
     if (initialpos == 0xFFFF)
-        initialpos = CurrentPosition;
+    {
+        PreviousPosition = CurrentPosition;
+        initialpos = 0;
+    }
     positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
     //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
     PreviousPosition = CurrentPosition;
@@ -169,20 +174,12 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     if (MotorDataIndex == 999) MotorDataIndex = 0;
     static int pooler_counter = 0;
     pooler_counter++;
+    if (pooler_counter%10 == 0)
+    {
+        SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
+    }
     if (pooler_counter>=100)
     {
-        /*{
-"HeaterGroupId": 0,
-"Zone1Temp": 80,
-"Zone2Temp": 2641,
-"Heater1Active": false,
-"Heater2Active": false,
-"Heater1Percentage": 3,
-"Heater2Percentage": 4000,
-"InfoMessage": "Standard DC"
-}  void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message)
-
-}*/
         //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed");
 
         HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/length,positionDiff/*(int)error_integered*/,CurrentProcessedLength,CurrentRequestedLength, "FeederLength");
@@ -190,6 +187,8 @@ 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();
@@ -223,15 +222,6 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         MotorControlConfig[index].m_mesuredParam = ReadValue;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
-/*        if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
-        {
-            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
-        }
-        if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
-        {
-            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
-        }
-*/
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
  return OK;
@@ -289,8 +279,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     if(MotorControlConfig[index].m_isEnabled )
     {
         DancerId = ThreadMotorIdToDancerId[index];
+        if (ReadValue < 10)
+            return;
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
-        if (index == POOLER_MOTOR)
+        if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
         //TranslatedReadValue = 0;//test
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
@@ -303,14 +295,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
-        /*if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
-        {
-            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
-        }
-        if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
-        {
-            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
-        }*/
+        if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+            MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
@@ -363,6 +349,7 @@ bool InitialProcess = false;
  uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i, HW_Motor_Id, Pid_Id;
+    CurrentSegmentId = 0;
     //start thread control for all motors
     for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
@@ -393,7 +380,7 @@ bool InitialProcess = false;
                     RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
                     SpeedControlId = 0xFF;
                 }
-                SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+                //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);
             }
@@ -406,7 +393,7 @@ bool InitialProcess = false;
                     CurrentControlledSpeed[Motor_i] = 0;
                 }
                 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);
+                //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(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 n//ot be controlled
             {
@@ -521,6 +508,7 @@ void ThreadDistanceToSpoolEnded(void)
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
 {
     JobTicket* JobTicket = JobDetails;
+    CurrentSegmentId = SegmentId;
     ThreadUpdateProcessLength (JobTicket->segments[SegmentId]->length,(void *)ThreadSegmentEnded);
     return OK;
 }