state machine changes, memory allocation bug

1 files changed, 117 insertions, 7 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index bb866d608..2af3a699d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -6,6 +6,7 @@
  **************************************************************************************************************************/
 #include "include.h"
 #include "thread.h"
+#include "thread_ex.h"
 #include "../control/control.h"
 #include "../control/pidalgo.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
@@ -44,10 +45,55 @@ typedef struct
 
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
+
+uint32_t PreviousPosition = 0, CurrentPosition = 0;
+double totalLength = 0.0;
+double CurrentRequestedLength = 0.0;
+double CurrentProcessedLength = 0.0;
+
+typedef  void (* ProcessedLengthFunc)(void);
+ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
+// segment/intersegment/distance to spool finished
+void ThreadSegmentEnded(void);
+void ThreadInterSegmentEnded(void);
+void ThreadDistanceToSpoolEnded(void);
 ////////////////////////Slow Motor State////////////////////////////////////
-uint32_t ThreadPreSegmentState(void *JobDetails);
+//uint32_t ThreadPreSegmentState(void *JobDetails);
 
 ////////////////////////////////////////////////////////////////////////////
+/********************************************************************
+*
+*    Name        : GTIME_Delta_Time_Pass
+*
+*    Parameters  : start_time.
+*
+*    Return      : time pass from start time
+*
+*    Description :
+*
+*********************************************************************/
+
+uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
+{
+    uint32_t Time_Pass;
+  #define   MAX_COUNTER 0x3FFF  //14 bits
+
+
+  if (Current_Read < Previous_Read)
+    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
+  else
+    Time_Pass = Current_Read - Previous_Read;
+
+  return (Time_Pass);
+}
+/*****************************************************************************************
+ *
+ *
+ *
+ *
+ *
+ *
+ * **************************************************************************************/
 
 uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
@@ -89,6 +135,46 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
  return OK;
 }
+void ThreadUpdateProcessLength (double length, void *Funcptr)
+{
+    CurrentRequestedLength = length;
+    CurrentProcessedLength = 0;
+    ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
+}
+uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    uint32_t positionDiff = 0;
+    double length = 0.0;
+    int index = MAX_THREAD_MOTORS_NUM;
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+    if (index != FEEDER_MOTOR)
+    {
+        LOG_ERROR (IfIndex, "Wrong Motor");
+        return 0xFFFFFFFF;
+    }
+    CurrentPosition = MotorGetPositionFromFPGA_Res(ThreadMotorIdToMotorId[index]);
+    positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
+    PreviousPosition = CurrentPosition;
+
+    // total length = (position diff / full cycle) * pulley perimeter
+    //(positionDiff/pulseperround)*((2*PI*motor_Radius)
+
+    length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius);
+    totalLength+=length;
+    CurrentProcessedLength+=length;
+    if (CurrentProcessedLength>=CurrentRequestedLength )
+    {
+        // segment/intersegment/distance to spool finished
+        if (ProcessedLengthFuncPtr)
+            ProcessedLengthFuncPtr();
+    }
+return OK;
+}
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -97,7 +183,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
-    int Pid_Id,DancerId;
+    int DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
     double NormalizedError;
     if (IfIndex>>8 != IfTypeThread)
@@ -121,7 +207,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     if(MotorControlConfig[index].m_isEnabled )
     {
-        Pid_Id = ThreadMotorIdToControlId[index];
         DancerId = ThreadMotorIdToDancerId[index];
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
@@ -170,6 +255,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
      ThreadPreSegmentState(request);
     return OK;
 }
+bool InitialProcess = false;
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
@@ -198,7 +284,9 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     #ifdef DEBUG_TEST_FUNCTIONS
 
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eOneMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[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 not be controlled
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eHundredMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
     //            continue;
@@ -215,7 +303,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     }
     Winder_Prepare();
     //set 3 dancers to the profile positions
-
+    InitialProcess = true;
     return OK;
 }
 
@@ -244,18 +332,40 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     MotorSetSpeed(MOTOR_RLOADING, 1, 1);
     MotorSetSpeed(MOTOR_LLOADING, 1,1);
 
+
     // activate control fr all motors
     //set speed for both rocker motors
     //wait for all motors to get to the required speed (set the target speed for the control to check)
     //call the job state machine when the thread system is ready
-    PreSegmentReady(Module_Thread,OK);
+    if ((InitialProcess==false) && JobTicket->enableintersegment == true)
+    {
+        ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded);
+    }
+    else
+    {
+        PreSegmentReady(Module_Thread,ModuleDone);
+        InitialProcess = false;
+    }
 
     return OK;
 }
+void ThreadInterSegmentEnded(void)
+{
+    PreSegmentReady(Module_Thread,ModuleDone);
+}
+void ThreadSegmentEnded(void)
+{
+    SegmentReady(Module_Thread,ModuleDone);
+}
+void ThreadDistanceToSpoolEnded(void)
+{
 
+}
 //********************************************************************************************************************
- uint32_t ThreadSegmentState(void *JobDetails)
+uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
 {
+    JobTicket* JobTicket = JobDetails;
+    ThreadUpdateProcessLength (JobTicket->segments[SegmentId]->length,(void *)ThreadSegmentEnded);
     return OK;
 }