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

10 files changed, 127 insertions, 16 deletions

diff --git a/Software/Embedded_SW/Embedded/Communication/Container.c b/Software/Embedded_SW/Embedded/Communication/Container.c
index f7c9ef9fc..2b12cbeec 100644
--- a/Software/Embedded_SW/Embedded/Communication/Container.c
+++ b/Software/Embedded_SW/Embedded/Communication/Container.c
@@ -63,6 +63,11 @@ MessageContainer createContainer(MessageType type, char* token, protobuf_c_boole
     //free(response);
     return container;
 }
+uint32_t msgId[50] = {0};
+uint32_t Length[50] = {0};
+uint32_t DataLength[50] = {0};
+byte index = 0;
+
 void receive_callback(char* buffer, size_t length)
 {
      //SendChars(buffer,length);
@@ -75,7 +80,13 @@ void receive_callback(char* buffer, size_t length)
      //writeLine("Parsing message...");
 
      MessageContainer* requestContainer = message_container__unpack(NULL, length, (uint8_t*)buffer);
+     HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/length,requestContainer->type,0,0, "Container");
 
+     msgId[index] = requestContainer->type;
+     Length[index] = length;
+     DataLength[index] = requestContainer->data.len;
+     if (index++>=50)
+         index = 0;
      switch(requestContainer->type)
      {
          case MESSAGE_TYPE__CalculateRequest:
diff --git a/Software/Embedded_SW/Embedded/DataDef.h b/Software/Embedded_SW/Embedded/DataDef.h
index 1015203a9..c10727a14 100644
--- a/Software/Embedded_SW/Embedded/DataDef.h
+++ b/Software/Embedded_SW/Embedded/DataDef.h
@@ -11,6 +11,9 @@
 
 #define MAX_STRING_LEN              255 //Embedded  version + filter.c
 
+#define MAX_ERR_SAMPLES 5 //Number of Samples before sending error
+#define ERROR_TEMPERATURE_MEASUREMENT 0xFFFF
+
 #define ON  1
 #define OFF 0
 
@@ -23,8 +26,9 @@ enum
 };
 
 
-#define OK     0
-#define ERROR  1
+#define OK       0
+#define ERROR    1
+#define WARNING  2
 
 #define ENABLE  1
 #define DISABLE 0
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_Comm.h
index 4be867ee7..ffd484d65 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_Comm.h
@@ -1216,8 +1216,11 @@ typedef union
 #define RESERVE_BIT14     BIT14
 #define RESERVE_BIT15     BIT15
 
-#define DYEINGH_SSR10_CTRL BIT2
-#define DYEINGH_SSR11_CTRL BIT4
+//F1_gpo_01
+#define DYEINGH_SSR10_CTRL BIT2 //HeadHeaterZ5
+
+//F3_GPO_01_bus
+#define DYEINGH_SSR11_CTRL BIT4 //HeadHeaterZ6
 
 
 /*
diff --git a/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c b/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c
index 077841577..b4fc95621 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Heater/TemperatureSensor.c
@@ -15,6 +15,7 @@
 #include "drivers/FPGA/FPGA_Comm.h"
 #include "modules/control/millisecTask.h"
 #include "PT100RTD.h"
+#include <DataDef.h>
 
 uint32_t TemperatureSensorReadFromFPGA_Res(TEMPERATURE_SENSOR_ID_ENUM SensorId);
 //uint32_t TemperatureSendSensorDummyClk(TEMP_SENSE_ANALOG_DYEINGH_TEMP1);
@@ -231,6 +232,33 @@ uint32_t Filter_Temparature_Measurement(TEMPERATURE_SENSOR_ID_ENUM SensorId, int
     return Status;
 }
 
+
+uint32_t Filter_Error_Temparature_Measurements(TEMPERATURE_SENSOR_ID_ENUM SensorId, int32_t Current_Measurement_C)
+{
+    static uint8_t counter[MAX_TEMPERATURE_SENSOR_ID];
+
+    uint32_t Status = OK;
+
+    if (Current_Measurement_C >= ERROR_TEMPERATURE_MEASUREMENT)
+    {
+        if(counter[SensorId]==MAX_ERR_SAMPLES)
+        {
+            Status =  ERROR;
+        }
+        else
+        {
+            counter[SensorId]+=1;
+            Status =  WARNING;
+        }
+    }
+    else
+    {
+        counter[SensorId] = 0;
+    }
+
+    return Status;
+}
+
 uint32_t TemperatureSensorReadFromFPGA(TEMPERATURE_SENSOR_ID_ENUM SensorId)
 {
     uint32_t Status = OK;
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index bc7e08e75..a03d74dce 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -461,7 +461,10 @@ uint32_t MotorMovetoLimitSwitch (TimerMotors_t _motorId,bool direction, uint32_t
     assert (LimitSwitchId<=MAX_GPI);
 
     if (MotorMovetoLimitSwitchControlId[_motorId] != 0xFF)
-        return ERROR;
+    {
+        RemoveControlCallback(MotorMovetoLimitSwitchControlId[_motorId], MotorMoveToLimitSwitchCallBackFunction );
+        //return ERROR;
+    }
     //call driver action to device id with the parameter
     //SetMotorSpeed (deviceId, parameter);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c
index 41af277a8..cd827b6bf 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.c
@@ -85,6 +85,28 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams)
                     HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, false,
                     ProcessParams->headzone4temp);
     }
+    if (ProcessParams->has_headzone5temp)
+    {
+        if (ProcessParams->headzone5temp)
+            status |= HeaterCommandRequestMessage(
+                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, true,
+                    ProcessParams->headzone5temp);
+        else
+            status |= HeaterCommandRequestMessage(
+                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, false,
+                    ProcessParams->headzone5temp);
+    }
+    if (ProcessParams->has_headzone6temp)
+    {
+        if (ProcessParams->headzone6temp)
+            status |= HeaterCommandRequestMessage(
+                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, true,
+                    ProcessParams->headzone6temp);
+        else
+            status |= HeaterCommandRequestMessage(
+                    HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, false,
+                    ProcessParams->headzone6temp);
+    }
     if (ProcessParams->has_dryerzone1temp)
     {
         if (ProcessParams->dryerzone1temp)
@@ -150,7 +172,7 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams)
     {
         dryerairflow = ProcessParams->dryerairflow;
     }
-    return status;
+    return OK;//status;
 }
 
 void ProcessRequestFunc(MessageContainer* requestContainer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index aa9e6ec94..a119b0b16 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -199,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_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index cfff7773b..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];
@@ -324,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;
@@ -393,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;
@@ -489,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
@@ -556,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);
 
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
index fafe1dd8e..d0b17fbf5 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.c
@@ -288,11 +288,17 @@ uint32_t EndState(void *JobDetails, char *Message)
     DiagnosticsStop();
     if (Configured[Module_Winder])
     {
+        PrepareWaiting[Module_Winder] = ModuleIdle;
+        SegmentWaiting[Module_Winder] = ModuleIdle;
+        PreSegmentWaiting[Module_Winder] = ModuleIdle;
         //    EndWaiting[Module_Winder] = ModuleWaiting;
         Winder_End();
     }
     if (Configured[Module_IDS])
     {
+        PrepareWaiting[Module_IDS] = ModuleIdle;
+        SegmentWaiting[Module_IDS] = ModuleIdle;
+        PreSegmentWaiting[Module_IDS] = ModuleIdle;
         //EndWaiting[Module_IDS] = ModuleWaiting;
         IDSEndState(JobDetails);
     }
@@ -304,6 +310,9 @@ uint32_t EndState(void *JobDetails, char *Message)
     }*/
     if (Configured[Module_Thread])
     {
+        PrepareWaiting[Module_Thread] = ModuleIdle;
+        SegmentWaiting[Module_Thread] = ModuleIdle;
+        PreSegmentWaiting[Module_Thread] = ModuleIdle;
         //EndWaiting[Module_Thread] = ModuleWaiting;
         ThreadEndState(CurrentJob);
     }
@@ -347,7 +356,7 @@ void PrintSTMMsgHandler(void * msg)
     JobMessageStruc *Message = msg;
     PrintMessageStruc *PrtMessage = (PrintMessageStruc *)Message->messageData;
 
-    Report(REPORT_LINE("PrintSTMMsgHandler"),__FILE__,__LINE__, RpMessage,0x1000,Message->messageId,PrtMessage->messageId);
+    Report("PrintSTMMsgHandler",__FILE__,__LINE__, RpMessage,0x1000,Message->messageId,PrtMessage->messageId);
 
     if (Message->messageId != PrintMessage)
     {
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h
index 0a210eeb1..61a51ba92 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/PrintingSTM.h
@@ -119,7 +119,7 @@ typedef struct PrintMessage{
 }PrintMessageStruc;
 
 extern bool Configured[MAX_SYSTEM_MODULES];
-
+extern ModuleStateEnum PrepareWaiting[MAX_SYSTEM_MODULES];
 extern JobTicket *CurrentJob;
 //extern JobTicket *PreviousJob;
 extern Mailbox_Handle JobmsgQ;