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

8 files changed, 163 insertions, 56 deletions

diff --git a/Software/Embedded_SW/Embedded/Common/report/report.h b/Software/Embedded_SW/Embedded/Common/report/report.h
index b43ed4991..ffe8cbfd4 100644
--- a/Software/Embedded_SW/Embedded/Common/report/report.h
+++ b/Software/Embedded_SW/Embedded/Common/report/report.h
@@ -556,7 +556,7 @@ extern void ReportSeveritySet(ErrorSeverity level);
 					  Report("ERROR :" msg,__FILE__,__LINE__,rc,RpWarning,0,0)
 
 #define  REPORT_MSG(rc,msg)   \
-					  Report("ERROR :" msg,__FILE__,__LINE__,rc,RpWarning,0,0)
+					  Report("MSG : " msg,__FILE__,__LINE__,rc,RpWarning,0,0)
 
 #define ON_ERROR_LOG_ERROR(msg)     if ( rc != 0 ) \
 					  Report("ERROR :" msg,__FILE__,__LINE__,rc,RpError,0,0)
diff --git a/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c b/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c
index 9d6f50e51..6cd17e5ef 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Uart_Comm/Uart.c
@@ -118,6 +118,8 @@ uint32_t Uart_rx_Counter = 0;
 //
 //*****************************************************************************
 //void UART0IntHandler(void)
+uint32_t uDataLength[50] = {0};
+byte uindex = 0;
 void UARTIntHandler(UArg arg0)
 {
     uint32_t ui32Status;
@@ -143,6 +145,9 @@ void UARTIntHandler(UArg arg0)
              if (U0_size_bar == 4)
              {
                  U0_expected_message_size = *(int *)size;
+                 //uDataLength[uindex] = U0_expected_message_size;
+                 //if (uindex++>=50)
+                 //    uindex = 0;
                  if (initArray( U0_expected_message_size) == 0)
                      U0_expected_message_size = 0;
                  U0_size_bar = 0;
@@ -200,6 +205,9 @@ uint32_t Uart_Tx(char *buffer,size_t length)
         }
 
     }
+    uDataLength[uindex] = length+4;
+    if (uindex++>=50)
+    uindex = 0;
 
 //    if (ret!=length)
 //        return 1;
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c
index 08ba6d2b4..94b49a8d9 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c
@@ -276,6 +276,7 @@ uint32_t DiagnosticsStart()
             DiagnosticsControlId = AddControlCallback(Diagnostics_ControlTrigger,DiagnosticLimit,DiagnosticsEmptyCBFunction,0,0,0);
         }
     }
+    return OK;
 }
 uint32_t DiagnosticsStop()
 {
@@ -286,6 +287,7 @@ uint32_t DiagnosticsStop()
             RemoveControlCallback(DiagnosticsControlId,Diagnostics_ControlTrigger);
         DiagnosticsControlId = 0xFF;
     }
+    return OK;
 }
 uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer)
 {
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;
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index 56c1b7be7..3384965d7 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -28,6 +28,8 @@
 #include <ti/sysbios/knl/Clock.h>
 #include <ti/sysbios/knl/task.h>
 
+#include "Common/report/report.h"
+
 #include "PMR/Printing/JobSegment.pb-c.h"
 #include "PMR/Printing/JobRequest.pb-c.h"
 #include "PMR/Printing/JobResponse.pb-c.h"
@@ -359,7 +361,8 @@ void JobRequestFunc(MessageContainer* requestContainer)
         jobStatus.progress = 0.0;
         jobStatus.has_currentsegmentindex = false;
         response.status = &jobStatus;
-        response.has_canceled = false;
+        response.has_canceled = true;
+        response.canceled = true;
         responseContainer = createContainer(MESSAGE_TYPE__JobResponse, JobToken, true, &response, &job_response__pack, &job_response__get_packed_size);
         container_buffer = malloc(message_container__get_packed_size(&responseContainer));
         responseContainer.has_error = true;
@@ -381,38 +384,46 @@ void SendJobProgress(double ProcessedLength, int SegmentId, bool done, char *Mes
     uint32_t status = NOT_SUPPORTED;
     MessageContainer responseContainer;
     uint8_t* container_buffer;
+
+    char logmsg[100];
+    int len;
+    static msdid = 0;
+    len = usnprintf(logmsg, 100, "MSG: Job Progress Length %f, Seg %d Done %d %s",ProcessedLength,  SegmentId,  done, Message);
+
+    //REPORT_MSG(msdid++,logmsg);
+    Report(logmsg,__FILE__,__LINE__,msdid,RpWarning,SegmentId,  done);
+
+
+    JobResponse response = JOB_RESPONSE__INIT;
+    JobStatus jobStatus = JOB_STATUS__INIT;
+
+
+    jobStatus.message = Message;
+
     if (ProcessedLength == 0.0)
     {
-        ProcessedLength = previousJobLength;
+        //ProcessedLength = previousJobLength;
+        jobStatus.has_progress = false;
+        jobStatus.progress = 0;
+        jobStatus.has_currentsegmentindex = false;
     }
     else
     {
-        previousJobLength =  ProcessedLength;
+        //previousJobLength =  ProcessedLength;
+        jobStatus.has_progress = true;
+        jobStatus.progress = ProcessedLength;
+        jobStatus.has_currentsegmentindex = true;
+        jobStatus.currentsegmentindex = SegmentId;
     }
+    response.status = &jobStatus;
 
-
-    JobResponse response = JOB_RESPONSE__INIT;
-    JobStatus jobStatus = JOB_STATUS__INIT;
     if (done == true)
     {
-        jobStatus.message = Message;
-        jobStatus.has_progress = true;
-        jobStatus.progress = ProcessedLength;
-        jobStatus.has_currentsegmentindex = false;
-        response.status = &jobStatus;
-        response.has_canceled = false;
         responseContainer = createContainer(MESSAGE_TYPE__JobResponse, JobToken, true, &response, &job_response__pack, &job_response__get_packed_size);
         container_buffer = malloc(message_container__get_packed_size(&responseContainer));
     }
     else
     {
-        jobStatus.message =Message;
-        jobStatus.has_progress = true;
-        jobStatus.progress = ProcessedLength;
-        jobStatus.has_currentsegmentindex = true;
-        jobStatus.currentsegmentindex = SegmentId;
-        response.status = &jobStatus;
-        response.has_canceled = false;
         responseContainer = createContainer(MESSAGE_TYPE__JobResponse, JobToken, false, &response, &job_response__pack, &job_response__get_packed_size);
         container_buffer = malloc(message_container__get_packed_size(&responseContainer));
     }