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

1 files changed, 27 insertions, 21 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index f9eb69de7..54a47e927 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -28,6 +28,7 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 #include "modules/General/process.h"
+#include "Control/MillisecTask.h"
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
@@ -209,34 +210,41 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
 return OK;
 }
+float SpeedSamples[MAX_CONTROL_SAMPLES] = {0};
+
 uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     //read value is the dancer angle
     int index=MAX_THREAD_MOTORS_NUM;
+    int32_t i, avreageSampleValue = 0;
+    //double tempcalcspeed = 0;
+    uint32_t calculated_speed;
+    float speed = getSensorSpeedData();
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
         return 0xFFFFFFFF;
     }
     index = IfIndex&0xFF;
-/*    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
-        if (ControlIdtoMotorId[i] == deviceID)
-        {
-            index = i;
-            break;
-        }
-    if (index==MAX_THREAD_MOTORS_NUM)
-    {
-        LOG_ERROR (deviceID, "No motor  for device");
-        return 0xFFFFFFFF;
-    }
-    */
+    SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue);
+    MotorSamplePointer[index]++;
+    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
+        MotorSamplePointer[index] = 0;
+    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+        avreageSampleValue += SpeedSamples[i];
+    avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
     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);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
+        calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
+        {
+            CurrentControlledSpeed[index] = calculated_speed;
+            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+        }
     }
  return OK;
 }
@@ -442,6 +450,11 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request)
     return OK;
 }
 bool InitialProcess = false;
+uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
+
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
@@ -515,17 +528,10 @@ bool InitialProcess = false;
                 }
                 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);*/
+//            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,ThreadEmptyCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
             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);
     }
     //testDancersControl();
     PrepareReady(Module_Thread,ModuleDone);