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

1 files changed, 62 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index be6644067..fb8fa4711 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -35,6 +35,7 @@
 #include "modules/ids/ids_ex.h"
 #include "Modules/AlarmHandling/AlarmHandling.h"
 #include "Control/MillisecTask.h"
+#include "drivers/Flash_ram/MCU_E2Prom.h"
 
 ////////////////////////////////State machine operation////////////////////////////////////
 //the state machine operation is used to operate in runtime correct profile flow execution
@@ -789,11 +790,61 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
 
     return OK;
 }
+uint32_t ThreadPrepare_Tension (int DancerId, double tension)
+{
+    int current, request = (int)tension,movement;
+    int HW_Motor_Id;
+    bool direction;
+    uint32_t status = OK, address = 0;
+    switch (DancerId)
+    {
+    case HARDWARE_DANCER_TYPE__LeftDancer:
+        address = EEPROM_WINDER_TENSION_POSITION;
+        HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER1;
+        break;
+    case HARDWARE_DANCER_TYPE__MiddleDancer:
+        if (Is_PP_Machine() == false) //LP machine - old LTFU
+            return OK;
+        address = EEPROM_PULLER_TENSION_POSITION;
+        HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2;
+        break;
+    case HARDWARE_DANCER_TYPE__RightDancer:
+        return ERROR;
+    default:
+        return ERROR;
+    }
+    status |= MCU_E2PromRead(address,&current);
+    if ((status!= OK )||(current == 0)||(current == 0xFFFF))
+        return status;
+    if (abs(current - request)<100)
+        return status;
+    else
+    {
+        if (current < request) //go down
+        {
+            direction = MotorsCfg[HW_Motor_Id].directionthreadwize;
+            movement = request - current;
+        }
+        else
+        {
+            direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize;
+            movement = current - request;
+        }
+        MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), NULL,20000);
+        status |= MCU_E2PromProgram(address,request);
+    }
+    usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d",
+              DancerId,request,current,movement,direction,HW_Motor_Id,address);
+    ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0);
+
+    return status;
+}
 //********************************************************************************************************************
  uint32_t ThreadPrepareState(void *JobDetails)
 {
     int Motor_i,i, HW_Motor_Id, Pid_Id;
     JobTicket* JobTicket = JobDetails;
+    uint32_t status = OK;
     CurrentSegmentId = 0;
 
     JobCounter = 0;
@@ -810,11 +861,22 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
     AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false);
     AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,false);
 
+//    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
+//      double feedertension = 0;
+//    double pullertension = 0;
+//    double windertension = 0;
 
     EnableLubrication = JobTicket->enablelubrication;
     EnableIntersegment = JobTicket->enableintersegment;
     IntersegmentLength = JobTicket->intersegmentlength;
 
+    status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension);
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,status,RpFatalError,(int)windertension,0);
+    status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension);
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,status,RpFatalError,(int)pullertension,0);
+    status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension);
+    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,status,RpFatalError,(int)feedertension,0);
+
     FirstCalcInJob = true;
     if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
     {