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

3 files changed, 25 insertions, 16 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 0bda4964b..db3bfc46e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -43,7 +43,7 @@ typedef enum threadMotorsEnum
 #define MAX_SYSTEM_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 
 extern TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM];
-extern DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
+extern HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM];
 extern HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM];
 
 extern HardwareMotor                MotorsCfg[NUM_OF_MOTORS];
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 3fb78d2f0..e957b0efc 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -2,13 +2,13 @@
 #ifndef MODULES_THREAD_THREAD_EX_H_
 #define MODULES_THREAD_THREAD_EX_H_
 
-typedef enum
-{
-    WINDER_DANCER,
-    POOLER_DANCER,
-    FEEDER_DANCER,
-    NUM_OF_DANCERS
-} DANCER_ENUM;
+//typedef enum
+//{
+#define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer
+#define POOLER_DANCER HARDWARE_DANCER_TYPE__MiddleDancer
+#define FEEDER_DANCER HARDWARE_DANCER_TYPE__RightDancer
+#define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
+//} DANCER_ENUM;
 
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 92939dfe4..67f2227d2 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 "../control/pidalgo.h"
 #include "PMR/Hardware/HardwareMotor.pb-c.h"
 #include "PMR/Hardware/HardwareMotorType.pb-c.h"
+#include "PMR/Hardware/HardwareDancerType.pb-c.h"
 #include "PMR/Printing/JobSegment.pb-c.h"
 #include "PMR/Printing/JobTicket.pb-c.h"
 
@@ -25,9 +26,10 @@
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 
+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};
-DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
+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};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 
@@ -209,6 +211,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     {
         DancerId = ThreadMotorIdToDancerId[index];
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
+        if (index == POOLER_MOTOR)
+            TranslatedReadValue = (-1*TranslatedReadValue);
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
         if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
@@ -227,9 +231,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
         }*/
-
-        MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index],  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
-        //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
+        uint32_t 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);
+        }
     }
 
  return OK;
@@ -270,7 +277,7 @@ bool InitialProcess = false;
             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.dt = eOneMillisecond;
+            MotorControlConfig[Motor_i].m_params.dt = 50;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
             MotorControlConfig[Motor_i].m_isEnabled = true;
@@ -281,11 +288,13 @@ bool InitialProcess = false;
 
             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 (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, eOneMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                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
-                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eHundredMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+                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;