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

3 files changed, 74 insertions, 31 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index e564e52d0..a119b0b16 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -145,37 +145,41 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
     return PreSegmentReady(Module_Winder,ModuleDone);
 }
 
-uint32_t Winder_Presegment(void *JobDetails)
+uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
 {
     JobTicket* JobTicket = JobDetails;
     int process_speed = JobTicket->processparameters->dyeingspeed;
     float screw_speed = 0;
     float RotationsPerSecond;
 
-    ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
+    if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job)
+    {
+        ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
+
+        //     * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec)
+        //     * calculate
+        //     * 1. calculate speed according to JobTicket->processparameters->dyeingspeed
+        //     * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
+        screw_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
+        // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
+        RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI);
+        // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
+        ScrewSpeed = screw_speed*RotationsPerSecond;
 
-    //     * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec)
-    //     * calculate
-    //     * 1. calculate speed according to JobTicket->processparameters->dyeingspeed
-    //     * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
-    screw_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
-    // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
-    RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI);
-    // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
-    ScrewSpeed = screw_speed*RotationsPerSecond;
+        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        //screw_speed = InternalWinderCfg.milimetersperrotation
+    //     * 2. determine optimal micro-step setting
+    //     * 3. calculate cart travel length from winding parameters
+    //     * 4. start move of travel length
+    //     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
+        ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
+        ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
 
-    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
-    //screw_speed = InternalWinderCfg.milimetersperrotation
-//     * 2. determine optimal micro-step setting
-//     * 3. calculate cart travel length from winding parameters
-//     * 4. start move of travel length
-//     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
-    ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
-    MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
-    ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
+        // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
+        //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
 
-    // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
-    //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
+    }
     PreSegmentReady(Module_Winder,ModuleDone);
 
     return OK;
@@ -195,7 +199,7 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void)
     uint32_t status;
     if  (Winder_ScrewHoming)
     {
-        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); //stop ASAP
     }
     status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index b7401c77f..4a7f1144c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -23,7 +23,7 @@ uint32_t ThreadInitialTestStub();
 //uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 uint32_t Winder_Init(void);
 uint32_t Winder_Prepare(void);
-uint32_t Winder_Presegment(void *JobDetails);
+uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId);
 uint32_t Winder_End(void);
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 6740d6304..1ae1145fb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -72,6 +72,7 @@ void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
 
 double KeepNormalizedError = 0;
+bool ThreadControlActive = false;
 ////////////////////////Slow Motor State////////////////////////////////////
 //uint32_t ThreadPreSegmentState(void *JobDetails);
 
@@ -122,8 +123,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
     initialpos = 0xFFFF;
 }
-double MotorSentData[1000] = {0};
-uint32_t PosDif[1000] = {0};
+double MotorSentData[100] = {0};
+uint32_t PosDif[100] = {0};
 
 
 int MotorDataIndex = 0;
@@ -175,7 +176,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
     MotorDataIndex+=1;
-    if (MotorDataIndex == 999) MotorDataIndex = 0;
+    if (MotorDataIndex == 99) MotorDataIndex = 0;
     static int pooler_counter = 0;
     pooler_counter++;
     if (pooler_counter%10 == 0)
@@ -248,6 +249,12 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
+double calculatedError[1000];
+int    readValue[1000];
+int    calculatedspeed[1000];
+int controlIndex = 0;
+int32_t KeepReadValue = 0;
+
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -259,6 +266,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int DancerId;
     static int pooler_counter = 0;
     int32_t TranslatedReadValue, avreageSampleValue = 0;
+    double tempcalcspeed = 0;
     uint32_t calculated_speed;
     double NormalizedError;
     char Message[60];
@@ -286,7 +294,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     {
         DancerId = ThreadMotorIdToDancerId[index];
         if (ReadValue < 10)
+        {
+            REPORT_MSG(ReadValue, "Dancer value read too small.");
+            return OK;
+        }
+        if (ReadValue == 0x3FFF)
+        {
+            LOG_ERROR(index, "Dancer value invalid.");
             return OK;
+        }
         TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
         if (index == POOLER_MOTOR)
         {
@@ -316,10 +332,26 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         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
+        {
             MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+        }
         else
-            KeepNormalizedError = NormalizedError;
+        {
+            //KeepNormalizedError = NormalizedError;
+        }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        if (index == POOLER_MOTOR)
+        {
+            if (KeepReadValue != TranslatedReadValue)
+            {
+                calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
+                readValue[controlIndex] = TranslatedReadValue;
+                calculatedspeed[controlIndex] = calculated_speed;
+                controlIndex++;
+                if (controlIndex >= 999) controlIndex = 0;
+                KeepReadValue = TranslatedReadValue;
+            }
+        }
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
             CurrentControlledSpeed[index] = calculated_speed;
@@ -385,7 +417,7 @@ bool InitialProcess = false;
             MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
             MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
             MotorControlConfig[Motor_i].m_params.epsilon = 0.1;
-            MotorControlConfig[Motor_i].m_params.dt = 50;
+            MotorControlConfig[Motor_i].m_params.dt = 1000;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
             MotorControlConfig[Motor_i].m_isEnabled = true;
@@ -481,10 +513,16 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 
     float process_speed;
     if (JobTicket->processparameters)
-        process_speed= JobTicket->processparameters->dyeingspeed;
+        if (JobTicket->processparameters->dyeingspeed)
+            process_speed= JobTicket->processparameters->dyeingspeed;
+        else
+        {
+            LOG_ERROR (-1," unknown job speed");
+            return ERROR;
+        }
 
     SetOriginMotorSpeed(process_speed);
-    //ControlStart();
+    ThreadControlActive = true;
     // 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]);
  //only for testing - when control works, these motors will take their speed from the dryer
@@ -548,6 +586,7 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
  uint32_t ThreadEndState(void *JobDetails)
 {
      int Motor_i;
+     ThreadControlActive = false;
      ThreadUpdateProcessLength (0.0,(void *)NULL);
     SetOriginMotorSpeed(0);