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

1 files changed, 41 insertions, 8 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 0701a2aac..d9aeb503a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -70,6 +70,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
 void ThreadSegmentEnded(void);
 void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
+uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue);
+void SetOriginMotorSpeed(float process_speed);
 
 double KeepNormalizedError = 0;
 bool ThreadControlActive = false;
@@ -248,12 +250,37 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
-double calculatedError[1000];
-int    readValue[1000];
-int    calculatedspeed[1000];
+/*double calculatedError[100];
+double eNormalizedError[100];
+int    readValue[100];
+int    TranslatedreadValue[100];
+int    AveragereadValue[100];
+int    calculatedspeed[100];
 int controlIndex = 0;
 int32_t KeepReadValue = 0;
-
+void testDancersControl()
+{
+    int mm20,mm10,mm5,mm2,mm1;
+    mm20 = (20*DancerStopActivityLimit[FEEDER_MOTOR])/(DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].maximalmovementmm*2);
+    mm2 = mm20/10;
+    mm5 = mm20/4;
+    mm10 = mm20/2;
+    mm1 = mm20/20;
+    ThreadControlActive = true;
+    SetOriginMotorSpeed(30.0);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm5);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm2);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm1);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm1);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm2);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm5);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm10);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm20);
+    ThreadControlActive = false;
+}*/
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -270,6 +297,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     double NormalizedError;
     char Message[60];
 
+    if (ThreadControlActive == false)
+        return OK;
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
@@ -339,18 +368,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             //KeepNormalizedError = NormalizedError;
         }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-        if (index == POOLER_MOTOR)
+        /*if (index == FEEDER_MOTOR)
         {
             if (KeepReadValue != TranslatedReadValue)
             {
+                eNormalizedError[controlIndex] = NormalizedError;
                 calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
-                readValue[controlIndex] = TranslatedReadValue;
+                readValue[controlIndex] = ReadValue;
+                TranslatedreadValue[controlIndex] = TranslatedReadValue;
+                AveragereadValue[controlIndex] = avreageSampleValue;
                 calculatedspeed[controlIndex] = calculated_speed;
                 controlIndex++;
-                if (controlIndex >= 999) controlIndex = 0;
+                if (controlIndex >= 99) controlIndex = 0;
                 KeepReadValue = TranslatedReadValue;
             }
-        }
+        }*/
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
             CurrentControlledSpeed[index] = calculated_speed;
@@ -482,6 +514,7 @@ bool InitialProcess = false;
 //            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);
     //set 3 dancers to the profile positions
     InitialProcess = true;