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

4 files changed, 117 insertions, 53 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 97f3811c7..19201c708 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -17,6 +17,7 @@
 
 #include "thread_ex.h"
 
+#define NORMAL_COEF_DIVIDER 100
 typedef struct
 {
     uint32_t startoffsetpulses;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index f8b0efb17..f1c8956ff 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -481,7 +481,7 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
-    Rotations+=0.01;
+    Rotations+=0.03;
 
     return ;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 4454565c1..265c751c6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -106,18 +106,18 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
 #ifdef TEST_LONGER_PID_THREAD
     MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
 #endif
-    for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+    for (i = 0;i < (int)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);
+    temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
 //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
     temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
-    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*2);
+    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2);
     DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index c292f7dcd..25485290b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -137,13 +137,11 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     CurrentRequestedLength = length*100;//Centimetres
     CurrentProcessedLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
-    initialpos = 0xFFFF;
-    Poolerinitialpos = 0xFFFF;
 }
 char Lenstr[150];
 uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
-    uint32_t positionDiff = 0;
+    uint32_t positionDiff = 0,prevprev;
     double length = 0.0;
 
     int index = MAX_THREAD_MOTORS_NUM;
@@ -165,23 +163,33 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         return 0xFFFFFFFF;
     }
     CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
-//    if (CurrentPosition == 0)
-//        return OK; //unusable data
-    if (initialpos == 0xFFFF)
+    if (CurrentPosition != 0)
     {
+        if (initialpos == 0xFFFF)
+        {
+            PreviousPosition = CurrentPosition;
+            initialpos = 0;
+        }
+        prevprev = PreviousPosition;
+        positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
+        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
         PreviousPosition = CurrentPosition;
-        initialpos = 0;
-    }
-    positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
-    //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
-    PreviousPosition = CurrentPosition;
 
-    // total length = (position diff / full cycle) * pulley perimeter
-    //(positionDiff/pulseperround)*((2*PI*motor_Radius)
+        // total length = (position diff / full cycle) * pulley perimeter
+        //(positionDiff/pulseperround)*((2*PI*motor_Radius)
 
-    //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
-    length = (double)(positionDiff)*LengthCalculationMultiplier;
+        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+        length = (double)(positionDiff)*LengthCalculationMultiplier;
+
+        if (length > 1000)
+        {
+            usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev);
+            SendJobProgress(0.0,0,false, Lenstr);
+            Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+
+        }
 
+    }
     CurrentProcessedLength+=length;
 
     static int pooler_counter = 0;
@@ -277,11 +285,11 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     index = IfIndex&0xFF;
     SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue);
     MotorSamplePointer[index]++;
-    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
+    if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode)
         MotorSamplePointer[index] = 0;
-    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+    for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++)
         avreageSampleValue += SpeedSamples[i];
-    avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+    avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode;
     if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
     {
         MotorControlConfig[index].m_mesuredParam = ReadValue;
@@ -315,9 +323,14 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
+
+int controlIndex = 0;
+bool keepdata = true;
+int32_t KeepReadValue = 0;
 //double eNormalizedError[100];
 //int    TranslatedreadValue[100];
-/*#define MAX_THREAD_CONTROL_LOG 100
+#ifdef TEST_PID_THREAD
+#define MAX_THREAD_CONTROL_LOG 100
 double calculatedError[MAX_THREAD_CONTROL_LOG+1];
 double NormError[MAX_THREAD_CONTROL_LOG+1];
 double mIntegral[MAX_THREAD_CONTROL_LOG+1];
@@ -325,10 +338,8 @@ int    MotorId[MAX_THREAD_CONTROL_LOG+1];
 int    readValue[MAX_THREAD_CONTROL_LOG+1];
 int    AveragereadValue[MAX_THREAD_CONTROL_LOG+1];
 int    calculatedspeed[MAX_THREAD_CONTROL_LOG+1];
-int    timestamp[MAX_THREAD_CONTROL_LOG+1];*/
-int controlIndex = 0;
-bool keepdata = true;
-/*int32_t KeepReadValue = 0;
+int    timestamp[MAX_THREAD_CONTROL_LOG+1];
+
 void testDancersControl()
 {
     int mm20,mm10,mm5,mm2,mm1;
@@ -337,8 +348,20 @@ void testDancersControl()
     mm5 = mm20/4;
     mm10 = mm20/2;
     mm1 = mm20/20;
+    char time[150];
+    int len;
     ThreadControlActive = true;
-    SetOriginMotorSpeed(30.0);
+    SetOriginMotorSpeed(50.0);
+    MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = 0;
+    MotorsControl[FEEDER_MOTOR].controloutputtype = 0.001;
+    MotorControlConfig[FEEDER_MOTOR].m_params.dt = 0.001;
+    DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint = 10000;
+    MotorsControl[FEEDER_MOTOR].pvinputfilterfactormode = 1;
+    len = usnprintf(time, 150, "params: speed 50, divider %d p %d * %d i %d * %d Dt*1000 %d Norm Coef %d initial speed %d",NORMAL_COEF_DIVIDER,(int)MotorsControl[FEEDER_MOTOR].proportionalgain,(int)MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue,
+                    (int)MotorsControl[FEEDER_MOTOR].integraltime,(int)MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp,(int)(MotorsControl[FEEDER_MOTOR].controloutputtype*1000),
+                    (int)(NormalizedErrorCoEfficient[FEEDER_MOTOR]*1000000000),OriginalMotorSpd_2PPS[FEEDER_MOTOR]);
+    Report(time,__FILE__,__LINE__,111,RpError,111,0);
+    Task_sleep(100);
     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);
@@ -351,10 +374,12 @@ void testDancersControl()
     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;
-}*/
+}
+#endif
 bool dancerinvalid = false;
 int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0};
-char TMessage[60];
+char TMessage[150];
+//char time[150];
 uint16_t BreakSensorCounter = 0;
 uint16_t BreakSensorLatchCounter = 0;
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
@@ -373,9 +398,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     if (ThreadControlActive == false)
         return OK;
+#ifndef TEST_PID_THREAD
     if (PrepareState == true)
         return OK;
-
+#endif
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
@@ -404,6 +430,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             }
             return OK;
         }
+        KeepReadValue = ReadValue;
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
         if (index == POOLER_MOTOR)
         {
@@ -414,15 +441,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         //TranslatedReadValue = 0;//test
         MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
         MotorSamplePointer[index]++;
-        if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
+        if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode)
             MotorSamplePointer[index] = 0;
 #ifdef TEST_LONGER_PID_THREAD
         else // test: handle tension once in pvinputfilterfactormode milliseconds
             return OK;
 #endif
-        for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
+        for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++)
             avreageSampleValue += MotorSamples[index][i];
-        avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+        avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode;
 
         if (BreakSensorenabled == true)
         {
@@ -466,6 +493,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             keepdata = false;
             usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
+            Report(TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0);
             //JobAbortedByUser = true;
             ThreadControlActive = false;
             //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]);
@@ -501,9 +529,13 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             //KeepNormalizedError = NormalizedError;
         }
-        if ((JobCounter % 1000) == 0)
+        /*if ((JobCounter % 100) == 0)
         {
-            if (JobCounter >= 20000)
+            //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit
+            //{
+              //  Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0);
+            //}
+            /`*if (JobCounter >= 3000)
             {
                 MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue);
                 MotorSpeedSamplePointer[index]++;
@@ -514,28 +546,45 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                 avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES;
                 //Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0);
                 OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue;
-            }
-        }
+            }*`/
+        }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
+#ifndef TEST_PID_THREAD
         if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+#else
+        if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+#endif
         {
-            /*if (keepdata == true)
-            {
-                 calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
-                    MotorId[controlIndex] = index;
-                    readValue[controlIndex] = ReadValue;
-                    AveragereadValue[controlIndex] = avreageSampleValue;
-                    calculatedspeed[controlIndex] = calculated_speed;
-                    NormError[controlIndex]
-                                    = MotorControlConfig[index].m_mesuredParam;
-                    mIntegral[controlIndex] = MotorControlConfig[index].m_integral;
-                    timestamp[controlIndex] = msec_millisecondCounter;
-                 if (controlIndex++>=MAX_THREAD_CONTROL_LOG)
-                     controlIndex = 0;
-            }*/
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
+            if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
+            {
+                Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0);
+            }
+#ifdef TEST_PID_THREAD
+            int len;
+            if ((JobCounter % 2000) == index*100)
+             //if (keepdata == true)
+            {
+                /*calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
+                MotorId[controlIndex] = index;
+                readValue[controlIndex] = ReadValue;
+                AveragereadValue[controlIndex] = avreageSampleValue;
+                calculatedspeed[controlIndex] = calculated_speed;
+                NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam;
+                mIntegral[controlIndex] = MotorControlConfig[index].m_integral;
+                timestamp[controlIndex] = msec_millisecondCounter;*/
+                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
+                                ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
+                                (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
+                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed);
+                Report(TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
+                //Task_sleep(100);
+                //if (controlIndex++>=MAX_THREAD_CONTROL_LOG)
+                //    controlIndex = 0;
+            }
+#endif
         }
 		else
            MotorFailedSample[index]++;
@@ -657,8 +706,10 @@ bool InitialProcess = false;
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                     CurrentControlledSpeed[Motor_i] = 0;
                 }
+#ifndef TEST_PID_THREAD
                 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);
+#endif
             }
             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
             {
@@ -669,7 +720,9 @@ bool InitialProcess = false;
                     CurrentControlledSpeed[Motor_i] = 0;
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
+#ifndef TEST_PID_THREAD
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+#endif
             }
             if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
             {
@@ -680,17 +733,23 @@ bool InitialProcess = false;
                     CurrentControlledSpeed[Motor_i] = 0;
                     ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
+#ifndef TEST_PID_THREAD
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
+#endif
             }
 //            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,TemplateDataReadCBFunction,(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;
     }
-    //testDancersControl();
+#ifdef TEST_PID_THREAD
+    testDancersControl();
+#endif
     PrepareReady(Module_Thread,ModuleDone);
     //set 3 dancers to the profile positions
     InitialProcess = true;
+    initialpos = 0xFFFF;
+    Poolerinitialpos = 0xFFFF;
     return OK;
 }
 
@@ -708,6 +767,8 @@ void SetOriginMotorSpeed(float process_speed)
         //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
         OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
         CurrentControlledSpeed[Motor_i] = (int) motor_speed;
+        Report("Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0);
+
         for (i = 0; i <= MAX_CONTROL_SAMPLES; i++)
             MotorSpeedSamples[Motor_i][i] = motor_speed;
     }
@@ -731,8 +792,10 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
         SetOriginMotorSpeed(process_speed);
         ThreadControlActive = true;
         PrepareState = false;
+#ifndef TEST_PID_THREAD
         // set the new speed in the dryer motor to the speed of the new segment
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
+#endif
 #ifdef HUNDRED_MICROSECONDS_DANCER_READ
      MillisecLogInit();
 #endif