merge

3 files changed, 37 insertions, 12 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index c7dd4f7ae..effa8b2c7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -8,6 +8,8 @@
 #include "thread.h"
 
 #include "drivers/Motors/Motor.h"
+#include <drivers/FPGA/FPGA_INTERRUPTS/fpga_interrupts.h>
+
 #include "StateMachines/Printing/PrintingSTM.h"
 #include "Modules/Control/Control.h"
 #include "Modules/Control/MillisecTask.h"
@@ -21,6 +23,8 @@
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
+
+
 bool Winder_ScrewHoming = false;
 //bool Winder_Active = false;
 
@@ -128,7 +132,7 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewNumberOfSteps = 0;
     DirectionChangeCounter = 0;
     REPORT_MSG(BusyFlag, "Winder_ScrewAtOffsetCallback");
-
+    //Screw_Interrupt(true,ScrewFreeInterrupt);
     PrepareReady(Module_Winder, ModuleDone);
     return OK;
 }
@@ -338,6 +342,7 @@ uint32_t Winder_End(void)
         RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
     CurrentControlledSpeed[SCREW_MOTOR] = 0;
     pend = MillisecFlushMsgQ(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    //Screw_Interrupt(false,NULL);
     return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 2961bd105..c5e3edc85 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -27,6 +27,7 @@ uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadConfigBreakSensor(void *request);
 
 uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
+double ThreadGetMotorCalculatedError(int DancerId);
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index bbe537263..ec2ba407a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -70,10 +70,12 @@ double TotalProcessedLength = 0.0;
 double LengthCalculationMultiplier;
 
 uint32_t PoolerPreviousPosition = 0, PoolerCurrentPosition = 0;
-double PoolerCurrentProcessedLength = 0.0;
 double PoolerTotalProcessedLength = 0.0;
 double PoolerLengthCalculationMultiplier;
 
+double TempPoolerTotalProcessedLength = 0.0;
+double TempTotalProcessedLength = 0.0;
+
 bool PrepareState = false;
 int CurrentSegmentId = 0;
 typedef  void (* ProcessedLengthFunc)(void);
@@ -132,17 +134,16 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
 {
     CurrentRequestedLength = length*100;//Centimetres
     CurrentProcessedLength = 0;
-    PoolerCurrentProcessedLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
     initialpos = 0xFFFF;
     Poolerinitialpos = 0xFFFF;
 }
-
 char Lenstr[150];
 uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     uint32_t positionDiff = 0;
     double length = 0.0;
+
     int index = MAX_THREAD_MOTORS_NUM;
  //   if (ThreadControlActive == false)
  //       return OK;
@@ -184,6 +185,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     static int pooler_counter = 0;
     pooler_counter++;
     TotalProcessedLength+= (length/100);
+    TempTotalProcessedLength = TotalProcessedLength;
     if (pooler_counter%10 == 0)
     {
         if (PrepareState == true)
@@ -247,7 +249,8 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     PoolerPreviousPosition = PoolerCurrentPosition;
 
     length = (double)(positionDiff)*PoolerLengthCalculationMultiplier;
-    PoolerCurrentProcessedLength+=length;
+    PoolerTotalProcessedLength+= (length/100);
+    TempPoolerTotalProcessedLength = PoolerTotalProcessedLength;
 
 return OK;
 }
@@ -439,7 +442,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
             //JobAbortedByUser = true;
             ThreadControlActive = false;
-            JobEndReason = JOB_WINDER_DANCER_FAIL+index;
+            MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]);
+            JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId;
             SendJobProgress(0.0,0,false, Message);
             //EndState(CurrentJob,Message );
             SegmentReady(Module_Thread,ModuleFail);
@@ -490,6 +494,21 @@ uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId)
 {
     return  CurrentControlledSpeed[MotorId];
 }
+//********************************************************************************************************************
+double ThreadGetMotorCalculatedError(int DancerId)
+{
+    switch (DancerId)
+    {
+        case FEEDER_DANCER:
+            return  (double)MotorControlConfig[FEEDER_MOTOR].m_calculatedError;
+        case POOLER_DANCER:
+            return  (double)MotorControlConfig[POOLER_MOTOR].m_calculatedError;
+        case WINDER_DANCER:
+            return  (double)MotorControlConfig[WINDER_MOTOR].m_calculatedError;
+
+    }
+    return 0;
+}
 
 //********************************************************************************************************************
 uint32_t ThreadInitialTestStub(HardwareMotor * request)
@@ -561,7 +580,7 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                     PoolerSpeedControlId = 0xFF;
                 }
                 //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+                PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
                 PoolerSpeedControlId = AddControlCallback(PoolerThreadLengthCBFunction, 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
@@ -648,8 +667,8 @@ 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
-    if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0)
+#warning rocker disabled
+/*    if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0)
     {
         MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize);
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 2);
@@ -658,8 +677,8 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
     {
         MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
         MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 2);
-    }
- //#warning rocker disabled
+    }*/
+ #warning rocker disabled
 
 //    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO
 
@@ -719,7 +738,7 @@ char Endstr[150];
      int Motor_i;
      ThreadControlActive = false;
 
-     usnprintf(Endstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
      SendJobProgress(0.0,0,false, Endstr);
      Report(Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);