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

1 files changed, 50 insertions, 9 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index fb7a7651a..cc3e317fe 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -57,6 +57,7 @@ uint32_t PreviousPosition = 0, CurrentPosition = 0;
 double totalLength = 0.0;
 double CurrentRequestedLength = 0.0;
 double CurrentProcessedLength = 0.0;
+double LengthCalculationMultiplier;
 
 typedef  void (* ProcessedLengthFunc)(void);
 ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
@@ -112,7 +113,7 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     totalLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
 }
-uint32_t MotorSentData[1000] = {0};
+double MotorSentData[1000] = {0};
 uint32_t PosDif[1000] = {0};
 uint32_t tick[1000] = {0};
 uint32_t initialpos = 0xFFFF;
@@ -131,12 +132,16 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         return 0xFFFFFFFF;
     }
     index = IfIndex&0xFF;
+    if (CurrentRequestedLength == 0.0)
+        return;
     if (index != FEEDER_MOTOR)
     {
         LOG_ERROR (IfIndex, "Wrong Motor");
         return 0xFFFFFFFF;
     }
-    CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
+    CurrentPosition = MotorGetPositionFromFPGA_Res(ThreadMotorIdToMotorId[index]);
+    if (CurrentPosition == 0)
+        return; //unusable data
     if (initialpos == 0xFFFF)
         initialpos = CurrentPosition;
     positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
@@ -146,7 +151,8 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     // total length = (position diff / full cycle) * pulley perimeter
     //(positionDiff/pulseperround)*((2*PI*motor_Radius)
 
-    length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius);
+    //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
+    length = (double)(positionDiff)*LengthCalculationMultiplier;
     if (length > 0.1)
     {
         totalLength+=length;
@@ -155,11 +161,33 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 #warning control disabled
         CurrentProcessedLength+=length;
 #warning control disabled
-    PosDif[MotorDataIndex] = positionDiff;
+    PosDif[MotorDataIndex] = CurrentPosition;
+    //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
     tick[MotorDataIndex] = UsersysTickGet();
     MotorDataIndex+=1;
     if (MotorDataIndex == 999) MotorDataIndex = 0;
+    static int pooler_counter = 0;
+    pooler_counter++;
+    if (pooler_counter>=100)
+    {
+        /*{
+"HeaterGroupId": 0,
+"Zone1Temp": 80,
+"Zone2Temp": 2641,
+"Heater1Active": false,
+"Heater2Active": false,
+"Heater1Percentage": 3,
+"Heater2Percentage": 4000,
+"InfoMessage": "Standard DC"
+}  void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message)
+
+}*/
+        //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed");
+
+        HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/length,positionDiff/*(int)error_integered*/,CurrentProcessedLength,CurrentRequestedLength, "FeederLength");
+            pooler_counter = 0;
+    }
     if (CurrentProcessedLength>=CurrentRequestedLength )
     {
         // segment/intersegment/distance to spool finished
@@ -355,15 +383,18 @@ bool InitialProcess = false;
             MotorControlConfig[Motor_i].m_preError = 0;
             MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
 
-            MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
+            if (MotorsCfg[HW_Motor_Id].has_directionthreadwize)
+                MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
 
             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
             {
                 if (SpeedControlId != 0xFF)
                 {
                     RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+                    SpeedControlId = 0xFF;
                 }
                 SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+                LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
                 SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[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
@@ -371,6 +402,8 @@ bool InitialProcess = false;
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
                 {
                     RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                    ControlIdtoMotorId[Motor_i] = 0xFF;
+                    CurrentControlledSpeed[Motor_i] = 0;
                 }
                 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);
@@ -380,6 +413,8 @@ bool InitialProcess = false;
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
                 {
                     RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                    CurrentControlledSpeed[Motor_i] = 0;
+                    ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             }
@@ -388,6 +423,8 @@ bool InitialProcess = false;
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
                 {
                     RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+                    CurrentControlledSpeed[Motor_i] = 0;
+                    ControlIdtoMotorId[Motor_i] = 0xFF;
                 }
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
             }
@@ -442,10 +479,14 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
  //only for testing - when control works, these motors will take their speed from the dryer
     //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR]);
 
-#warning rocker disabled
-//    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 50);
-//    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 50);
-#warning rocker disabled
+//#warning rocker disabled
+    if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize)
+        MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 10);
+    if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize)
+        MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10);
+//#warning rocker disabled
 
 
     // activate control fr all motors