1 files changed, 7 insertions, 2 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index a1e91f6fc..4454565c1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -2,6 +2,7 @@
  **************************************************************************************************************************/
 
 #include <DataDef.h>
+#include <Drivers/FPGA/Motors_Driver/PowerSTEP01.h>
 #include "include.h"
 
 #include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h"
@@ -17,12 +18,13 @@
 #include "drivers/Flash_Memory/fatfs/ff.h"
 #include "drivers/SSI_Comm/Dancer/Dancer.h"
 
-#include "drivers/FPGA/Moters_Driver/PowerSTEP01.h"
-
 #include "thread.h"
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
+int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
+
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
@@ -105,7 +107,10 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
 #endif
     for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+    {
         MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
+        MotorSpeedSamples[Motor_i][i] = 0;
+    }
     NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
     temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);