real time improvement, screw speed recalculated

6 files changed, 41 insertions, 29 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 8243c5faa..a29eea480 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -387,23 +387,23 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         Speed_Data = Read_Speed_Sensor();
     }
-    if (Tick98)
+/*    if (Tick98)
     {
         MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
         MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
         MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
         MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_WINDER);
         MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
-    }
+    }/*
     if (Hundred_msTick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
-        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
+       /* MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
         MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING);
         MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING);
         MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_WINDER);
-        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+        MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);*/
     }
 
     if (O900Millisecond_Tick)
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index ead95388a..c2d79bb69 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -201,11 +201,16 @@ void Diagnostic100msecCollection(void)
 
     if (JobIsActive())
     {
-        DiagnosticLoadMotor(FEEDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING));
+/*        DiagnosticLoadMotor(FEEDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)); ThreadGetMotorSpeed
         DiagnosticLoadMotor(DRYER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING));
         DiagnosticLoadMotor(POOLER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING));
         DiagnosticLoadMotor(WINDER_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER));
-        DiagnosticLoadMotor(SCREW_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW));
+        DiagnosticLoadMotor(SCREW_MOTOR, MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW));*/
+        DiagnosticLoadMotor(FEEDER_MOTOR, ThreadGetMotorSpeed (FEEDER_MOTOR));
+        DiagnosticLoadMotor(DRYER_MOTOR, ThreadGetMotorSpeed (DRYER_MOTOR));
+        DiagnosticLoadMotor(POOLER_MOTOR, ThreadGetMotorSpeed (POOLER_MOTOR));
+        DiagnosticLoadMotor(WINDER_MOTOR, ThreadGetMotorSpeed (WINDER_MOTOR));
+        DiagnosticLoadMotor(SCREW_MOTOR, ThreadGetMotorSpeed (SCREW_MOTOR));
     }
 /*
     feedermotorfrequency[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
@@ -221,9 +226,9 @@ void Diagnostic100msecCollection(void)
     DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP4));
     DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5, TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP5));
     DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6, TemperatureSensorRead(TEMP_SENSE_ANALOG_MIXCHIP_TEMP));
-    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1));
-    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2));
-    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3));
+    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1));
+    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2));
+    DiagnosticLoadTemperature(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary, TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3));
 /*
     mixertemperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_AN_ENCLOSURETEMP3)/100;
     headzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP1)/100;
@@ -343,9 +348,9 @@ void SendDiagnostics(void)
     DiagnosticsMonitor.headzone4temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4];
     DiagnosticsMonitor.headzone5temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5];
     DiagnosticsMonitor.headzone6temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6];
-    DiagnosticsMonitor.dryerzone1temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w];
-    DiagnosticsMonitor.dryerzone2temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1];
-    DiagnosticsMonitor.dryerzone3temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2];
+    DiagnosticsMonitor.dryerzone1temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature];
+    DiagnosticsMonitor.dryerzone2temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain];
+    DiagnosticsMonitor.dryerzone3temperature = HeaterTemperature[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary];
 
     DiagnosticsMonitor.n_mixertemperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__MixerHeater];
     DiagnosticsMonitor.n_headzone1temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ1];
@@ -354,9 +359,9 @@ void SendDiagnostics(void)
     DiagnosticsMonitor.n_headzone4temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ4];
     DiagnosticsMonitor.n_headzone5temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ5];
     DiagnosticsMonitor.n_headzone6temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ6];
-    DiagnosticsMonitor.n_dryerzone1temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeater1000w];
-    DiagnosticsMonitor.n_dryerzone2temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w1];
-    DiagnosticsMonitor.n_dryerzone3temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeater200w2];
+    DiagnosticsMonitor.n_dryerzone1temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerAirTemperature];
+    DiagnosticsMonitor.n_dryerzone2temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain];
+    DiagnosticsMonitor.n_dryerzone3temperature = HeaterCounterIndex[HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary];
 
     memset(HeaterCounterIndex,0,sizeof(HeaterCounterIndex));
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index 794559d57..486f8384f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -168,7 +168,7 @@ numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__
 uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
 {
     uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
-    float screw_speed = 0;
+    float screw_horizontal_speed = 0;
     float RotationsPerSecond;
 
     if (Counter)
@@ -178,14 +178,15 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
             NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
             REPORT_MSG(ScrewNumberOfSteps, "Head Backing");
             ScrewNumberOfSteps--;
-            screw_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
+            screw_horizontal_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 = dyeingspeed / (InternalWinderCfg.diameter * PI);
             RotationsPerSecond =  CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
             // 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;
+            ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
 
             MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+            CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
 
         }
         if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1))
@@ -258,7 +259,7 @@ uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
 uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
 {
     //JobTicket* JobTicket = JobDetails;
-    float screw_speed = 0;
+    float screw_horizontal_speed = 0;
     float RotationsPerSecond;
 
     if (dyeingspeed == 0)
@@ -275,15 +276,16 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         //     * 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;
+        screw_horizontal_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 = dyeingspeed / (InternalWinderCfg.diameter * PI);
         RotationsPerSecond =  CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
         // 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;
+        ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
 
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
-        //screw_speed = InternalWinderCfg.milimetersperrotation
+        CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+        //screw_horizontal_speed = InternalWinderCfg.milimetersperrotation
     //     * 2. determine optimal micro-step setting
     //     * 3. calculate cart travel length from winding parameters
     //     * 4. start move of travel length
@@ -292,7 +294,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         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);
+        // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_horizontal_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;
 
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 7405a91d9..79c2bf843 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -25,6 +25,7 @@ extern double TotalProcessedLength;
 
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 
+uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *JobDetails);
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 8d1ea20fa..d56ff34cf 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -127,8 +127,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
     initialpos = 0xFFFF;
 }
-double MotorSentData[102] = {0};
-uint32_t PosDif[102] = {0};
+double MotorSentData[100] = {0};
+uint32_t PosDif[100] = {0};
 
 
 int MotorDataIndex = 0;
@@ -181,7 +181,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     //PosDif[MotorDataIndex] = positionDiff;
     MotorSentData[MotorDataIndex] = length;
     MotorDataIndex+=1;
-    if (MotorDataIndex >= 97) MotorDataIndex = 0;
+    if (MotorDataIndex == 99) MotorDataIndex = 0;
     static int pooler_counter = 0;
     pooler_counter++;
     TotalProcessedLength+= (length/100);
@@ -415,6 +415,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 }
 
 //********************************************************************************************************************
+uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId)
+{
+    return  CurrentControlledSpeed[MotorId];
+}
 
 //********************************************************************************************************************
 uint32_t ThreadInitialTestStub(HardwareMotor * request)
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index cb0f2508f..9d12befe1 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -294,6 +294,9 @@ void JobAbortFunc(MessageContainer* requestContainer)
 
     AbortJobRequest* request = abort_job_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
+    JobAbortedByUser = true;
+    EndState(CurrentJob, "Job Aborted by user");
+    //AbortJob();
     AbortJobResponse response = ABORT_JOB_RESPONSE__INIT;
     responseContainer = createContainer(MESSAGE_TYPE__AbortJobResponse, requestContainer->token, false, &response, &abort_job_response__pack, &abort_job_response__get_packed_size);
     container_buffer = malloc(message_container__get_packed_size(&responseContainer));
@@ -307,9 +310,6 @@ void JobAbortFunc(MessageContainer* requestContainer)
         job_request__free_unpacked(CurrentJobRequest,NULL);
     CurrentJob = NULL;
     CurrentJobRequest = NULL;
-    JobAbortedByUser = true;
-    EndState(CurrentJob, "Job Aborted by user");
-    //AbortJob();
 
 
 }