First print and some winder improvements

1 files changed, 12 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 92bfa9c92..3ebb0304b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -22,6 +22,7 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/Heater/Heater.h"
 #include "drivers/Motors/Motor.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "modules/heaters/heaters.h"
 
 ////////////////////////////////State machine operation////////////////////////////////////
@@ -66,6 +67,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
 void ThreadSegmentEnded(void);
 void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
+
+double KeepNormalizedError = 0;
 ////////////////////////Slow Motor State////////////////////////////////////
 //uint32_t ThreadPreSegmentState(void *JobDetails);
 
@@ -176,7 +179,9 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     pooler_counter++;
     if (pooler_counter%10 == 0)
     {
-        SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
+        //SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
+        SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false);
+
     }
     if (pooler_counter>=100)
     {
@@ -280,7 +285,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     {
         DancerId = ThreadMotorIdToDancerId[index];
         if (ReadValue < 10)
-            return;
+            return OK;
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
         if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite
             TranslatedReadValue = (-1*TranslatedReadValue);
@@ -295,8 +300,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
-        if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+        if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
             MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+        else
+            KeepNormalizedError = NormalizedError;
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
@@ -324,7 +331,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 }*/
             //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]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed");
+            HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed");
                 pooler_counter = 0;
         }
     }
@@ -475,6 +482,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10);
 //#warning rocker disabled
 
+//    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO
 
     // activate control fr all motors
     //set speed for both rocker motors