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

14 files changed, 293 insertions, 77 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 5ec7c6b16..9dc03601c 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -26,6 +26,7 @@
 
 Task_Handle AlarmHandling_Task_Handle;
 Mailbox_Handle          AlarmHandlingMsgQ = NULL;
+static GateMutex_Handle gateAlarmHandlingDB;
 
 /******************** Functions  ********************************************/
 //uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
@@ -56,6 +57,13 @@ void AlarmHandlingInit(void)
 
     //memset(AlarmHandlingDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
 
+    /*gateAlarmHandlingDB = GateMutex_create(NULL, &eb);
+    if (gateAlarmHandlingDB == NULL)
+    {
+        System_abort("Could not create USB Wait gate");
+    }*/
+
+
     return;
 }
 void AlarmHandlingIterate(UArg arg0)
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index a1de29b08..847b777fa 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -31,6 +31,7 @@
 #include "drivers/FPGA/FPGA.h"
 
 #include "modules/thread/thread_ex.h"
+#include "Drivers/Peripheral_GPIO/GPIO.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -236,7 +237,6 @@ uint32_t MillisecLoop(uint32_t tick)
         FPGA_Read_limit_Switches_Registers();
         //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
 #endif
-
         temp += MotorActivity;
    if (MotorActivity)
    {
@@ -340,6 +340,7 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
+        ACTIVITY_RED_LED_CHANGE_MODE;
     }
 
     if (Onesecond_Tick)
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index a0ba50078..8e999a668 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -51,6 +51,7 @@
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
+#include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "control.h"
 #include "MillisecTask.h"
 /******************** Definitions  ********************************************/
@@ -93,6 +94,8 @@ static GateMutex_Handle gateControlDB;
 Task_Handle Control_Task_Handle;
 ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
 uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
+uint16_t ControlBacklog[1000]={0};
+uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
 /******************** Functions  ********************************************/
 void OneMilliSecondFunction(UArg arg0);
@@ -148,6 +151,25 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
+uint32_t ControlActivityLed( uint32_t Parameter1, uint32_t Parameter2)
+{
+    static bool flag = false;
+    if (flag==true)
+    {
+        COMM_RED_LED_ON;
+        flag = false;
+    }
+    else
+    {
+        COMM_RED_LED_OFF;
+        flag = true;
+    }
+return OK;
+}
+uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
 
 void ControlStart(void)
 {
@@ -159,6 +181,8 @@ void ControlStart(void)
         TimerEnable(Control_timerBase, TIMER_A);
 
         ADCAcquireStart(0,1);
+        AddControlCallback( ControlEmptyCBFunction,  eHundredMillisecond, ControlActivityLed,0, 0, 0 );
+
     }
 }
 
@@ -260,45 +284,72 @@ uint32_t ControlLoop(uint32_t tick)
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
-    Tick998         =   (tick%eOneSecond == 998)         ?true:false;
+    Tick998         =   (tick%eOneSecond == 996)         ?true:false;
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
         if (ControlArray[Device_i].ControlActive)
         {
+            ControlBacklog[backlogindex]=Device_i;
+            if ( ++backlogindex >= 999)
+                backlogindex = 0;
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
+                    if(ControlArray[Device_i].ControlDataReadPtr)
                         ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                    else
+                        LOG_ERROR (Device_i, "Invalid callback ptr");
+                    if(ControlArray[Device_i].ControlCallbackPtr)
                         ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                    else
+                        LOG_ERROR (Device_i, "Invalid callback ptr");
                     break;
                 case eTenMilliSecond:
                     if (Ten_msTick)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
                 case eHundredMillisecond:
                     if (Tick98)
                     {
                         //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     if (Hundred_msTick)
                     {
                         //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
                 case eOneSecond:
                     if (Tick998)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     if (Onesecond_Tick)
                     {
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlCallbackPtr)
+                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     break;
                 default:
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 69ff06880..ff9acdb94 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -486,7 +486,7 @@ uint32_t HeatersControlLoop(uint32_t tick)
 {
     //char str[100];
     //uint8_t len = 0;
-    int DcHeaterId;
+    int DcHeaterId,HeaterSliceCounter;
     /*len = usnprintf(str, 100, "\r\n EightMilliSecondHeatersInterrupt SliceCounter %d Owner %d H1000 %d  H2000 %d"
                     ,SliceCounter,TimeSliceAllocation[SliceCounter],HeatersRestart,NumberOFSlicesInUse);
     Report(str, __FILE__,__LINE__,0, RpMessage, SliceCounter, TimeSliceAllocation[SliceCounter]);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index abd9794b8..6739619a7 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -35,7 +35,6 @@ double DispenserNormalizedErrorCoEfficient[MAX_SYSTEM_DISPENSERS] = {0};
 
 HardwarePidControlType ThreadDispenserIdToControlId[MAX_SYSTEM_DISPENSERS] = {  HARDWARE_PID_CONTROL_TYPE__Dispenser1,HARDWARE_PID_CONTROL_TYPE__Dispenser2,HARDWARE_PID_CONTROL_TYPE__Dispenser3,HARDWARE_PID_CONTROL_TYPE__Dispenser4,HARDWARE_PID_CONTROL_TYPE__Dispenser5,HARDWARE_PID_CONTROL_TYPE__Dispenser6,HARDWARE_PID_CONTROL_TYPE__Dispenser7,HARDWARE_PID_CONTROL_TYPE__Dispenser8};
 
-bool    DispenserReady[MAX_SYSTEM_DISPENSERS] = {true};
 
 /******************** STRUCTURES AND ENUMs  ********************************************/
 /******************** GLOBAL PARAMETERS  ********************************************/
@@ -84,7 +83,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 //callback - calls printing stm with the result
 
 // registration - 10 msec, dispenser pressure sensor
-//    AddControlCallback(DeviceId2Dispenser[DispenserId], DispenserControlCBFunction, eHundredMillisecond);
+//    AddControlCallback(DeviceId2Heater[HeaterId], HeaterControlCBFunction, eHundredMillisecond);
 // start the dispenser pressure building - move up in a TBD speed, valve closed
 //Dispenser Speed control
 //callback - handles speed
@@ -95,7 +94,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 //********************************************************************************************************************
  uint32_t IDSPrepareState(void *JobDetails)
 {
-    int Motor_i, HW_Motor_Id, Pid_Id,i;
+    int Motor_i, HW_Motor_Id, Pid_Id;
     //start IDS control for all motors
     for (Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++)
     {
@@ -118,11 +117,6 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
 
             MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); //set the dispenser to the
 #warning activate valve
-            /*Start the dispensers to build initial pressure
-             * check different handling for dispensers that participate in the first segment and idle dispensers
-             * start control for initial pressure
-             *
-             */
             //ValveCommand (Enable,MixerDirection);
     #ifdef DEBUG_TEST_FUNCTIONS
 // add control here
@@ -130,33 +124,16 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
     #endif
     }
     //set 3 dancers to the profile positions
-    for (i = 0; i < MAX_SYSTEM_DISPENSERS; i++)
-    {
-        DispenserReady[i] = true;
-    }
 
     return OK;
 }
- void DispenserPrepareReady(void)
- {
-     int i;
-     for (i=0;i<MAX_SYSTEM_DISPENSERS;i++)
-     {
-         if (DispenserReady[i] == false)
-         {
-             return; //not all configured Dispensers are ready
-         }
-     }
-     PrepareReady(Module_IDS,ModuleDone);
- }
 
 //********************************************************************************************************************
 uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
 {
 //set the speed only before the first segment, speed is constant accros job
     JobTicket* JobTicket = JobDetails;
-    int Dispenser_i;
-    TimerMotors_t HW_Motor_Id;
+    int Dispenser_i, HW_Motor_Id;
     int segmentfirst_speed;
 int CurrentSegment = 0;
     JobBrushStopId = 0;
@@ -171,12 +148,12 @@ int CurrentSegment = 0;
         OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
     }
     //ControlStart();
+    // set the new speed in the dryer Dispenser to the speed of the new segment
     MotorSetSpeed(HW_Motor_Id, OriginalDispenserSpd_2PPS[HW_Motor_Id]);
 
     // activate control fr all motors
-    /* wait for all dispensers  to get to the required pressure
-     * move the presegment ready when all dispensers are ready.
-     */
+    //set speed for both rocker motors
+    //wait for all motors to get to the required speed (set the target speed for the control to check)
     //call the job state machine when the thread system is ready
     PreSegmentReady(Module_IDS,ModuleDone);
 
@@ -193,10 +170,12 @@ int CurrentSegment = 0;
  uint32_t IDSEndState(void *JobDetails)
 {
      int Motor_i;
-     for ( Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++)
+     for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
      {
-        StopMotor(DispenserIdToMotorId[Motor_i],Hard_Hiz);
+        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
+     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
+     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
index 49c7d0d3b..669df84a5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
@@ -16,6 +16,8 @@
 
 #include "Stub_Status.h"
 
+#include "drivers/Motors/Motor.h"
+
 void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 {
     uint32_t status = NOT_SUPPORTED;
@@ -23,7 +25,12 @@ void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 
     StubCartridgeReadRequest* request = stub_cartridge_read_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
-	//request->cartridgeid
+    MillisecStart();
+    ControlStart();
+
+    //MotorMovetoLimitSwitch (14,1, 1000, 32, MotorSetSpeedCallBackFunction);
+    MotorSetSpeed( 15,  1000);
+    //request->cartridgeid
 
     writeLine("cartridge Request: ");
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c
index 11030f9c6..9584ee39b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.c
@@ -4,6 +4,13 @@
 #include <PMR/Common/MessageContainer.pb-c.h>
 #include <PMR/Stubs/StubI2CRequest.pb-c.h>
 #include <PMR/Stubs/StubI2CResponse.pb-c.h>
+
+#include <PMR/Stubs/StubI2CWriteBytesRequest.pb-c.h>
+#include <PMR/Stubs/StubI2CWriteBytesResponse.pb-c.h>
+
+#include <PMR/Stubs/StubI2CReadBytesRequest.pb-c.h>
+#include <PMR/Stubs/StubI2CReadBytesResponse.pb-c.h>
+
 #include <stdbool.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -23,6 +30,7 @@
 
 void Stub_I2CRequest(MessageContainer* requestContainer)
 {
+
     uint32_t status = PASSED;
     MessageContainer responseContainer;
 
@@ -35,7 +43,24 @@ void Stub_I2CRequest(MessageContainer* requestContainer)
 
 	response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytetowrite );
 	response.has_readbyte= true;
+/*
+	//--------------------------
+
+    response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytestwrite[0]);
+    response.has_readbyte= true;
 
+	uint32_t I2C_W_buf[256] = {0},i;
+
+	for(i = 0; i < request->n_bytestwrite;i++)
+	{
+	    I2C_W_buf[i] = request->bytestwrite[i];
+	}
+
+	response.readbytes = I2C_W_buf;
+	response.n_readbytes = request->n_bytestwrite;
+
+	//-------------------------------
+*/
 	status_response(status,&response.status, &response.statusword ,&response.has_statusword);
 
     writeLine("Sending Response: ");
@@ -52,4 +77,127 @@ void Stub_I2CRequest(MessageContainer* requestContainer)
     SendChars((char*)container_buffer, container_size);
 
     stub_i2_crequest__free_unpacked(request,NULL);
+
 }
+
+
+void Stub_I2CWriteBytesRequest(MessageContainer* requestContainer)
+{
+
+    uint32_t status = PASSED;
+    MessageContainer responseContainer;
+
+    StubI2CWriteBytesRequest* request = stub_i2_cwrite_bytes_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StubI2CWriteBytesResponse response =   STUB_I2_CWRITE_BYTES_RESPONSE__INIT;
+
+    response.i2cid = request->i2cid;
+    response.has_i2cid = true;
+    response.slaveaddress = request->slaveaddress;
+    response.has_slaveaddress = true;
+
+    //response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytetowrite );
+    //response.has_readbyte= true;
+
+    //--------------------------
+
+    //response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytestwrite[0]);
+    //response.has_readbyte= true;
+
+    uint8_t I2C_W_buf[256] = {0},i;
+
+    for(i = 0; i < request->n_bytestwrite;i++)
+    {
+        I2C_W_buf[i] = (uint8_t)(request->bytestwrite[i] & 0xff);
+    }
+
+    //response.readbytes = I2C_W_buf;
+    //response.n_readbytes = request->n_bytestwrite;
+
+    //-------------------------------
+
+
+    //----------------------
+
+    status = I2C_Write(request->i2cid, request->slaveaddress, I2C_W_buf, request->n_bytestwrite);
+
+    //--------------------------
+
+    status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+    writeLine("Sending Response: ");
+//
+
+    Write_status_response(status);
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubI2CWriteBytesResponse, requestContainer->token, true, &response, &stub_i2_cwrite_bytes_response__pack, &stub_i2_cwrite_bytes_response__get_packed_size);
+
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+
+    stub_i2_cwrite_bytes_request__free_unpacked(request,NULL);
+
+}
+
+void Stub_I2CReadBytesRequest(MessageContainer* requestContainer)
+{
+    uint8_t I2C_R_buf[256] = {0},i;
+    uint32_t I2C_R32_buf[256] = {0};
+
+    uint32_t status = PASSED;
+    MessageContainer responseContainer;
+
+    StubI2CReadBytesRequest* request = stub_i2_cread_bytes_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    StubI2CReadBytesResponse response =   STUB_I2_CREAD_BYTES_RESPONSE__INIT;
+
+    response.i2cid = request->i2cid;
+    response.has_i2cid = true;
+    response.slaveaddress = request->slaveaddress;
+    response.has_slaveaddress = true;
+
+    status = I2C_Read(request->i2cid, request->slaveaddress, I2C_R_buf, request->numberofbytestoread);
+
+    //response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytetowrite );
+    //response.has_readbyte= true;
+
+    //--------------------------
+
+    //response.readbyte = I2C_control(request->i2cid, request->slaveaddress, request->readorwrite,request->bytestwrite[0]);
+    //response.has_readbyte= true;
+
+    //uint32_t I2C_W_buf[256] = {0},i;
+
+    for(i = 0; i < request->numberofbytestoread;i++)
+    {
+        I2C_R32_buf[i]= (uint32_t)I2C_R_buf[i] ;
+    }
+
+    response.readbytes = I2C_R32_buf;
+    response.n_readbytes = request->numberofbytestoread;
+
+    //-------------------------------
+
+    status_response(status,&response.status, &response.statusword ,&response.has_statusword);
+
+    writeLine("Sending Response: ");
+//
+
+    Write_status_response(status);
+
+    responseContainer = createContainer(MESSAGE_TYPE__StubI2CReadBytesResponse, requestContainer->token, true, &response, &stub_i2_cread_bytes_response__pack, &stub_i2_cread_bytes_response__get_packed_size);
+
+    //-------------------------------------------------------------------------------------------
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars((char*)container_buffer, container_size);
+
+    stub_i2_cread_bytes_request__free_unpacked(request,NULL);
+}
+
+
+
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.h b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.h
index b2d6e0dbc..e91a5fd09 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.h
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_I2C.h
@@ -2,5 +2,7 @@
 #define STUB_MOTORENCODER_H
 
 void Stub_I2CRequest(MessageContainer* requestContainer);
+void Stub_I2CReadBytesRequest(MessageContainer* requestContainer);
+void Stub_I2CWriteBytesRequest(MessageContainer* requestContainer);
 
 #endif //STUB_STUB_MOTORENCODER_H
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
index d27036523..01c91c72b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -43,7 +43,7 @@
 #include "driverlib/ssi.h"
 #include "drivers/SPI/SPI_Comm.h"
 #include "Modules/Thread/Thread_ex.h"
-#include "drivers/Motors/Motor.h"
+
 
 #include "Modules/thread/thread.h"
 #include "Drivers/Motors/Motor.h"
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 57b7a9d99..dad7a1db6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -55,7 +55,7 @@ extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
-extern double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
+extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
 
 
 uint32_t InternalWinderConfigMessage(HardwareWinder* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 164e5eb23..37f60e8cf 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -9,6 +9,7 @@
 #include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "drivers/Motors/Motor.h"
 #include "StateMachines/Printing/PrintingSTM.h"
+#include "Modules/Control/Control.h"
 
 #include "drivers/FPGA/FPGA.h"
 
@@ -16,10 +17,12 @@
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
 bool Winder_ScrewHoming = false;
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps);
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag);
 uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue);
 
+bool ScrewCurrentDirection = false;
 double ScrewSpeed = 0;
+uint32_t ScrewControlId = 0xFF;
 
 uint32_t Winder_Init(void)
 {
@@ -75,7 +78,7 @@ uint32_t Winder_Prepare(void *JobDetails)
 
     float process_speed = JobTicket->processparameters->dyeingspeed;
 //    double ScrewSpeed = (50 *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
+    double ScrewSpeed = (process_speed*20 );//*  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
 
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
@@ -90,22 +93,23 @@ uint32_t Winder_Prepare(void *JobDetails)
     else
     {
         Winder_ScrewHoming = true;
-//        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2);
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1);
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_LEFT, Winder_PrepareStage2);
+//        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1);
     }
     return status;
 }
 uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 {
-    uint32_t status;
-    uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses;
+    uint32_t status=OK;
+    uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
      * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
      * report ready to the job STM
      */
-        status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out
-//        status |= MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
+        //status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out
+        //status |= StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0);
+        status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback);
         //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback);
         //set motor location 0 here
         return status;
@@ -116,6 +120,24 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
 {
     return PreSegmentReady(Module_Winder,ModuleDone);
 }
+uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
+{
+    //ScrewCurrentDirection: false moves out, true moves home
+    if (BusyFlag == NOTBUSY)
+    {
+        if (ScrewCurrentDirection == false)
+        {
+            ScrewCurrentDirection = true;
+        }
+        else
+        {
+            ScrewCurrentDirection = false;
+        }
+        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back.
+    }
+    return OK;
+
+}
 uint32_t Winder_Presegment(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
@@ -123,8 +145,9 @@ uint32_t Winder_Presegment(void *JobDetails)
     float screw_speed = 0;
     float RotationsPerSecond;
 
+    ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
 //    double ScrewSpeed = (process_speed *  MotorsCfg[SCREW_MOTOR].pulseperround *  MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius);
-    double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
+    //double ScrewSpeed = (process_speed *  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);
 
 /*typedef struct
 {
@@ -150,16 +173,19 @@ uint32_t Winder_Presegment(void *JobDetails)
 //     * 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
+    MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep); //process: set point 0, set max speed, move to the specified length, return back.
+    ScrewControlId = AddControlCallback(ScrewDirectionChange, eTenMilliSecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
 
-    //MotorMove (InternalWinderCfg.segmentoffsetpulses,screw_speed); process: set point 0, set max speed, move to the specified length, return back.
-     MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
+    // 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;
 }
 uint32_t Winder_End(void)
 {
     //stop screw
+    RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
     return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
@@ -176,19 +202,10 @@ void Winder_ScrewOutLimitSwitchInterrupt(void)
     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
 }
-uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps)
+uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
 {
-    if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses)
-    {
-        PrepareReady(Module_Winder, ModuleDone);
-        return OK;
-    }
-    else
-    {
-        //do we want to do something?
-        PrepareReady(Module_Winder, ModuleFail);
-        return ERROR;
-    }
-
+    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+    PrepareReady(Module_Winder, ModuleDone);
+    return OK;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 28a2346c1..b7401c77f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -2,6 +2,8 @@
 #ifndef MODULES_THREAD_THREAD_EX_H_
 #define MODULES_THREAD_THREAD_EX_H_
 
+#include "PMR/Printing/JobSpool.pb-c.h"
+
 //typedef enum
 //{
 #define WINDER_DANCER HARDWARE_DANCER_TYPE__LeftDancer
@@ -10,6 +12,8 @@
 #define NUM_OF_DANCERS HARDWARE_DANCER_TYPE__RightDancer+1
 //} DANCER_ENUM;
 
+uint32_t InternalWindingConfigMessage(JobSpool* request);
+
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
@@ -18,7 +22,7 @@ uint32_t ThreadInitialTestStub();
 
 //uint32_t MotorPidRequestMessage(HardwarePidControl* request);
 uint32_t Winder_Init(void);
-uint32_t Winder_Prepare(void *JobDetails);
+uint32_t Winder_Prepare(void);
 uint32_t Winder_Presegment(void *JobDetails);
 uint32_t Winder_End(void);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index e2d492b47..3c9adbc31 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -17,7 +17,7 @@ HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 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};
-double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
+int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
 InternalWinderConfigStruc InternalWinderCfg = {0};
 HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};
 
@@ -101,8 +101,9 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
     temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
     NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
 //    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
-    DancerStopActivityLimit[Motor_i] = ((1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits)*(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm)*2));
-    DancerStopActivityLimit[Motor_i] = DancerStopActivityLimit[Motor_i]/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
+    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
+    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*2);
+    DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
     return OK;
 }
 uint32_t DancerConfigMessage(HardwareDancer * request)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 56e1bf844..fdae44171 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -121,7 +121,6 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
 }
 double MotorSentData[1000] = {0};
 uint32_t PosDif[1000] = {0};
-uint32_t tick[1000] = {0};
 
 
 int MotorDataIndex = 0;
@@ -172,15 +171,14 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     PosDif[MotorDataIndex] = CurrentPosition;
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
-    tick[MotorDataIndex] = UsersysTickGet();
     MotorDataIndex+=1;
     if (MotorDataIndex == 999) MotorDataIndex = 0;
     static int pooler_counter = 0;
     pooler_counter++;
     if (pooler_counter%10 == 0)
     {
-        //SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
-        SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false);
+        SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
+        //SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false);
 
     }
     if (pooler_counter>=100)
@@ -486,10 +484,10 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 //#warning rocker disabled
     if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize)
         MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize);
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 10);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 5);
     if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize)
         MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
-    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10);
+    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 5);
 //#warning rocker disabled
 
 //    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO