Thread control improved

1 files changed, 29 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 d4a146db9..6ffbf75ae 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -188,7 +188,7 @@ 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)
+/*        if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
         }
@@ -196,11 +196,29 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }
-
+*/
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
     }
  return OK;
 }
+uint32_t _speed;
+uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    int index;
+    if (IfIndex>>8 != IfTypeThread)
+    {
+        LOG_ERROR (IfIndex, "Wrong  Interface type");
+        return 0xFFFFFFFF;
+    }
+    index = IfIndex&0xFF;
+
+    if(MotorControlConfig[index].m_isEnabled )
+    {
+        int MotorId = ThreadMotorIdToMotorId[index];
+        _speed = MotorGetSpeedFromFPGA_Res (MotorId);
+    }
+    return OK;
+}
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -212,6 +230,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int DancerId;
     static int pooler_counter = 0;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
+    uint32_t calculated_speed;
     double NormalizedError;
     if (IfIndex>>8 != IfTypeThread)
     {
@@ -257,19 +276,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }*/
-        uint32_t calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed,  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
         }
     }
-    if (index == POOLER_MOTOR)
+    if (index == FEEDER_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");
+            float error_integered = MotorControlConfig[index].m_calculatedError*1000;
+            //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]*/_speed,(int)error_integered,calculated_speed,ReadValue, "FeederSpeed");
                 pooler_counter = 0;
         }
     }
@@ -332,6 +353,7 @@ bool InitialProcess = false;
                     RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
                 }
                 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);
             }
             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
             {
@@ -392,8 +414,8 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
  //only for testing - when control works, these motors will take their speed from the dryer
     MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR],  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep);
 
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 50, 1);
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 50,1);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 20, 1);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 20,1);
 
 
     // activate control fr all motors