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

1 files changed, 56 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 89ff63363..6e4624723 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -11,6 +11,7 @@
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
 #include "drivers/Motors/Motor.h"
+#include "Drivers/Danser_SSI/ssi_comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/Heater/Heater.h"
 ////////////////////////////////State machine operation////////////////////////////////////
@@ -28,6 +29,7 @@ typedef enum
 
 
 uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
+uint32_t ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
 
 /********************************************************************************************
 * functions describes motor operation flow and movement state during profile execution
@@ -128,8 +130,43 @@ uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
 static PrintingState_t gPrintingState;
 ////////////////////////////////////////////////////////////////////////////
 
+uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
+{
+    //read value is the dancer angle
+    int i,index=MAX_THREAD_MOTORS_NUM;
+    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
+        if (DeviceId2Motor[i] == deviceID)
+        {
+            index = i;
+            break;
+        }
+    if (index==MAX_THREAD_MOTORS_NUM)
+    {
+        LOG_ERROR (deviceID, "No motor  for device");
+        return 0xFFFFFFFF;
+    }
+    if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
+    {
+        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;
+}
 uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
 {
+    //read value is the dancer angle
     int i,index=MAX_THREAD_MOTORS_NUM;
     for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
         if (DeviceId2Motor[i] == deviceID)
@@ -173,6 +210,13 @@ static ReturnCode EntryState(void *JobDetails)
 }
 
 //********************************************************************************************************************
+#ifdef DEBUG_TEST_FUNCTIONS
+uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2)
+{
+
+}
+#endif
+//********************************************************************************************************************
 static ReturnCode PrepareState(void *JobDetails)
 {
     int Motor_i;
@@ -192,7 +236,18 @@ static ReturnCode PrepareState(void *JobDetails)
         MotorControlConfig[Motor_i].m_mesuredParam = 0;
         MotorControlConfig[Motor_i].m_preError = 0;
         MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
-        AddControlCallback(ThreadControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,Motor_i,0);
+#ifdef DEBUG_TEST_FUNCTIONS
+        if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0);
+        else
+            AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Debug_Get_Dancer_Read,ThreadMotorIdToDancerId[Motor_i],0);
+#else
+        if (Motor_i == DRYER_MOTOR) // 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,ThreadMotorIdToMotorId[Motor_i],0);
+        else
+            AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],0);
+#endif
     }
 
     //set 3 dancers to the profile positions