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

4 files changed, 62 insertions, 27 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 2243007b8..ec4e336cd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -80,6 +80,8 @@ uint32_t DrawerFansStatus = 0;
 
 bool watchdogCriticalAlarm = false;
 
+uint32_t msec_millisecondCounter = 0;
+
 MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
@@ -99,6 +101,7 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 //**********************************************************************
 uint32_t MSBacklog[200]={0};
 uint8_t Motor_Id[200]={0};
+uint32_t MSTick[200]={0};
 uint16_t MsecLogindex = 0;
 
 void MillisecInit(void)
@@ -150,7 +153,6 @@ void MillisecStart(void)
     ADCAcquireStart(0,1);
 }
 
-uint32_t msec_millisecondCounter = 0;
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
@@ -191,14 +193,18 @@ int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
 }
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
 uint32_t MotorActivity = 0;
+
 int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
 {
     //==========================
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
     if (MsecLogindex>=199)
         MsecLogindex = 0;
+    LOG_ERROR(Data, "MillisecMoveScrew");
+
     //==========================
 
     ScrewMovePending.Callback = Callback;
@@ -219,10 +225,12 @@ int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
     //==========================
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
     if (MsecLogindex>=199)
         MsecLogindex = 0;
     //==========================
+    LOG_ERROR(Data, "MillisecSetScrewSpeed");
 
     ScrewSetMaxSpeedPending.Callback = Callback;
     ScrewSetMaxSpeedPending.Data = Data;
@@ -241,6 +249,7 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
     if (MotorId >= NUM_OF_MOTORS) return -1;
     //==========================
     MSBacklog[MsecLogindex]=Data;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
     if (MsecLogindex>=199)
@@ -276,6 +285,7 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng
     MillisecMotorDataStruc MotorData = {0};
     //==========================
     MSBacklog[MsecLogindex]=Data;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
     if (MsecLogindex>=199)
@@ -298,6 +308,7 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
 //==========================
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=MotorId;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
     if (MsecLogindex>=199)
         MsecLogindex = 0;
@@ -360,23 +371,28 @@ uint32_t MillisecLoop(uint32_t tick)
                 MotorActivity--;
             }
 
-            if (ScrewSetMaxSpeedPending.Active == true)
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
             {
-                MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
-                MotorActivity--;
-                ScrewSetMaxSpeedPending.Active = false;
-                if (ScrewSetMaxSpeedPending.Callback)
-                    ScrewSetMaxSpeedPending.Callback(Motor_i,0);
+                if (ScrewSetMaxSpeedPending.Active == true)
+                {
+                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
+                    MotorActivity--;
+                    ScrewSetMaxSpeedPending.Active = false;
+                    if (ScrewSetMaxSpeedPending.Callback)
+                        ScrewSetMaxSpeedPending.Callback(Motor_i,0);
 
-            }
-            else if (ScrewMovePending.Active == true)
-            {
-                MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
-                MotorActivity--;
-                ScrewMovePending.Active = false;
-                if (ScrewMovePending.Callback)
-                    ScrewMovePending.Callback(Motor_i,0);
+                }
+                else if (ScrewMovePending.Active == true)
+                {
+                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
+                    MotorActivity--;
+                    /*if (Screwindex>=50)
+                        Screwindex = 0;*/
+                    ScrewMovePending.Active = false;
+                    if (ScrewMovePending.Callback)
+                        ScrewMovePending.Callback(Motor_i,0);
 
+                }
             }
             if (SpeedSetPending[Motor_i].Active == true)
             {
@@ -441,13 +457,13 @@ uint32_t MillisecLoop(uint32_t tick)
     Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
     if (Ten_msTick)
     {
-        Speed_Data = Calculate_Speed_Sensor_Velocity();
+        //Speed_Data = Calculate_Speed_Sensor_Velocity();
         //MillisecReadFromTempSensor(Sensor_Read, NULL);
         //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
     }
     if (Hundred_msTick)
     {
-//        Speed_Data = Calculate_Speed_Sensor_Velocity();
+        Speed_Data = Calculate_Speed_Sensor_Velocity();
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
         {
             MillisecReadFromTempSensor(Sensor_i, NULL);
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 4da305c33..66c149f33 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -259,7 +259,7 @@ void DiagnosticTenMsecCollection(void)
     DiagnosticLoadDancer(WINDER_DANCER,Control_Read_Dancer_Position(WINDER_DANCER, 0,0));
     DiagnosticLoadDancer(POOLER_DANCER,Control_Read_Dancer_Position(POOLER_DANCER, 0,0));
     DiagnosticLoadDancer(FEEDER_DANCER,Control_Read_Dancer_Position(FEEDER_DANCER, 0,0));
-    DiagnosticLoadSpeedSensor(getSensorSpeedData());
+    //DiagnosticLoadSpeedSensor(getSensorSpeedData());
 
 }
 void Diagnostic100msecCollection(void)
@@ -268,6 +268,7 @@ void Diagnostic100msecCollection(void)
     if (DiagnosticsActive == false)
         return;
 
+    DiagnosticLoadSpeedSensor(getSensorSpeedData());
 //    dancer1angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(WINDER_DANCER, 0,0);
 //    dancer2angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(POOLER_DANCER, 0,0);
 //    dancer3angle[DiagnosticsIndex] =  Control_Read_Dancer_Position(FEEDER_DANCER, 0,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index f1b9247d3..acaca441d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -391,7 +391,7 @@ uint32_t HeaterRecalculateHeaterParams(uint32_t deviceId, uint32_t new_outputpro
    HeaterControl[deviceId].outputproportionalpowerlimit = new_outputproportionalpowerlimit;
 
    //mark the time slices  for heaters operation as empty / Heater1000 / Heater 200
-   DCTimeSliceAllocation[deviceId] = (HeaterControl[deviceId].outputproportionalpowerlimit * NumberOFSlicesInUse) / 100;
+   DCTimeSliceAllocation[deviceId] = (HeaterControl[deviceId].outputproportionalpowerlimit/100 * NumberOFSlicesInUse) / 100;
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 83cd02fa2..e2baf6d53 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -310,12 +310,13 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
     }
 
     //start thread control for all motors
-    HeaterPIDConfig[HeaterId].m_params.MAX = HeaterControl[HeaterId].outputproportionalpowerlimit;
+    HeaterPIDConfig[HeaterId].m_params.MAX = HeaterControl[HeaterId].outputproportionalpowerlimit*100;
     HeaterPIDConfig[HeaterId].m_params.MIN = 0.0;
     HeaterPIDConfig[HeaterId].m_params.Kd = HeaterControl[HeaterId].kd;
     HeaterPIDConfig[HeaterId].m_params.Kp = HeaterControl[HeaterId].kp;
     HeaterPIDConfig[HeaterId].m_params.Ki = HeaterControl[HeaterId].ki;
-    HeaterPIDConfig[HeaterId].m_params.dt = 0.1;
+    HeaterPIDConfig[HeaterId].m_params.dt = 0.01;
+    HeaterPIDConfig[HeaterId].m_params.epsilon = 10;
     HeaterPIDConfig[HeaterId].m_calculatedError = 0;
     HeaterPIDConfig[HeaterId].m_integral = 0;
     HeaterPIDConfig[HeaterId].m_isEnabled = true;
@@ -325,7 +326,7 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
     HeaterPIDConfig[HeaterId].m_SetParam = SetTemperatue*100;//need to update SetParams on presegment stage
     if (HeaterId >= MAX_AC_HEATERS) //DC Heaters
     {
-        ControlIdtoHeaterId [HeaterId] = AddControlCallback( DCHeaterControlCBFunction, eHundredMillisecond,TemperatureSensorRead,(IfTypeHeaters*0x100+HeaterId),HeaterId2PT100Id[HeaterId],0);
+        ControlIdtoHeaterId [HeaterId] = AddControlCallback( DCHeaterControlCBFunction, /*eOneSecond*/eHundredMillisecond,TemperatureSensorRead,(IfTypeHeaters*0x100+HeaterId),HeaterId2PT100Id[HeaterId],0);
         DCInitialHeating[HeaterId] = true;
         HeaterReady[HeaterId] = false;
     }
@@ -497,7 +498,7 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
                     ActivateHeater(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain);
                     return OK;
                 }
-        else
+
         {
             InitialHeating = false;
             DeActivateHeater(HARDWARE_PID_CONTROL_TYPE__DryerHeaterSecondary);
@@ -526,7 +527,7 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
                                                                            &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
         correction = HeaterPIDConfig[index].m_calculatedError;
-        HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, (int)(HeaterPIDConfig[index].m_calculatedError*100));
+        HeaterRecalculateSharedHeatersParams(HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain, (int)(HeaterPIDConfig[index].m_calculatedError));
     }
 
 
@@ -537,11 +538,17 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
  * called by: Communication from host
  * initialized all global data
  */
+//float error[HARDWARE_PID_CONTROL_TYPE__MixerHeater][100];
+//float integral[HARDWARE_PID_CONTROL_TYPE__MixerHeater][100];
+//float output[HARDWARE_PID_CONTROL_TYPE__MixerHeater][100];
+//int Counter[HARDWARE_PID_CONTROL_TYPE__MixerHeater] = {0,0,0,0,0,0,0};
+//char logmsg[HARDWARE_PID_CONTROL_TYPE__MixerHeater][254];
 
 uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 {
     int index=MAX_HEATERS_NUM;
     double temperror = 0.0;
+    int len;
     /*char str[100];
     uint8_t len = 0;*/
     if (IfIndex>>8 != IfTypeHeaters)
@@ -584,7 +591,7 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
                     //Report("DC HEATER Under temperature ", __FILE__,__LINE__,index, RpMessage, readValue, 0);
                     return OK;
                 }
-        else
+
         {
             DCInitialHeating[index] = false;
             HeatersControlStart();
@@ -609,8 +616,17 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
         //HeaterPIDConfig[index].m_mesuredParam = temperror;
         HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
                                                                               &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-//#warning PID is now only proportional (above)
-                HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError*100));
+
+//        error[index][Counter[index]]    = HeaterPIDConfig[index].m_preError;
+//        integral[index][Counter[index]] = HeaterPIDConfig[index].m_integral;
+//        output[index][Counter[index]]   = HeaterPIDConfig[index].m_calculatedError;
+//        if (Counter[index]++ >=100)
+//            Counter[index] = 0;
+//        len = usnprintf(logmsg[index], 254, "PID%d: Temp %d Integral %d Output %d ",index,(int)HeaterPIDConfig[index].m_mesuredParam ,(int)HeaterPIDConfig[index].m_integral,(int)HeaterPIDConfig[index].m_calculatedError);
+//        Report(logmsg[index],__FILE__,__LINE__,index,RpWarning,index,  Counter[index]);
+
+        //#warning PID is now only proportional (above)
+        HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError));
     }
 
 
@@ -706,10 +722,12 @@ uint32_t HeatersControlLoop(uint32_t tick)
         if (DCTimeSliceAllocation[DcHeaterId] > 0) //heater active
         {
             if (SliceCounter == 0)
+            {
                 if (HeaterMaxTempFlag[DcHeaterId] == false)
                 {
                     ActivateHeater (DcHeaterId);
                 }
+            }
             else if (SliceCounter >= DCTimeSliceAllocation[DcHeaterId]) //turn off
             {
                 DeActivateHeater (DcHeaterId);