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

2 files changed, 119 insertions, 60 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index e957b0efc..ea75bffa3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -20,6 +20,8 @@ uint32_t ThreadInitialTestStub();
 uint32_t Winder_Init(void);
 uint32_t Winder_Prepare(void);
 uint32_t Winder_Presegment(void *JobDetails);
+uint32_t Winder_End(void);
+
 
 
 #endif
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index c036f4913..9a7e8cf88 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -21,6 +21,9 @@
 #include "drivers/Danser_SSI/ssi_comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/Heater/Heater.h"
+#include "drivers/Motors/Motor.h"
+#include "modules/heaters/heaters.h"
+
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
 //by recieved esign flow of the user from the UI
@@ -31,6 +34,8 @@ uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
 HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
+uint32_t    SpeedControlId=0xFF;
+
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 
 typedef struct
@@ -78,7 +83,8 @@ void ThreadDistanceToSpoolEnded(void);
 uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
 {
     uint32_t Time_Pass;
-  #define   MAX_COUNTER 0x3FFF  //14 bits
+//  #define   MAX_COUNTER 0x3FFF  //14 bits
+    #define   MAX_COUNTER 0x3FFFFF  //22 bits
 
 
   if (Current_Read < Previous_Read)
@@ -97,6 +103,64 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
  *
  * **************************************************************************************/
 
+void ThreadUpdateProcessLength (double length, void *Funcptr)
+{
+    CurrentRequestedLength = length*100;//Centimetres
+    CurrentProcessedLength = 0;
+    PreviousPosition = 0;
+    CurrentPosition = 0;
+    ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
+}
+uint32_t MotorSentData[1000] = {0};
+uint32_t PosDif[1000] = {0};
+uint32_t tick[1000] = {0};
+uint32_t initialpos = 0xFFFF;
+
+
+int MotorDataIndex = 0;
+
+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]);
+    if (initialpos == 0xFFFF)
+        initialpos = CurrentPosition;
+    positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
+    positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+    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;
+    PosDif[MotorDataIndex] = positionDiff;
+    MotorSentData[MotorDataIndex] = length;
+    tick[MotorDataIndex] = UsersysTickGet();
+    MotorDataIndex+=1;
+    if (MotorDataIndex == 999) MotorDataIndex = 0;
+    if (CurrentProcessedLength>=CurrentRequestedLength )
+    {
+        // segment/intersegment/distance to spool finished
+        if (ProcessedLengthFuncPtr)
+            ProcessedLengthFuncPtr();
+    }
+return OK;
+}
 uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
@@ -137,46 +201,6 @@ 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
@@ -186,6 +210,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     int DancerId;
+    static int pooler_counter = 0;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
     double NormalizedError;
     if (IfIndex>>8 != IfTypeThread)
@@ -213,6 +238,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
         if (index == POOLER_MOTOR)
             TranslatedReadValue = (-1*TranslatedReadValue);
+        //TranslatedReadValue = 0;//test
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
@@ -238,6 +264,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed,  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
         }
     }
+    if (index == POOLER_MOTOR)
+    {
+        pooler_counter++;
+        if (pooler_counter>=1000)
+        {
+            HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed");
+                pooler_counter = 0;
+        }
+    }
 
  return OK;
 }
@@ -245,13 +280,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 //********************************************************************************************************************
 
 //********************************************************************************************************************
-/*#ifdef DEBUG_TEST_FUNCTIONS
-uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2)
-{
-
-    return (rand() % (103 + 1 + 103) - 103);
-}
-#endif*/
 uint32_t ThreadInitialTestStub(HardwareMotor * request)
 {
 
@@ -276,7 +304,7 @@ bool InitialProcess = false;
             MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
             MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
             MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
-            MotorControlConfig[Motor_i].m_params.epsilon = 0.01;
+            MotorControlConfig[Motor_i].m_params.epsilon = 0.1;
             MotorControlConfig[Motor_i].m_params.dt = 50;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
@@ -287,29 +315,52 @@ bool InitialProcess = false;
             MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
 
             MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
-    #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(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+            {
+                if (SpeedControlId != 0xFF)
+                {
+                    RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+                }
+                SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+                SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+            }
             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
+            {
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                }
+                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(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 n//ot be controlled
+            {
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(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
+            }
+            if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
+            {
+                if (ControlIdtoMotorId[Motor_i] != 0xFF)
+                {
+                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,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;
                 AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
             else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
                 AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
-    #else
             if (Motor_i == 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
                 continue;
                 //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
-            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
-                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
-    #endif
+//            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
+//                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
     }
-    Winder_Prepare();
     PrepareReady(Module_Thread,ModuleDone);
     //set 3 dancers to the profile positions
     InitialProcess = true;
@@ -323,9 +374,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     JobTicket* JobTicket = JobDetails;
     int Motor_i, HW_Motor_Id;
 
-    int process_speed = JobTicket->processparameters->dyeingspeed;
-
-    process_speed = 50; //debug
+    float process_speed = JobTicket->processparameters->dyeingspeed;
 
     for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
     {
@@ -387,8 +436,16 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
  uint32_t ThreadEndState(void *JobDetails)
 {
      int Motor_i;
+     if (SpeedControlId != 0xFF)
+     {
+         RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+     }
      for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
      {
+         if (ControlIdtoMotorId[Motor_i] != 0xFF)
+         {
+             RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+         }
         StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
      StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);