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

4 files changed, 122 insertions, 36 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index dad7a1db6..7780287fb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -48,7 +48,7 @@ extern HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM];
 
 extern HardwareMotor                MotorsCfg[NUM_OF_MOTORS];
 extern HardwarePidControl           MotorsControl[MAX_THREAD_MOTORS_NUM];
-extern InternalWinderConfigStruc    InternalWinderCfg;
+//extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 
 #define MAX_CONTROL_SAMPLES 10
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index a119b0b16..33086b636 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -1,7 +1,7 @@
 /*
  * Thread_Winder.c
  *
- *  Created on: 25 áîøõ 2018
+ *  Created on: 25 áîøõ 2018
  *      Author: shlomo
  */
 #include"include.h"
@@ -11,6 +11,8 @@
 #include "StateMachines/Printing/PrintingSTM.h"
 #include "Modules/Control/Control.h"
 
+#include "Common/report/report.h"
+
 #include "drivers/FPGA/FPGA.h"
 
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
@@ -27,10 +29,35 @@ uint32_t ScrewControlId = 0xFF;
 uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
 uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw  - will be used to build the cone
 
+InternalWinderConfigStruc InternalWinderCfg = {0};
+
 uint32_t Winder_Init(void)
 {
     return OK;
 }
+
+
+uint32_t InternalWinderConfigMessage(HardwareWinder* request)
+{
+    uint32_t status = PASSED;
+
+    InternalWinderCfg.milimetersperrotation = request->millimeterperrotation;
+
+    return status;
+}
+uint32_t InternalWindingConfigMessage(JobSpool* request)
+{
+    uint32_t status = PASSED;
+
+    InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
+    InternalWinderCfg.spoolbackingrate = request->backingrate;
+    InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
+    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
+    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
+    InternalWinderCfg.diameter = request->diameter;
+
+    return status;
+}
 /*
  *uint32_t Winder_Prepare(void *JobDetails)
 
@@ -107,7 +134,56 @@ uint32_t ScrewControlId = 0xFF;
 uint32_t ScrewNumberOfSteps = 0;
 uint32_t DirectionChangeCounter = 0;
 */
+/*
+    InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
+    InternalWinderCfg.spoolbackingrate = request->backingrate;
+    InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
+    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
+    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
+ *
+Calculate the number of steps.
+Initial home position = ...
+Initial out movement = ScrewNumberOfSteps
+if DirectionChangeCounter %= backingrate : reduce one from the  ScrewNumberOfSteps
+if DirectionChangeCounter %= bottombackingrate && direction was out: ADD one to the  ScrewNumberOfSteps,
+// WRONG? if the flag is raised - lower it and reduce one from the  ScrewNumberOfSteps
+30:100 - 70
+30:99 - 69
+30:98 - 68
+29:98 - 69
+29:97 -68
+InternalWinderCfg.segmentoffsetpulses
+  int32_t backingrate;
+  int32_t bottombackingrate;
+  
+numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+*/
+uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
+{
+    uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 
+    if (Counter)
+    {
+        if (Counter%InternalWinderCfg.spoolbackingrate == 0)
+        {
+            NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
+            REPORT_MSG(ScrewNumberOfSteps, "Head Backing");
+            ScrewNumberOfSteps--;
+        }
+        if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 0))
+        {
+            if (direction != MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize)
+            {
+                NumberOfSteps += (Counter/InternalWinderCfg.SpoolBottomBackingRate);
+                ScrewNumberOfSteps++;
+                REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing");
+            }
+
+        }
+    }
+    return CalculateNumberOfSteps;
+
+}
 uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     if (BusyfirstCall)
@@ -118,6 +194,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     //ScrewCurrentDirection: false moves out, true moves home
     if (BusyFlag == NOTBUSY)
     {
+        CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection);
         if (ScrewCurrentDirection == false)
         {
             ScrewCurrentDirection = true;
@@ -127,7 +204,6 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
             ScrewCurrentDirection = false;
         }
         MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
-        DirectionChangeCounter++;
     }
 /*
  * calculate new ScrewSpeed and call     MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
@@ -197,6 +273,7 @@ uint32_t Winder_End(void)
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
     uint32_t status;
+    //handle glitch - send information to the next time that the motor stops
     if  (Winder_ScrewHoming)
     {
         MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); //stop ASAP
@@ -205,6 +282,7 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void)
 }
 void Winder_ScrewOutLimitSwitchInterrupt(void)
 {
+    //handle glitch - send information to the next time that the motor stops
     uint32_t status;
     status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 3c9adbc31..c5f45da78 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -18,35 +18,10 @@ int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
 int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
-InternalWinderConfigStruc InternalWinderCfg = {0};
 HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
 
 HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0};
 
-
-
-uint32_t InternalWinderConfigMessage(HardwareWinder* request)
-{
-    uint32_t status = PASSED;
-
-    InternalWinderCfg.milimetersperrotation = request->millimeterperrotation;
-
-    return status;
-}
-uint32_t InternalWindingConfigMessage(JobSpool* request)
-{
-    uint32_t status = PASSED;
-
-    InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
-    InternalWinderCfg.spoolbackingrate = request->backingrate;
-    InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
-    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
-    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
-    InternalWinderCfg.diameter = request->diameter;
-
-    return status;
-}
-
 //********************************************************************************************************************
 uint32_t MotorsConfigMessage(HardwareMotor * request)
 {
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 0701a2aac..d9aeb503a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -70,6 +70,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
 void ThreadSegmentEnded(void);
 void ThreadInterSegmentEnded(void);
 void ThreadDistanceToSpoolEnded(void);
+uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue);
+void SetOriginMotorSpeed(float process_speed);
 
 double KeepNormalizedError = 0;
 bool ThreadControlActive = false;
@@ -248,12 +250,37 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     return OK;
 }
-double calculatedError[1000];
-int    readValue[1000];
-int    calculatedspeed[1000];
+/*double calculatedError[100];
+double eNormalizedError[100];
+int    readValue[100];
+int    TranslatedreadValue[100];
+int    AveragereadValue[100];
+int    calculatedspeed[100];
 int controlIndex = 0;
 int32_t KeepReadValue = 0;
-
+void testDancersControl()
+{
+    int mm20,mm10,mm5,mm2,mm1;
+    mm20 = (20*DancerStopActivityLimit[FEEDER_MOTOR])/(DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].maximalmovementmm*2);
+    mm2 = mm20/10;
+    mm5 = mm20/4;
+    mm10 = mm20/2;
+    mm1 = mm20/20;
+    ThreadControlActive = true;
+    SetOriginMotorSpeed(30.0);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm5);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm2);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm1);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm1);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm2);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm5);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm10);
+    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm20);
+    ThreadControlActive = false;
+}*/
 uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
 //#define MAX_CONTROL_SAMPLES 6
@@ -270,6 +297,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     double NormalizedError;
     char Message[60];
 
+    if (ThreadControlActive == false)
+        return OK;
     if (IfIndex>>8 != IfTypeThread)
     {
         LOG_ERROR (IfIndex, "Wrong  Interface type");
@@ -339,18 +368,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             //KeepNormalizedError = NormalizedError;
         }
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-        if (index == POOLER_MOTOR)
+        /*if (index == FEEDER_MOTOR)
         {
             if (KeepReadValue != TranslatedReadValue)
             {
+                eNormalizedError[controlIndex] = NormalizedError;
                 calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
-                readValue[controlIndex] = TranslatedReadValue;
+                readValue[controlIndex] = ReadValue;
+                TranslatedreadValue[controlIndex] = TranslatedReadValue;
+                AveragereadValue[controlIndex] = avreageSampleValue;
                 calculatedspeed[controlIndex] = calculated_speed;
                 controlIndex++;
-                if (controlIndex >= 999) controlIndex = 0;
+                if (controlIndex >= 99) controlIndex = 0;
                 KeepReadValue = TranslatedReadValue;
             }
-        }
+        }*/
         if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
         {
             CurrentControlledSpeed[index] = calculated_speed;
@@ -482,6 +514,7 @@ bool InitialProcess = false;
 //            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
 //                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
     }
+    //testDancersControl();
     PrepareReady(Module_Thread,ModuleDone);
     //set 3 dancers to the profile positions
     InitialProcess = true;